diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index ef0cd5c77c..a6628c8b6d 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -117,7 +117,8 @@ pipeline { "spracing_h7extreme_default", "thepeach_k1_default", "thepeach_r1_default", - "uvify_core_default" + "uvify_core_default", + "siyi_n7_default" ], image: docker_images.nuttx, archive: true diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index c53467ab8c..a9ac652d18 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -29,7 +29,8 @@ "twxs.cmake", "uavcan.dsdl", "wholroyd.jinja", - "zixuanwang.linkerscript" + "zixuanwang.linkerscript", + "ms-vscode.makefile-tools" ], "containerUser": "user", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 062b6a0891..de5a0b7c72 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -73,7 +73,8 @@ jobs: raspberrypi_pico, sky-drones_smartap-airlink, spracing_h7extreme, - uvify_core + uvify_core, + siyi_n7 ] steps: - uses: actions/checkout@v1 diff --git a/.gitmodules b/.gitmodules index 78c25c2335..8b26803957 100644 --- a/.gitmodules +++ b/.gitmodules @@ -62,3 +62,13 @@ path = src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client url = https://github.com/PX4/Micro-XRCE-DDS-Client.git branch = px4 +[submodule "src/lib/cdrstream/cyclonedds"] + path = src/lib/cdrstream/cyclonedds + url = https://github.com/px4/cyclonedds +[submodule "src/lib/cdrstream/rosidl"] + path = src/lib/cdrstream/rosidl + url = https://github.com/px4/rosidl +[submodule "src/modules/zenoh/zenoh-pico"] + path = src/modules/zenoh/zenoh-pico + url = https://github.com/px4/zenoh-pico + branch = pr-zubf-werror-fix diff --git a/.vscode/extensions.json b/.vscode/extensions.json index b49f659083..ec49a0c75f 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -18,6 +18,7 @@ "twxs.cmake", "uavcan.dsdl", "wholroyd.jinja", - "zixuanwang.linkerscript" + "zixuanwang.linkerscript", + "ms-vscode.makefile-tools" ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index 35c3146dcf..67016f0983 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -127,5 +127,6 @@ "terminal.integrated.scrollback": 15000, "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" - } + }, + "ros.distro": "humble" } diff --git a/CMakeLists.txt b/CMakeLists.txt index 741e8e574a..b1587b157a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -414,6 +414,8 @@ endif() # add_library(parameters_interface INTERFACE) add_library(kernel_parameters_interface INTERFACE) +add_library(events_interface INTERFACE) +add_library(kernel_events_interface INTERFACE) include(px4_add_library) add_subdirectory(src/lib EXCLUDE_FROM_ALL) @@ -440,8 +442,11 @@ add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL) if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) target_link_libraries(parameters_interface INTERFACE usr_parameters) target_link_libraries(kernel_parameters_interface INTERFACE parameters) + target_link_libraries(events_interface INTERFACE usr_events) + target_link_libraries(kernel_events_interface INTERFACE events) else() target_link_libraries(parameters_interface INTERFACE parameters) + target_link_libraries(events_interface INTERFACE events) endif() # firmware added last to generate the builtin for included modules diff --git a/Kconfig b/Kconfig index 0bcdf14ff9..6427534f91 100644 --- a/Kconfig +++ b/Kconfig @@ -205,3 +205,5 @@ menu "platforms" depends on PLATFORM_QURT || PLATFORM_POSIX source "platforms/common/Kconfig" endmenu + +source "src/lib/*/Kconfig" diff --git a/README.md b/README.md index 0665aef3fa..c2909b1a9e 100644 --- a/README.md +++ b/README.md @@ -114,7 +114,7 @@ These boards are maintained to be compatible with PX4-Autopilot by the Manufactu ### Community supported -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members. +These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members. ### Experimental diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow index e59ce4d24f..28c3e34f71 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow @@ -31,6 +31,7 @@ param set-default PWM_MAIN_FUNC4 104 # EKF2 param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 0 param set-default EKF2_EVP_NOISE 0.05 param set-default EKF2_EVA_NOISE 0.05 param set-default EKF2_OF_CTRL 1 @@ -39,6 +40,9 @@ param set-default EKF2_OF_CTRL 1 param set-default LPE_FUSION 242 param set-default LPE_FAKE_ORIGIN 1 +# Commander +# param set-default COM_HOME_EN 0 # Disable setting of home position + param set-default MPC_ALT_MODE 2 param set-default SENS_FLOW_ROT 6 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid similarity index 100% rename from ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid rename to ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post similarity index 100% rename from ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid.post rename to ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera new file mode 100644 index 0000000000..3dc7b0fd75 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera @@ -0,0 +1,8 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor with a depth camera (forward-facing) +# +# @type Quadrotor Wide +# + +. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera b/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera new file mode 100644 index 0000000000..b7e7bf51b9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera @@ -0,0 +1,8 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor with a depth camera (downward-facing) +# +# @type Quadrotor Wide +# + +. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult index d730e4a55f..9ce6cb61dd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult @@ -7,6 +7,7 @@ param set-default FW_LAUN_DETCN_ON 1 param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming +param set-default FW_LAUN_AC_THLD 10 param set-default FW_LND_ANG 8 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane index 768a8fbf84..a998b87831 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane @@ -8,7 +8,6 @@ param set-default FW_LND_ANG 8 -param set-default FW_THR_LND_MAX 0 param set-default NPFG_PERIOD 12 @@ -36,7 +35,6 @@ param set-default NAV_DLL_ACT 2 param set-default RWTO_TKOFF 1 -#param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 @@ -61,7 +59,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - - -set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix -set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 0ba0c0be9d..ded0ace48b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -39,8 +39,10 @@ px4_add_romfs_files( 1012_gazebo-classic_iris_rplidar 1013_gazebo-classic_iris_vision 1013_gazebo-classic_iris_vision.post - 1015_gazebo-classic_iris_obs_avoid - 1015_gazebo-classic_iris_obs_avoid.post + 1014_gazebo-classic_iris_obs_avoid + 1014_gazebo-classic_iris_obs_avoid.post + 1015_gazebo-classic_iris_depth_camera + 1016_gazebo-classic_iris_downward_depth_camera 1017_gazebo-classic_iris_opt_flow_mockup 1019_gazebo-classic_iris_dual_gps 1021_gazebo-classic_uuv_hippocampus diff --git a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 index 4cc12633de..e9127020c3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 +++ b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 @@ -38,5 +38,3 @@ param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 -param set-default PWM_MAIN_TIM0 -4 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index 994ff02383..7424b9fdb7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -43,7 +43,6 @@ param set-default MC_ROLLRATE_P 0.07 param set-default MC_YAW_P 3 param set-default MPC_THR_HOVER 0.7 -param set-default MPC_THR_MAX 1 param set-default MPC_Z_P 1.5 param set-default MPC_Z_VEL_P_ACC 8 param set-default MPC_Z_VEL_I_ACC 6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index e340fb8f44..f9bf887f1e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -42,7 +42,6 @@ param set-default MC_ROLLRATE_P 0.07 param set-default MC_YAW_P 3 param set-default MPC_THR_HOVER 0.7 -param set-default MPC_THR_MAX 1 param set-default MPC_Z_P 1.5 param set-default MPC_Z_VEL_P_ACC 8 param set-default MPC_Z_VEL_I_ACC 6 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 77b10d8cfc..06c6bf717a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -34,6 +34,7 @@ param set-default EKF2_REQ_EPV 10 param set-default EKF2_REQ_HDRIFT 0.5 param set-default EKF2_REQ_SACC 1 param set-default EKF2_REQ_VDRIFT 1.0 +param set-default EKF2_RNG_QLTY_T 3.0 param set-default RTL_TYPE 1 param set-default RTL_RETURN_ALT 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index dd7a1d6bf2..490038c608 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -172,6 +172,12 @@ then ms5525dso start -X fi +# TE ASP5033 differential pressure sensor external I2C +if param compare -s SENS_EN_ASP5033 1 +then + asp5033 start -X +fi + # SHT3x temperature and hygrometer sensor, external I2C if param compare -s SENS_EN_SHT3X 1 then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4a391d39f9..1d75bf12a2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -93,6 +93,14 @@ then fi fi + # Check for an update of the ext_autostart folder, and replace the old one with it + if [ -e /fs/microsd/ext_autostart_new ] + then + echo "Updating external autostart files" + rm -r $SDCARD_EXT_PATH + mv /fs/microsd/ext_autostart_new $SDCARD_EXT_PATH + fi + set PARAM_FILE /fs/microsd/params set PARAM_BACKUP_FILE "/fs/microsd/parameters_backup.bson" fi @@ -529,6 +537,10 @@ else cyphal start fi fi + if param greater -s ZENOH_ENABLE 0 + then + zenoh start + fi # # End of autostart. diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 6704ac262f..895ffe6b33 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -26,4 +26,7 @@ exec find boards msg src platforms test \ -path src/lib/crypto/libtomcrypt -prune -o \ -path src/lib/crypto/libtommath -prune -o \ -path src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client -prune -o \ + -path src/lib/cdrstream/cyclonedds -prune -o \ + -path src/lib/cdrstream/rosidl -prune -o \ + -path src/drivers/zenoh/zenoh-pico -prune -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN diff --git a/Tools/auterion/remote_update_fmu.sh b/Tools/auterion/remote_update_fmu.sh new file mode 100755 index 0000000000..2eeb66b999 --- /dev/null +++ b/Tools/auterion/remote_update_fmu.sh @@ -0,0 +1,115 @@ +#!/bin/bash +# Flash PX4 to a device running AuterionOS in the local network +if [ "$1" == "-h" ] || [ "$1" == "--help" ] || [ $# -lt 2 ]; then + echo "Usage: $0 -f [-c ] -d [-u ] [-p ] [--revert]" + exit 1 +fi + +ssh_port=22 +ssh_user=root + +while getopts ":f:c:d:p:u:r" opt; do + case ${opt} in + f ) + if [ -n "$OPTARG" ]; then + firmware_file="$OPTARG" + else + echo "ERROR: -f requires a non-empty option argument." + exit 1 + fi + ;; + c ) + if [ -f "$OPTARG/rc.autostart" ]; then + config_dir="$OPTARG" + else + echo "ERROR: -c configuration directory is empty or does not contain a valid rc.autostart" + exit 1 + fi + ;; + d ) + if [ "$OPTARG" ]; then + device="$OPTARG" + else + echo "ERROR: -d requires a non-empty option argument." + exit 1 + fi + ;; + p ) + if [[ "$OPTARG" =~ ^[0-9]+$ ]]; then + ssh_port="$OPTARG" + else + echo "ERROR: -p ssh_port must be a number." + exit 1 + fi + ;; + u ) + if [ "$OPTARG" ]; then + ssh_user="$OPTARG" + else + echo "ERROR: -u requires a non-empty option argument." + exit 1 + fi + ;; + r ) + revert=true + ;; + esac +done + +if [ -z "$device" ]; then + echo "Error: missing device" + exit 1 +fi + +target_dir=/shared_container_dir/fmu +target_file_name="update-dev.tar" + +if [ "$revert" == true ]; then + # revert to the release version which was originally deployed + cmd="cp $target_dir/update.tar $target_dir/$target_file_name" + ssh -t -p $ssh_port $ssh_user@$device "$cmd" +else + # create custom update-dev.tar + tmp_dir="$(mktemp -d)" + config_path="" + firmware_path="" + + if [ -d "$config_dir" ]; then + cp -r "$config_dir" "$tmp_dir/config" + config_path=config + fi + + if [ -f "$firmware_file" ]; then + extension="${firmware_file##*.}" + cp "$firmware_file" "$tmp_dir/firmware.$extension" + if [ "$extension" == "elf" ]; then + # ensure the file is stripped to reduce file size + arm-none-eabi-strip "$tmp_dir/firmware.$extension" + fi + firmware_path="firmware.$extension" + fi + + pushd "$tmp_dir" &>/dev/null + + if [ -z $firmware_path ] && [ -z $config_path ]; then + exit 1 + fi + + tar_name="tar" + + if [ -x "$(command -v gtar)" ]; then + # check if gnu-tar is installed on macOS and use that instead + tar_name="gtar" + fi + + $tar_name -C "$tmp_dir" --sort=name --owner=root:0 --group=root:0 --mtime='2019-01-01 00:00:00' -cvf $target_file_name $firmware_path $config_path + + # send it to the target to start flashing + scp -P $ssh_port "$target_file_name" $ssh_user@"$device":$target_dir + popd &>/dev/null + rm -rf "$tmp_dir" +fi + +# grab status output for flashing progress +cmd="tail --follow=name $target_dir/update_status 2>/dev/null || true" +ssh -t -p $ssh_port $ssh_user@$device "$cmd" diff --git a/Tools/auterion/upload_skynode.sh b/Tools/auterion/upload_skynode.sh new file mode 100755 index 0000000000..10d8740b42 --- /dev/null +++ b/Tools/auterion/upload_skynode.sh @@ -0,0 +1,51 @@ +#!/usr/bin/env bash + +DIR="$(dirname $(readlink -f $0))" +DEFAULT_AUTOPILOT_HOST=10.41.1.1 +DEFAULT_AUTOPILOT_PORT=33333 +DEFAULT_AUTOPILOT_USER=auterion + +for i in "$@" +do + case $i in + --file=*) + PX4_BINARY_FILE="${i#*=}" + ;; + --default-ip=*) + DEFAULT_AUTOPILOT_HOST="${i#*=}" + ;; + --default-port=*) + DEFAULT_AUTOPILOT_PORT="${i#*=}" + ;; + --default-user=*) + DEFAULT_AUTOPILOT_USER="${i#*=}" + ;; + --revert) + REVERT_AUTOPILOT_ARGUMENT=-r + ;; + --wifi) + DEFAULT_AUTOPILOT_HOST=10.41.0.1 + ;; + *) + # unknown option + ;; + esac +done + +# allow these to be overridden +[ -z "$AUTOPILOT_HOST" ] && AUTOPILOT_HOST=$DEFAULT_AUTOPILOT_HOST +[ -z "$AUTOPILOT_PORT" ] && AUTOPILOT_PORT=$DEFAULT_AUTOPILOT_PORT +[ -z "$AUTOPILOT_USER" ] && AUTOPILOT_USER=$DEFAULT_AUTOPILOT_USER + +ARGUMENTS=() +ARGUMENTS+=(-d "$AUTOPILOT_HOST") +ARGUMENTS+=(-p "$AUTOPILOT_PORT") +ARGUMENTS+=(-u "$AUTOPILOT_USER") +ARGUMENTS+=(${PX4_BINARY_FILE:+-f "$PX4_BINARY_FILE"}) +ARGUMENTS+=($REVERT_AUTOPILOT_ARGUMENT) + +echo "Flashing $AUTOPILOT_HOST ..." + +"$DIR"/remote_update_fmu.sh "${ARGUMENTS[@]}" + +exit 0 diff --git a/Tools/msg/px_generate_uorb_topic_files.py b/Tools/msg/px_generate_uorb_topic_files.py index 4a26cbf10a..fe2a0e0d33 100755 --- a/Tools/msg/px_generate_uorb_topic_files.py +++ b/Tools/msg/px_generate_uorb_topic_files.py @@ -70,9 +70,9 @@ __license__ = "BSD" __email__ = "thomasgubler@gmail.com" -TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] +TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em', 'uorb_idl_header.h.em'] TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] -OUTPUT_FILE_EXT = ['.h', '.cpp'] +OUTPUT_FILE_EXT = ['.h', '.cpp', '.h'] INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] PACKAGE = 'px4' TOPICS_TOKEN = '# TOPICS ' @@ -150,6 +150,7 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template em_globals = { "name_snake_case": full_type_name_snake, "file_name_in": filename, + "file_base_name": file_base_name, "search_path": search_path, "msg_context": msg_context, "spec": spec, @@ -161,7 +162,10 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template os.makedirs(outputdir) template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) - output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx]) + if format_idx == 2: + output_file = os.path.join(outputdir, file_base_name + OUTPUT_FILE_EXT[format_idx]) + else: + output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx]) return generate_by_template(output_file, template_file, em_globals) @@ -217,6 +221,7 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='Convert msg files to uorb headers/sources') parser.add_argument('--headers', help='Generate header files', action='store_true') parser.add_argument('--sources', help='Generate source files', action='store_true') + parser.add_argument('--uorb-idl-header', help='Generate uORB compatible idl header', action='store_true') parser.add_argument('-f', dest='file', help="files to convert (use only without -d)", nargs="+") @@ -241,6 +246,11 @@ if __name__ == "__main__": generate_idx = 0 elif args.sources: generate_idx = 1 + elif args.uorb_idl_header: + for f in args.file: + print(f) + generate_output_from_file(2, f, args.outputdir, args.package, args.templatedir, INCL_DEFAULT) + exit(0) else: print('Error: either --headers or --sources must be specified') exit(-1) diff --git a/Tools/msg/templates/cdrstream/uorb_idl_header.h.em b/Tools/msg/templates/cdrstream/uorb_idl_header.h.em new file mode 100644 index 0000000000..4ad097a54c --- /dev/null +++ b/Tools/msg/templates/cdrstream/uorb_idl_header.h.em @@ -0,0 +1,65 @@ +@{ +import genmsg.msgs +import re + +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%name_snake_case +uorb_struct_upper = name_snake_case.upper() +}@ + +/**************************************************************** + + PX4 Cyclone DDS IDL to C Translator compatible idl struct + Source: @file_name_in + Compatible with Cyclone DDS: V0.11.0 + +*****************************************************************/ +#ifndef DDSC_IDL_UORB_@(uorb_struct_upper)_H +#define DDSC_IDL_UORB_@(uorb_struct_upper)_H + +#include "dds/ddsc/dds_public_impl.h" +#include "dds/cdr/dds_cdrstream.h" +#include + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + + print('#include "%s.h"'%(name)) + name = re.sub(r'(?'%(name)) +}@ + + +#ifdef __cplusplus +extern "C" { +#endif + +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + + print('typedef px4_msg_%s px4_msg_px4__msg__%s;' % (name,name)) +}@ + + + +typedef struct @uorb_struct px4_msg_@(file_base_name); + +extern const struct dds_cdrstream_desc px4_msg_@(file_base_name)_cdrstream_desc; + +#ifdef __cplusplus +} +#endif + +#endif /* DDSC_IDL_UORB_@(uorb_struct_upper)_H */ diff --git a/Tools/msg/templates/ucdr/msg.h.em b/Tools/msg/templates/ucdr/msg.h.em index 5f68b9256e..d9cbc839f4 100644 --- a/Tools/msg/templates/ucdr/msg.h.em +++ b/Tools/msg/templates/ucdr/msg.h.em @@ -124,8 +124,9 @@ static inline constexpr int ucdr_topic_size_@(topic)() return @(struct_size); } -bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf, int64_t time_offset = 0) +bool ucdr_serialize_@(topic)(const void* data, ucdrBuffer& buf, int64_t time_offset = 0) { + const @(uorb_struct)& topic = *static_cast(data); @{ for field_type, field_name, field_size, padding in fields: if padding > 0: diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index af7ec2fbff..a6bdf9b780 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -165,7 +165,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then source $HOME/.profile # load changed path for the case the script is reran before relogin if [ $(which arm-none-eabi-gcc) ]; then GCC_VER_STR=$(arm-none-eabi-gcc --version) - GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}") + GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}" || true) fi if [[ "$GCC_FOUND_VER" == "1" ]]; then diff --git a/Tools/simulation/gz/tools/avl_automation/README.md b/Tools/simulation/gz/tools/avl_automation/README.md new file mode 100755 index 0000000000..472cee03b6 --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/README.md @@ -0,0 +1,149 @@ +## Purpose + +The idea of this tool is to automate the writing of the Advanced Lift Drag plugin by automatizing the coefficient generation and requiring minimal user calculations. + +## Setup + +In order to run this tool, it is necessary to follow these steps: + +1. Download AVL 3.36 from . The file for AVL version 3.36 can be found about halfway down the page. +2. After downloading, extract AVL and move it to the home directory using: + +```shell +sudo tar -xf avl3.36.tgz +mv ./Avl /home/ +``` + +Follow the README.md found in Avl to finish the setup process for AVL (requires to set up plotlib and eispack libraries). I recommend using the gfortran compile option. This might require you to install gfortran. This can be done by running: + +```shell +sudo apt update +sudo apt install gfortran +``` + +When running the Makefile for AVL, you might encounter an Error 1 message stating that there is a directory missing. This does not prevent AVL from working for our purposes. Once the process described in the AVL README is completed, AVL is ready to be used. No further set up is required on the side of the AVL or the tool. +If you want to move the location of the AVL directory, this can simply be done by passing the `--avl_path` flag to the `input_avl.py` file, using the desired directory location for the flag (don't forget to place a "/" behind the last part of the path). Running this will automatically also adjust the paths where necessary. + +## Run + +To run the tool all that is needed is to modify the `input.yml` to the plane that you desire and then run `python input_avl.py .yml` Note that you require to have the yaml and argparse packages in your python environment to run this. An example template has been provided in the form of the `input.yml` that implements a standard plane with two ailerons, an elevator and a rudder. This example template can be run using: `python input_avl.py --yaml_file input.yml`. +Once the script has been executed, the generated .avl, .sdf and a plot of the proposed control surfaces can be found in directory. The sdf file is the generated Advanced Lift Drag Plugin that can be copied and pasted straight into a model.sdf file, which can then be run in Gazebo. + +## Functionality + +The tool first asks the user for a range of vehicle specific parameters that are needed in order to specify the geometry and physical properties of the plane. The user has the choice to define a completely custom model, or alternatively select a predefined model template (such as a Cessna or a VTOL), which has a known number of control surfaces, and then provide only some physical properties, without having to define the entire model themselves. The input_avl.py file takes the provided parameter and creates an .avl file from this that can be read by AVL (the program). This happens in the process.sh file. The necessary output generated by AVL will be saved in two files: custom_vehicle_body_axis_derivatives.txt and custom_vehicle_stability_derivatives.txt. These two files contain the parameters that are required in order to populate the Advanced Lift Drag Plugin. Finally, avl_out_parse.py reads the generated .txt files and accordingly assigns parameters to the correct element in sdf. Once this is done, it is only a question of copy and pasting the generated Advanced Lift Drag plugin (found as .sdf into the desired model.sdf file. ) + + +## Usability + +The current implementation provides a minimal working example. More accurate measurements can be made by adjusting the chosen number of vortices along span and chord according to desired preferences. A good starting point for this can be found here: . Furthermore, one can also more accurately model a vehicle by using a larger number of sections. In the current .yml file, only a left and right edge are defined for each surface yielding exactly one section, but the code supports expanding this to any number of desired sections. + +## IMPORTANT POINTS TO NOTE + +- A control surface in AVL is always defined from left to right. This means you need to first provide the left edge of a surface and then the right edge. If you do this the opposite way around, a surface will essentially be defined upside down. +- The tool is designed to only support at most two control surfaces of any type on any one vehicle. Having more surfaces than that can lead to faulty behavior. +- Another important point is that these scripts make use of the match, case syntax, which was only introduced in Python in version 3.10. +- The primary reference resource for AVL can be found at . This document was written by the creators of AVL and contains all the variables that could be required in defining the control surfaces. +- AVL cannot predict stall values. As such these need to be calculated/estimated another way. In the current implementation, default stall values have been taken from PX4's Advanced Plane. These should naturally be changed for new/different models. + +## Parameter Assignment + +Below is a comprehensive list on how the parameters are assigned at output and what files in AVL they are taken from. I am by no means an AVL expert, so please verify that these are actually the correct parameters required by the Advanced Lift Drag Plugin. For an explanation of what the parameters do, please see take a look at the Advanced Lift Drag Plugin. + +(name-in-AVL) -> (name-in-plugin) + +From the stability derivatives log file, the following advanced lift drag plugin parameters are taken: + +Alpha -> alpha The angle of attack + +Cmtot -> Cem0 Pitching moment coefficient at zero angle of attack + +CLtot -> CL0 Lift Coefficient at zero angle of attack + +CDtot -> CD0 Drag coefficient at zero angle of attack + +CLa -> CLa dCL/da (slope of CL-alpha curve) + +CYa -> CYa dCy/da (sideforce slope wrt alpha) + +Cla -> Cella dCl/da (roll moment slope wrt alpha) + +Cma -> Cema dCm/da (pitching moment slope wrt alpha - before stall) + +Cna -> Cena dCn/da (yaw moment slope wrt alpha) + +CLb -> CLb dCL/dbeta (lift coefficient slope wrt beta) + +CYb -> CYb dCY/dbeta (side force slope wrt beta) + +Clb -> Cellb dCl/dbeta (roll moment slope wrt beta) + +Cmb -> Cemb dCm/dbeta (pitching moment slope wrt beta) + +Cnb -> Cenb dCn/dbeta (yaw moment slope wrt beta) + + +From the body axis derivatives log file, the following advanced lift drag plugin parameters are taken: + +e -> eff Wing efficiency (Oswald efficiency factor for a 3D wing) + +CXp -> CDp dCD/dp (drag coefficient slope wrt roll rate) + +CYp -> CYp dCY/dp (sideforce slope wrt roll rate) + +CZp -> CLp dCL/dp (lift coefficient slope wrt roll rate) + +Clp -> Cellp dCl/dp (roll moment slope wrt roll rate) + +Cmp -> Cemp dCm/dp (pitching moment slope wrt roll rate) + +Cmp -> Cenp dCn/dp (yaw moment slope wrt roll rate) + +CXq -> CDq dCD/dq (drag coefficient slope wrt pitching rate) + +CYq -> CYq dCY/dq (side force slope wrt pitching rate) + +CZq -> CLq dCL/dq (lift coefficient slope wrt pitching rate) + +Clq -> Cellq dCl/dq (roll moment slope wrt pitching rate) + +Cmq -> Cemq dCm/dq (pitching moment slope wrt pitching rate) + +Cnq -> Cenq dCn/dq (yaw moment slope wrt pitching rate) + +CXr -> CDr dCD/dr (drag coefficient slope wrt yaw rate) + +CYr -> CYr dCY/dr (side force slope wrt yaw rate) + +CZr -> CLr dCL/dr (lift coefficient slope wrt yaw rate) + +Clr -> Cellr dCl/dr (roll moment slope wrt yaw rate) + +Cmr -> Cemr dCm/dr (pitching moment slope wrt yaw rate) + +Cnr -> Cenr dCn/dr (yaw moment slope wrt yaw rate) + + +Furthermore, every control surface also has six own parameters, which are also derived from this log file. {i} below ranges from 1 to the number of unique control surface types in the model. + +CXd{i} -> CD_ctrl Effect of the control surface's deflection on drag + +CYd{i} -> CY_ctrl Effect of the control surface's deflection on side force + +CZd{i} -> CL_ctrl Effect of the control surface's deflection on lift + +Cld{i} -> Cell_ctrl Effect of the control surface's deflection on roll moment + +Cmd{i} -> Cem_ctrl Effect of the control surface's deflection on pitching moment + +Cnd{i} -> Cen_ctrl Effect of the control surface's deflection on yaw moment + + +## Future Work + +The tool, while self-contained, could be expanded into multiple directions. + +1. Currently hinge positions and gains are set at default levels, and these could, if desired be further customized for more control. +2. More vehicles could be added to provide default templates that require less input. At the moment, only "custom" works completely. +3. Fuselage modelling could be included to further improve the accuracy of calculated coefficients. +4. At the moment only NACA airfoils are provided as a way to generate cambered surfaces. An alternative to this would be to use custom airfoil files. diff --git a/Tools/simulation/gz/tools/avl_automation/avl_out_parse.py b/Tools/simulation/gz/tools/avl_automation/avl_out_parse.py new file mode 100755 index 0000000000..ecc38e0bd0 --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/avl_out_parse.py @@ -0,0 +1,342 @@ +#!/usr/bin/env + +import argparse +import shutil +import fileinput +import subprocess +import os +from typing import TextIO + + +""" +Get the desired coefficient from the AVL output files by looking through the file line by line and picking it out when encountered. + +Args: + file (TextIO): The file from which the desired coefficient should be read. + token (str): The coefficient which to look for. + +Return: + value (str): The value associated with the desired coefficient. + +""" +def get_coef(file: TextIO,token: str) -> str: + + linesplit = [] + for line in file: + if f' {token} ' in line: + linesplit = line.split() + break + + index = 0 + for i,v in enumerate(linesplit): + if v == token: + index = i + value = linesplit[index+2] + return value + + + +""" +Write all gathered, model-wide coefficients to the sdf file. + +Args: + file (TextIO): The file to which the desired coefficient should be written. + token_str (str): The coefficients for which the associated value should be written. + token (str): The value which should be placed in the avl. + +Return: + None. + +""" +def write_coef(file: TextIO, token_str: str, token: str): + old_line = f'<{token_str}>' + new_line = f'<{token_str}>{token}' + with fileinput.FileInput(file, inplace=True) as output_file: + for line in output_file: + print(line.replace(old_line, new_line), end='') + + + +""" +Write all gathered, control surface specific parameters to the sdf file. + +Args: + file (TextIO): The file to which the desired coefficients should be written. + ctrl_surface_vec (list): A vector that contains all 6 necessary coefficient values for the control surface in question. + index (str): The model-wide index number of the control surface in question. + direction (str): The direction in which the control surface can be actuated. + +Return: + None. +""" +def ctrl_surface_coef(file: TextIO,ctrl_surface_vec: list,index: str, direction: str): + + extracted_text = '' + with open("./templates/control_surface.sdf",'r') as open_file: + for line in open_file: + extracted_text += line + open_file.close() + + # Insert necessary coefficient values, index and direction in correct sdf location. + extracted_text = extracted_text.replace("",f'servo_{index}') + extracted_text = extracted_text.replace("",f'{index}') + extracted_text = extracted_text.replace("",f'{direction}') + extracted_text = extracted_text.replace("",f'{ctrl_surface_vec[0]}') + extracted_text = extracted_text.replace("",f'{ctrl_surface_vec[1]}') + extracted_text = extracted_text.replace("",f'{ctrl_surface_vec[2]}') + extracted_text = extracted_text.replace("",f'{ctrl_surface_vec[3]}') + extracted_text = extracted_text.replace("",f'{ctrl_surface_vec[4]}') + extracted_text = extracted_text.replace("",f'{ctrl_surface_vec[5]}') + + + # Create model specific template + with open(file,'a') as plugin_file: + plugin_file.write(extracted_text + "\n") + plugin_file.close() + +""" +Read out the necessary log files to gather the desired parameters and write them to the sdf plugin file. +Arguments provided here are passed in the input_avl.py file. + +Args: + file_name (TextIO): The file to which the desired coefficients should be written. + vehicle_type (str): The type of vehicle in use. + AR (str): The calculated aspect ratio. + mac (str): The calculated mean aerodynamic chord. + ref_pt_x (str): The x coordinate of the reference point, at which forces and moments are applied. + ref_pt_y (str): The y coordinate of the reference point, at which forces and moments are applied. + ref_pt_z (str): The z coordinate of the reference point, at which forces and moments are applied. + num_ctrl_surfaces (str): The number of control surfaces that the model uses. + area (str): The wing surface area. + ctrl_surface_order (list): A list containing the types of control surfaces, in theorder in which + they have been defined in the .avl file. + avl_path (str): A string containing the directory where the AVL directory should be moved to. + +Return: + None. +""" + +def main(file_name: TextIO, vehicle_type: str, AR: str, mac: str, ref_pt_x: str, ref_pt_y: str, ref_pt_z: str, num_ctrl_surfaces: str, area: str, ctrl_surface_order: list, avl_path:str): + + # Set current path for user + curr_path = subprocess.run(['pwd'], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + if curr_path.returncode == 0: + # Save the output in a variable + savedir = curr_path.stdout.strip() + else: + raise LookupError("Invalid path to directory. Check both the avl_automation directory and the Avl directory are positioned correctly.") + + # Set the file directory path from where the AVL output logs can be read. + filedir = f'{avl_path}Avl/runs/' + + # Read out all necessary parameters from the stability and body axis derivatives files. + with open(f'{filedir}custom_vehicle_stability_derivatives.txt','r+') as stability_file: + original_position = stability_file.tell() + + # As plane is modelled at 0 degree AoA, the total coefficients should(?) correspond to the + # 0 degree coefficients required by the plugin. + alpha = get_coef(stability_file,"Alpha") + Cem0 = get_coef(stability_file,"Cmtot") + CL0 = get_coef(stability_file,"CLtot") + CD0 = get_coef(stability_file,"CDtot") + + CLa = get_coef(stability_file,"CLa") + CYa = get_coef(stability_file,"CYa") + Cella = get_coef(stability_file,"Cla") + Cema = get_coef(stability_file,"Cma") + Cena = get_coef(stability_file,"Cna") + + stability_file.seek(original_position) + + CLb = get_coef(stability_file,"CLb") + CYb = get_coef(stability_file,"CYb") + Cellb = get_coef(stability_file,"Clb") + Cemb = get_coef(stability_file,"Cmb") + Cenb = get_coef(stability_file,"Cnb") + stability_file.close() + + with open(f'{filedir}custom_vehicle_body_axis_derivatives.txt') as bodyax_file: + original_position = bodyax_file.tell() + + eff = get_coef(bodyax_file,"e") + + bodyax_file.seek(original_position) + + CDp = get_coef(bodyax_file,"CXp") + CYp = get_coef(bodyax_file,"CYp") + CLp = get_coef(bodyax_file,"CZp") + Cellp = get_coef(bodyax_file,"Clp") + Cemp = get_coef(bodyax_file,"Cmp") + Cenp = get_coef(bodyax_file,"Cnp") + + bodyax_file.seek(original_position) + + CDq = get_coef(bodyax_file,"CXq") + CYq = get_coef(bodyax_file,"CYq") + CLq = get_coef(bodyax_file,"CZq") + Cellq = get_coef(bodyax_file,"Clq") + Cemq = get_coef(bodyax_file,"Cmq") + Cenq = get_coef(bodyax_file,"Cnq") + + bodyax_file.seek(original_position) + + CDr = get_coef(bodyax_file,"CXr") + CYr = get_coef(bodyax_file,"CYr") + CLr = get_coef(bodyax_file,"CZr") + Cellr = get_coef(bodyax_file,"Clr") + Cemr = get_coef(bodyax_file,"Cmr") + Cenr = get_coef(bodyax_file,"Cnr") + bodyax_file.close() + + plane_type = vehicle_type + ctrl_surface_mat = [] + + # Maybe in the future you want more types of set aircraft. Thus us a case differentiator. + match plane_type: + + case "custom": + ctrl_surface_vec = [] + with open(f'{filedir}custom_vehicle_body_axis_derivatives.txt') as bodyax_file: + original_position = bodyax_file.tell() + for i in range(1,(len(set(ctrl_surface_order)))+1): + ctrl_surface_vec = [] + ctrl_surface_vec.append(get_coef(bodyax_file,f'CXd{i}')) + ctrl_surface_vec.append(get_coef(bodyax_file,f'CYd{i}')) + ctrl_surface_vec.append(get_coef(bodyax_file,f'CZd{i}')) + ctrl_surface_vec.append(get_coef(bodyax_file,f'Cld{i}')) + ctrl_surface_vec.append(get_coef(bodyax_file,f'Cmd{i}')) + ctrl_surface_vec.append(get_coef(bodyax_file,f'Cnd{i}')) + bodyax_file.seek(original_position) + ctrl_surface_mat.append(ctrl_surface_vec) + + + # SPECIFY STALL PARAMETERS BASED ON AIRCRAFT TYPE (IF PROVIDED) + if not os.path.exists(f'{savedir}/{file_name}'): + os.makedirs(f'{savedir}/{file_name}') + file_name = f'{savedir}/{file_name}/{file_name}.sdf' + shutil.copy(f'{savedir}/templates/advanced_lift_drag_template.sdf',file_name) + + # Get argument coefficients taken directly from the input file. + write_coef(file_name,"a0",alpha) + write_coef(file_name,"CL0",CL0) + write_coef(file_name,"CD0",CD0) + write_coef(file_name,"Cem0",Cem0) + write_coef(file_name,"AR",AR) + write_coef(file_name,"area",area) + write_coef(file_name,"mac",mac) + write_coef(file_name,"air_density",1.2041) # TODO: Provide custom air density option + write_coef(file_name,"forward","1 0 0") + write_coef(file_name,"upward","0 0 1") + write_coef(file_name,"link_name","base_link") + write_coef(file_name,"cp",f'{ref_pt_x} {ref_pt_y} {ref_pt_z}') + write_coef(file_name,"num_ctrl_surfaces",num_ctrl_surfaces) + + write_coef(file_name,"CLa",CLa) + write_coef(file_name,"CYa",CYa) + write_coef(file_name,"Cella",Cella) + write_coef(file_name,"Cema",Cema) + write_coef(file_name,"Cena",Cena) + write_coef(file_name,"CLb",CLb) + write_coef(file_name,"CYb",CYb) + write_coef(file_name,"Cellb",Cellb) + write_coef(file_name,"Cemb",Cemb) + write_coef(file_name,"Cenb",Cenb) + + write_coef(file_name,"CDp",CDp) + write_coef(file_name,"CYp",CYp) + write_coef(file_name,"CLp",CLp) + write_coef(file_name,"Cellp",Cellp) + write_coef(file_name,"Cemp",Cemp) + write_coef(file_name,"Cenp",Cenp) + write_coef(file_name,"CDq",CDq) + write_coef(file_name,"CYq",CYq) + write_coef(file_name,"CLq",CLq) + write_coef(file_name,"Cellq",Cellq) + write_coef(file_name,"Cemq",Cemq) + write_coef(file_name,"Cenq",Cenq) + write_coef(file_name,"CDr",CDr) + write_coef(file_name,"CYr",CYr) + write_coef(file_name,"CLr",CLr) + write_coef(file_name,"Cellr",Cellr) + write_coef(file_name,"Cemr",Cemr) + write_coef(file_name,"Cenr",Cenr) + + write_coef(file_name,"eff",eff) + + # TODO: Improve this for custom stall values + # Note: Currently these stall values are simply taken from advanced_plane presets. + + write_coef(file_name,"alpha_stall","0.3391428111") + write_coef(file_name,"CLa_stall","-3.85") + write_coef(file_name,"CDa_stall","-0.9233984055") + write_coef(file_name,"Cema_stall","0") + + # Check whether a particular type of control surface has been seen before. If it has, + # then the current control surface is the (right) counterpart. + + # ASSUMPTION: There is the assumption that an vehicle will only ever have two of any + # particular type of control surface. (left and right). If this is not the case, the negation + # below will likely not work correctly. + type_seen = list() + + # Dictionary containing the directions that each type of control surface can move. + ctrl_direction = {"aileron": 1,"elevator": -1,"rudder": 1} + + # More set types in the future? + match plane_type: + + case "custom": + for i, ctrl_surface in enumerate(ctrl_surface_order): + + # Check whether a particular type of control surface has been seen before. If it has, + # then the current control surface is the (right) counterpart. Depending on the exact + # nature of the encountered type you then need to negate the correct parameters. + if ctrl_surface in type_seen: + # Work out what the corresponding index for the first encounter of the ctrl surface is. + seen_index = type_seen.index(ctrl_surface) + + if ctrl_surface == 'aileron': + #Change for right wing aileron by flipping sign + ctrl_surface_mat[seen_index][3] = -float(ctrl_surface_mat[0][3]) + ctrl_surface_mat[seen_index][5] = -float(ctrl_surface_mat[0][5]) + + # Split Elevators are assumed to never run differentially. Feel free to add a + # condition if your plane does require differential elevator action. + + else: + # If a ctrl surface has not been encountered add it to the type_seen list and + # set the index to the length of the list - 1 as this corresponds to the newest + # unseen element in ctrl_surface_mat . + type_seen.append(ctrl_surface) + seen_index = len(type_seen) - 1 + + ctrl_surface_coef(file_name,ctrl_surface_mat[seen_index],i,ctrl_direction[ctrl_surface]) + + + # close the sdf file with plugin + with open(file_name,'a') as plugin_file: + plugin_file.write("") + plugin_file.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + + parser.add_argument("file_name", help="The file to which the desired coefficients should be written.") + parser.add_argument("vehicle_type", help="The type of vehicle in use.") + parser.add_argument("AR", help="The calculated aspect ratio.") + parser.add_argument("mac", help="The calculated mean aerodynamic chord.") + parser.add_argument("ref_pt_x", help="The x coordinate of the reference point, at which forces and moments are applied.") + parser.add_argument("ref_pt_y", help="The y coordinate of the reference point, at which forces and moments are applied.") + parser.add_argument("ref_pt_z", help="The z coordinate of the reference point, at which forces and moments are applied.") + parser.add_argument("num_ctrl_surfaces", help="The number of control surfaces that the model uses.") + parser.add_argument("area", help= "The wing surface area.") + parser.add_argument("ctrl_surface_order", help=" A list containing the types of control surfaces, in theorder in which \ + they have been defined in the .avl file.") + parser.add_argument("avl_path",help="A string containing the directory where the AVL directory should be moved to.") + + args = parser.parse_args() + + main(args.file_name,args.vehicle_type,args.AR,args.mac,args.ref_pt_x,args.ref_pt_y, + args.ref_pt_z,args.num_ctrl_surfaces,args.area,args.ctrl_surface_order,args.avl_path) diff --git a/Tools/simulation/gz/tools/avl_automation/avl_steps.txt b/Tools/simulation/gz/tools/avl_automation/avl_steps.txt new file mode 100755 index 0000000000..48c20a52c1 --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/avl_steps.txt @@ -0,0 +1,10 @@ +oper +x +n custom_plane +st custom_vehicle_stability_derivatives.txt +sb custom_vehicle_body_axis_derivatives.txt +g +h + + +quit diff --git a/Tools/simulation/gz/tools/avl_automation/input.yml b/Tools/simulation/gz/tools/avl_automation/input.yml new file mode 100755 index 0000000000..3231757b09 --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/input.yml @@ -0,0 +1,142 @@ +# Enter a name for your vehicle +vehicle_name: plane_example_2 + +# Enter the type of airframe you would like to use: +frame_type: custom + +# First define some model-wide parameters for custom models: +reference_area: 12 +wing_span: 15 +# Provide a reference point at which the forces and moments generated will act. +reference_point: + X: 0 + Y: 0 + Z: 0 + +#Provide information on each of the Control Surfaces +num_ctrl_surfaces: 4 +control_surfaces: + - name: right_wing + type: aileron + nchord: 1 + cspace: 1 + nspan: 16 + sspace: -2 + angle: 4 + translation: + X: 0 + Y: 0 + Z: 0 + naca: 2412 + sections: + - name: section_1 + position: + X: -0.25 + Y: 0 + Z: 0 + chord: 1 + ainc: 0 + nspan: 8 + sspace: 1 + - name: section_2 + position: + X: -0.175 + Y: 5 + Z: 0.5 + chord: 0.7 + ainc: 0 + nspan: 0 + sspace: 0 + + + - name: left_wing + type: aileron + nchord: 1 + cspace: 1 + nspan: 16 + sspace: -2 + angle: 4 + translation: + X: 0 + Y: 0 + Z: 0 + naca: 2412 + sections: + - name: section_1 + position: + X: -0.175 + Y: -5 + Z: 0.5 + chord: 0.7 + ainc: 0 + nspan: 0 + sspace: 0 + - name: section_2 + position: + X: -0.25 + Y: 0 + Z: 0 + chord: 1 + ainc: 0 + nspan: 8 + sspace: 1 + + - name: elevator + type: elevator + nchord: 1 + cspace: 1 + nspan: 7 + sspace: -2 + translation: + X: 6 + Y: 0 + Z: 0.5 + sections: + - name: section_1 + position: + X: -0.1 + Y: 0 + Z: 0 + chord: 0.4 + ainc: 0 + nspan: 7 + sspace: -1.25 + - name: section_2 + position: + X: -0.075 + Y: 2 + Z: 0 + chord: 0.3 + ainc: 0 + nspan: 0 + sspace: 0 + + - name: fin + type: rudder + nchord: 1 + cspace: 1 + nspan: 10 + sspace: 1 + translation: + X: 6 + Y: 0 + Z: 0.5 + sections: + - name: section_1 + position: + X: -0.1 + Y: 0 + Z: 0 + chord: 0.4 + ainc: 0 + nspan: 7 + sspace: -1.25 + - name: section_2 + position: + X: -0.075 + Y: 0 + Z: 1 + chord: 0.3 + ainc: 0 + nspan: 0 + sspace: 0 diff --git a/Tools/simulation/gz/tools/avl_automation/input_avl.py b/Tools/simulation/gz/tools/avl_automation/input_avl.py new file mode 100755 index 0000000000..1a260efe88 --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/input_avl.py @@ -0,0 +1,314 @@ +#!/usr/bin/env + +import argparse +import avl_out_parse +import os +import yaml +import subprocess +import shutil + +""" +Write individual airfoil section definitions to the .avl file. +Sections are defined through a 3D point in space and assigned properties such as chord, angle of incidence etc. +AVL then links them up to the other sections of a particular surface. You can define any number of sections for +a particular surface, but there always have to be at least two (a left and right edge). + +Args: + plane_name (str): The name of the vehicle. + x (str): The x coordinate of the section. + y (str): The y coordinate of the section. + z (str): The z coordinate of the section. + chord (str): Chord in this section of the surface. Trailing edge is at x + chord, y, z. + ainc (str): Angle of incidence for this section. Taken as a rotation (RH rule) about the surface's + spanwise axis projected onto the Y-Z plane. + nspan (str): Number of spanwise vortices in until the next section. + sspan (str): Controls the spanwise spacing of the vortices. + naca_number (str): The chosen NACA number that will define the cambered properties of this section + of the surface. For help picking an airfoil go to: http://airfoiltools.com/airfoil/naca4digit. + ctrl_surface_type: The selected type of control surface. This should be consistent along the entirety of + the surface. (Question: Flap and Aileron along the same airfoil?) + +Return: + None. + +""" +def write_section(plane_name: str,x: str,y: str,z: str,chord: str,ainc: str,nspan: str,sspace: str,naca_number: str,ctrl_surf_type: str): + + with open(f'{plane_name}.avl','a') as avl_file: + avl_file.write("SECTION \n") + avl_file.write("!Xle Yle Zle Chord Ainc Nspanwise Sspace \n") + avl_file.write(f'{x} {y} {z} {chord} {ainc} {nspan} {sspace} \n') + if naca_number != "0000": + avl_file.write("NACA \n") + avl_file.write(f'{naca_number} \n') + avl_file.close() + + match ctrl_surf_type: + case 'aileron': + #TODO provide custom options for gain and hinge positions + with open(f'{plane_name}.avl','a') as avl_file: + avl_file.write("CONTROL \n") + avl_file.write("aileron 1.0 0.0 0.0 0.0 0.0 -1 \n") + avl_file.close() + + case 'elevator': + with open(f'{plane_name}.avl','a') as avl_file: + avl_file.write("CONTROL \n") + avl_file.write("elevator 1.0 0.0 0.0 0.0 0.0 1 \n") + avl_file.close() + + case 'rudder': + with open(f'{plane_name}.avl','a') as avl_file: + avl_file.write("CONTROL \n") + avl_file.write("rudder 1.0 0.0 0.0 0.0 0.0 1 \n") + avl_file.close() + + + +""" +Read the provided yaml file and generate the corresponding .avl file that can be read into AVL. +Also calls AVL and the avl_out_parse.py file that generates the sdf plugin. + +Args: + yaml_file: Path to the input yaml file + avl_path: Set the avl_path to provide a desired directory for where Avl should be located. + +Return: + None + +""" +def main(): + user = os.environ.get('USER') + # This will find Avl on a users machine. + for root, dirs, _ in os.walk(f'/home/{user}/'): + if "Avl" in dirs: + target_directory_path = os.path.join(root, "Avl") + break + parent_directory_path = os.path.dirname(target_directory_path) + filedir = f'{parent_directory_path}/' + print(filedir) + + parser = argparse.ArgumentParser() + parser.add_argument("--yaml_file", help="Path to input yaml file.") + parser.add_argument("--avl_path", default=filedir, help="Provide an absolute AVL path. If this argument is passed, AVL will be moved there and the files will adjust their paths accordingly.") + inputs = parser.parse_args() + + + # If the user passes the avl_path argument then move Avl to that location: + if inputs.avl_path != filedir: + + #Check if the directory is already there + if os.path.exists(f'{inputs.avl_path}/Avl') and os.path.isdir(f'{inputs.avl_path}/Avl'): + print("Avl is already at desired location") + else: + shutil.move(f'{filedir}Avl',inputs.avl_path) + + # Adjust paths to AVL in process.sh + print("Adjusting paths") + with open("./process.sh", "r") as file: + all_lines = file.readlines() + file.close() + + it = 0 + for line in all_lines: + if "cp $DIR_PATH/$CUSTOM_MODEL.avl" in line: + new_line = f'cp $DIR_PATH/$CUSTOM_MODEL.avl {inputs.avl_path}Avl/runs\n' + all_lines[it] = new_line + + if "/Avl/runs/plot.ps $DIR_PATH/" in line: + new_line =f'mv {inputs.avl_path}Avl/runs/plot.ps $DIR_PATH/\n' + all_lines[it] = new_line + + if "cd" in line and "/Avl/runs" in line: + new_line = f'cd {inputs.avl_path}Avl/runs\n' + all_lines[it] = new_line + it += 1 + + with open("./process.sh", "w") as file: + file.writelines(all_lines) + file.close() + + + with open(inputs.yaml_file,'r') as yaml_file: + yaml_data = yaml.safe_load(yaml_file) + + airframes = ['cessna','standard_vtol','custom'] + plane_name = yaml_data['vehicle_name'] + frame_type = yaml_data['frame_type'] + if not frame_type in airframes: + raise ValueError("\nThis is not a valid airframe, please choose a valid airframe. \n") + + # Parameters that need to be provided: + # General + # - Reference Area (Sref) + # - Wing span (Bref) (wing span squared / area = aspect ratio which is a required parameter for the sdf file) + # - Reference point (X,Y,Zref) point at which moments and forces are calculated + #Control Surface specific + # - type (select from options; aileron,elevator,rudder) + # - nchord + # - cspace + # - nspanwise + # - sspace + # - x,y,z 1. (section) + # - chord 1. (section) + # - ainc 1. (section) + # - Nspan 1. (optional for section) + # - sspace 1. (optional for section) + # - x,y,z 2. (section) + # - chord 2. (section) + # - ainc 2. (section) + # - Nspan 2. (optional for section) + # - sspace 2. (optional for section) + + # TODO: Find out if elevons are defined + ctrl_surface_types = ['aileron','elevator','rudder'] + # - Reference Chord (Cref) (= area/wing span) + delineation = '!***************************************' + sec_demark = '#--------------------------------------------------' + num_ctrl_surfaces = 0 + ctrl_surface_order = [] + area = 0 + span = 0 + + ref_pt_x = None + ref_pt_y = None + ref_pt_z = None + + # Future work: Provide some pre-worked frames for a Cessna and standard VTOL if there is a need for it + match frame_type: + + case "custom": + + # These parameters are consistent across all models. + # At the moment we do not use any symmetry axis for mirroring. + with open(f'{plane_name}.avl','w') as avl_file: + avl_file.write(f'{delineation} \n') + avl_file.write(f'!{plane_name} input dataset \n') + avl_file.write(f'{delineation} \n') + avl_file.write(f'{plane_name} \n') + avl_file.write('!Mach \n0.0 \n') + avl_file.write('!IYsym IZsym Zsym \n') + avl_file.write('0 0 0 \n') + avl_file.close() + + # First define some model-specific parameters for custom models + area = yaml_data["reference_area"] + span = yaml_data["wing_span"] + ref_pt_x = yaml_data["reference_point"]["X"] + ref_pt_y = yaml_data["reference_point"]["Y"] + ref_pt_z = yaml_data["reference_point"]["Z"] + + if(span != 0 and area != 0): + ref_chord = float(area)/float(span) + else: + raise ValueError("Invalid reference chord value. Check area and wing span values.") + + # Write the gathered model-wide parameters into the .avl file + with open(f'{plane_name}.avl','a') as avl_file: + avl_file.write('!Sref Cref Bref \n') + avl_file.write(f'{area} {str(ref_chord)} {span} \n') + avl_file.write('!Xref Yref Zref \n') + avl_file.write(f'{ref_pt_x} {ref_pt_y} {ref_pt_z} \n') + avl_file.close() + + num_ctrl_surfaces = yaml_data["num_ctrl_surfaces"] + for i, control_surface in enumerate(yaml_data["control_surfaces"]): + + # Wings always need to be defined from left to right + ctrl_surf_name = control_surface['name'] + ctrl_surf_type = control_surface['type'] + if ctrl_surf_type not in ctrl_surface_types: + raise ValueError(f'The selected type is invalid. Available types are: {ctrl_surface_types}') + + # The order of control surfaces becomes important in the output parsing + # to correctly assign derivatives to particular surfaces. + ctrl_surface_order.append(ctrl_surf_type) + + nchord = control_surface["nchord"] + cspace = control_surface["cspace"] + nspanwise = control_surface["nspan"] + sspace = control_surface["sspace"] + + # TODO: Add more control surface types that also require Angles. + if ctrl_surf_type.lower() == 'aileron': + angle = control_surface["angle"] + + #Translation of control surface, will move the whole surface to specified position + tx = control_surface["translation"]["X"] + ty = control_surface["translation"]["Y"] + tz = control_surface["translation"]["Z"] + + # Write common part of this surface to .avl file + with open(f'{plane_name}.avl','a') as avl_file: + avl_file.write(sec_demark) + avl_file.write("\nSURFACE \n") + avl_file.write(f'{ctrl_surf_name} \n') + avl_file.write("!Nchordwise Cspace Nspanwise Sspace \n") + avl_file.write(f'{nchord} {cspace} {nspanwise} {sspace} \n') + + # If we have a elevator, we can duplicate the defined control surface along the y-axis of the model + # as both sides are generally modelled and controlled as one in simulation. Adjust for split elevators if desired. + if ctrl_surf_type.lower() == 'elevator': + avl_file.write("\nYDUPLICATE\n") + avl_file.write("0.0\n\n") + + # Elevators and Rudders do not require an angle of incidence. + if ctrl_surf_type.lower() == 'aileron': + avl_file.write("ANGLE \n") + avl_file.write(f'{angle} \n') + + # Translate the surface to a particular position in space. + avl_file.write("TRANSLATE \n") + avl_file.write(f'{tx} {ty} {tz} \n') + avl_file.close() + + + # Define NACA airfoil shape. + # For help picking an airfoil go to: http://airfoiltools.com/airfoil/naca4digit + # NOTE: AVL can only use 4-digit NACA codes. + if ctrl_surf_type.lower() == "aileron": + naca_number = control_surface["naca"] + else: + # Provide a default NACA number for unused airfoils + naca_number = '0000' + + # Iterating over each defined section for the control surface. There need to be at least + # two in order to define a left and right edge, but there is no upper limit. + # CRITICAL: ALWAYS DEFINE YOUR SECTION FROM LEFT TO RIGHT + for j, section in enumerate(control_surface["sections"]): + + print(f'Defining {j}. section of {i+1}. control surface \n') + y = section["position"]["Y"] + z = section["position"]["Z"] + x = section["position"]["X"] + chord = section["chord"] + ainc = section["ainc"] + nspan = section["nspan"] + write_section(plane_name,x,y,z,chord,ainc,nspan,sspace,naca_number,ctrl_surf_type) + + print(f'\nPARAMETER DEFINITION FOR {i+1}. CONTROL SURFACE COMPLETED \n') + + + # Calculation of Aspect Ratio (AR) and Mean Aerodynamic Chord (mac) + AR = str((float(span)*float(span))/float(area)) + mac = str((2/3)*(float(area)/float(span))) + + # Call shell script that will pass the generated .avl file to AVL + os.system(f'./process.sh {plane_name}') + + # Call main function of avl parse script to parse the generated AVL files. + avl_out_parse.main(plane_name,frame_type,AR,mac,ref_pt_x,ref_pt_y,ref_pt_z,num_ctrl_surfaces,area,ctrl_surface_order,inputs.avl_path) + + # Finally move all generated files to a new directory and show the generated geometry image: + result = subprocess.run(['pwd'], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + if result.returncode == 0: + # Save the output in a variable + current_path = result.stdout.strip() + + # Run image plot from avl_automation directory. + os.system(f'mv ./{plane_name}.* ./{plane_name}' ) + os.system(f'evince {current_path}/{plane_name}/{plane_name}.ps') + +if __name__ == '__main__': + main() diff --git a/Tools/simulation/gz/tools/avl_automation/process.sh b/Tools/simulation/gz/tools/avl_automation/process.sh new file mode 100755 index 0000000000..fb8e8a19fb --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/process.sh @@ -0,0 +1,27 @@ +#!/bin/bash +CUSTOM_MODEL=$1 +DIR_PATH=$(pwd) + +cp $DIR_PATH/$CUSTOM_MODEL.avl /home/$USER/Avl/runs/ +cd +cd /home/$USER/Avl/runs + +old_stability_derivatives="custom_vehicle_stability_derivatives.txt" +old_body_ax_derivatives="custom_vehicle_body_axis_derivatives.txt" + +if [ -e "$old_stability_derivatives" ]; then + # Delete old stability derivative file + rm "$old_stability_derivatives" +fi +if [ -e "$old_body_ax_derivatives" ]; then + # Delete old body_axis derivative file + rm "$old_body_ax_derivatives" +fi + +#avl_steps.txt can be used to run commands on the AVL commandline. +../bin/avl $CUSTOM_MODEL.avl < $DIR_PATH/avl_steps.txt +echo "\n" + +#After completion move the plot to avl_automation directory +mv /home/$USER/Avl/runs/plot.ps $DIR_PATH/ +mv $DIR_PATH/plot.ps $DIR_PATH/$CUSTOM_MODEL.ps diff --git a/Tools/simulation/gz/tools/avl_automation/templates/advanced_lift_drag_template.sdf b/Tools/simulation/gz/tools/avl_automation/templates/advanced_lift_drag_template.sdf new file mode 100644 index 0000000000..d024658bdb --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/templates/advanced_lift_drag_template.sdf @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Tools/simulation/gz/tools/avl_automation/templates/control_surface.sdf b/Tools/simulation/gz/tools/avl_automation/templates/control_surface.sdf new file mode 100644 index 0000000000..79d5b38e97 --- /dev/null +++ b/Tools/simulation/gz/tools/avl_automation/templates/control_surface.sdf @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/Tools/zenoh/px_generate_zenoh_topic_files.py b/Tools/zenoh/px_generate_zenoh_topic_files.py new file mode 100755 index 0000000000..b9110dbea8 --- /dev/null +++ b/Tools/zenoh/px_generate_zenoh_topic_files.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +############################################################################# +# +# Copyright (C) 2013-2022 PX4 Pro 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. +# +############################################################################# + +""" +px_generate_zenoh_topic_files.py +Generates c/cpp header/source files for use with zenoh +message files +""" + +import os +import argparse +import re +import sys + +try: + import em +except ImportError as e: + print("Failed to import em: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user empy") + print("") + sys.exit(1) + +try: + import genmsg.template_tools +except ImportError as e: + print("Failed to import genmsg: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyros-genmsg") + print("") + sys.exit(1) + + +__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" +__copyright__ = "Copyright (C) 2013-2022 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + +ZENOH_TEMPLATE_FILE = ['Kconfig.topics.em', 'uorb_pubsub_factory.hpp.em'] +TOPICS_TOKEN = '# TOPICS ' + + +def get_topics(filename): + """ + Get TOPICS names from a "# TOPICS" line + """ + ofile = open(filename, 'r') + text = ofile.read() + result = [] + for each_line in text.split('\n'): + if each_line.startswith(TOPICS_TOKEN): + topic_names_str = each_line.strip() + topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") + topic_names_list = topic_names_str.split(" ") + for topic in topic_names_list: + # topic name PascalCase (file name) to snake_case (topic name) + topic_name = re.sub(r'(? +#include +@[for idx, topic_name in enumerate(full_base_names)]@ +#include +@[end for] + +@[for idx, topic_name in enumerate(datatypes)]@ +@{ +type_topic_count = len([e for e in topic_names_all if e.startswith(topic_name)]) +}@ +#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper()) +# define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT @(type_topic_count) +#else +# define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT 0 +#endif +@[end for] + +#define ZENOH_PUBSUB_COUNT \ +@[for idx, topic_name in enumerate(datatypes)]@ + CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT + \ +@[end for] 0 + +typedef struct { + const uint32_t *ops; + const orb_metadata* orb_meta; +} UorbPubSubTopicBinder; + +const UorbPubSubTopicBinder _topics[ZENOH_PUBSUB_COUNT] { +@{ +uorb_id_idx = 0 +}@ +@[for idx, topic_name in enumerate(datatypes)]@ +#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper()) +@{ +topic_names = [e for e in topic_names_all if e.startswith(topic_name)] +}@ +@[for topic_name_inst in topic_names]@ + { + px4_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops, + ORB_ID(@(topic_name_inst)) + }, +@{ +uorb_id_idx += 1 +}@ +@[end for]#endif +@[end for] +}; + +uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta) { + for (auto &pub : _topics) { + if(pub.orb_meta->o_id == meta->o_id) { + return new uORB_Zenoh_Publisher(meta, pub.ops); + } + } + return NULL; +} + + +uORB_Zenoh_Publisher* genPublisher(const char *name) { + for (auto &pub : _topics) { + if(strcmp(pub.orb_meta->o_name, name) == 0) { + return new uORB_Zenoh_Publisher(pub.orb_meta, pub.ops); + } + } + return NULL; +} + + +Zenoh_Subscriber* genSubscriber(const orb_metadata *meta) { + for (auto &sub : _topics) { + if(sub.orb_meta->o_id == meta->o_id) { + return new uORB_Zenoh_Subscriber(meta, sub.ops); + } + } + return NULL; +} + + +Zenoh_Subscriber* genSubscriber(const char *name) { + for (auto &sub : _topics) { + if(strcmp(sub.orb_meta->o_name, name) == 0) { + return new uORB_Zenoh_Subscriber(sub.orb_meta, sub.ops); + } + } + return NULL; +} diff --git a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig index 1e3a8a27d9..5b1c5bea7f 100644 --- a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig +++ b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-flow/nuttx-config/nsh/defconfig b/boards/ark/can-flow/nuttx-config/nsh/defconfig index 15d145881e..eb4cb5e929 100644 --- a/boards/ark/can-flow/nuttx-config/nsh/defconfig +++ b/boards/ark/can-flow/nuttx-config/nsh/defconfig @@ -115,6 +115,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-flow/src/boot_config.h b/boards/ark/can-flow/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-flow/src/boot_config.h +++ b/boards/ark/can-flow/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -93,7 +93,7 @@ */ #define OPT_WAIT_FOR_GETNODEINFO 0 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/can-flow/uavcan_board_identity b/boards/ark/can-flow/uavcan_board_identity index 8f389d0657..e49fbcac8a 100644 --- a/boards/ark/can-flow/uavcan_board_identity +++ b/boards/ark/can-flow/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} @@ -14,4 +14,4 @@ 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/ark/can-gps/nuttx-config/nsh/defconfig b/boards/ark/can-gps/nuttx-config/nsh/defconfig index f734bc8168..8addad7c7d 100644 --- a/boards/ark/can-gps/nuttx-config/nsh/defconfig +++ b/boards/ark/can-gps/nuttx-config/nsh/defconfig @@ -117,6 +117,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-gps/src/boot_config.h b/boards/ark/can-gps/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-gps/src/boot_config.h +++ b/boards/ark/can-gps/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -93,7 +93,7 @@ */ #define OPT_WAIT_FOR_GETNODEINFO 0 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/can-gps/uavcan_board_identity b/boards/ark/can-gps/uavcan_board_identity index ca6d098e59..00b23ea028 100644 --- a/boards/ark/can-gps/uavcan_board_identity +++ b/boards/ark/can-gps/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig index ba2d6f2143..34d42179c7 100644 --- a/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig +++ b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig @@ -117,6 +117,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-rtk-gps/src/boot_config.h b/boards/ark/can-rtk-gps/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-rtk-gps/src/boot_config.h +++ b/boards/ark/can-rtk-gps/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -93,7 +93,7 @@ */ #define OPT_WAIT_FOR_GETNODEINFO 0 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/can-rtk-gps/uavcan_board_identity b/boards/ark/can-rtk-gps/uavcan_board_identity index 5db21a4ffb..655384bc07 100644 --- a/boards/ark/can-rtk-gps/uavcan_board_identity +++ b/boards/ark/can-rtk-gps/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/cannode/nuttx-config/nsh/defconfig b/boards/ark/cannode/nuttx-config/nsh/defconfig index a0882b71be..8ec02cc3dd 100644 --- a/boards/ark/cannode/nuttx-config/nsh/defconfig +++ b/boards/ark/cannode/nuttx-config/nsh/defconfig @@ -117,6 +117,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/cannode/src/boot_config.h b/boards/ark/cannode/src/boot_config.h index eab3c76c6c..b43795ac4c 100644 --- a/boards/ark/cannode/src/boot_config.h +++ b/boards/ark/cannode/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -92,8 +92,12 @@ * */ #define OPT_WAIT_FOR_GETNODEINFO 0 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +/* The ARK CANnode uses PH1 for GPIO_BOOT_CONFIG but it is not + * compatible with px4_arch_gpioread as Port H = 7 which is greater + * than STM32_NPORTS + * #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 0 + */ +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/cannode/uavcan_board_identity b/boards/ark/cannode/uavcan_board_identity index 44ea18cc7d..490b7678bd 100644 --- a/boards/ark/cannode/uavcan_board_identity +++ b/boards/ark/cannode/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 4a6af4f6d8..4018e493a2 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -20,6 +20,7 @@ CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 2f2c8cfeae..7a26ee01c8 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -17,12 +17,21 @@ param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 param set-default SENS_EN_THERMAL 1 -param set-default SENS_TEMP_ID 2818058 param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_FF 0.0 #param set-default SENS_IMU_TEMP_I 0.025 #param set-default SENS_IMU_TEMP_P 1.0 +if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +then + param set-default SENS_TEMP_ID 2818058 +fi + +if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +then + param set-default SENS_TEMP_ID 3014666 +fi + if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007 then param set-default SYS_USE_IO 0 diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index 84f985cecb..0e04ed41cb 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -3,10 +3,12 @@ # ARK FMUARKV6X specific board sensors init #------------------------------------------------------------------------------ set HAVE_PM2 yes +set HAVE_PM3 yes if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004 then set HAVE_PM2 no + set HAVE_PM3 no fi if param compare -s ADC_ADS1115_EN 1 @@ -25,36 +27,68 @@ then then ina226 -X -b 2 -t 2 -k start fi + + if [ $HAVE_PM3 = yes ] + then + ina226 -X -b 3 -t 2 -k start + fi fi if param compare SENS_EN_INA228 1 then # Start Digital power monitors ina228 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] then ina228 -X -b 2 -t 2 -k start fi + + if [ $HAVE_PM3 = yes ] + then + ina228 -X -b 3 -t 2 -k start + fi fi if param compare SENS_EN_INA238 1 then # Start Digital power monitors ina238 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] then ina238 -X -b 2 -t 2 -k start fi + + if [ $HAVE_PM3 = yes ] + then + ina238 -X -b 3 -t 2 -k start + fi fi -# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz -iim42652 -R 3 -s -b 1 -C 32051 start +if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +then + # Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz + iim42652 -R 3 -s -b 1 -C 32051 start -# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz -icm42688p -R 9 -s -b 2 -C 32051 start + # Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz + icm42688p -R 9 -s -b 2 -C 32051 start -# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz -icm42688p -R 6 -s -b 3 -C 32051 start + # Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz + icm42688p -R 6 -s -b 3 -C 32051 start +fi + +if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +then + # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz + iim42653 -R 3 -s -b 1 -C 32051 start + + # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz + iim42653 -R 9 -s -b 2 -C 32051 start + + # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz + iim42653 -R 6 -s -b 3 -C 32051 start +fi # Internal magnetometer on I2C bmm150 -I start @@ -63,3 +97,4 @@ bmm150 -I start bmp388 -I start unset HAVE_PM2 +unset HAVE_PM3 diff --git a/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig b/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig index c414af3c28..c830932873 100644 --- a/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig @@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig index 384726875d..17da75424e 100644 --- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig @@ -118,7 +118,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -189,6 +189,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 1317d42668..001a31ae73 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -211,23 +211,18 @@ #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "ARKV6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3,4 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1 // Base/FMUM -#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Rev 0 -#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, BMI388 I2C2 Rev 1 -#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3 -#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4 -#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 -#define ARKV6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 -#define ARKV6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 -//#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, HB CM4 base Rev 0 // never shipped -//#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, BMI388 I2C2 HB CM4 base Rev 1 // never shipped -#define ARKV6X43 HW_VER_REV(0x4,0x3) // ARKV6X, Sensor Set HB CM4 base Rev 3 -#define ARKV6X44 HW_VER_REV(0x4,0x4) // ARKV6X, Sensor Set HB CM4 base Rev 4 -#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, ARKV6X Rev 0 with HB Mini Rev 5 -//#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, BMI388 I2C2 HB Mini Rev 1 // never shipped -#define ARKV6X53 HW_VER_REV(0x5,0x3) // ARKV6X, Sensor Set HB Mini Rev 3 -#define ARKV6X54 HW_VER_REV(0x5,0x4) // ARKV6X, Sensor Set HB Mini Rev 4 +#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Sensor Set Rev 0 +#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, Sensor Set Rev 1 +//#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3 +//#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4 +#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Sensor Set Rev 0 +#define ARKV6X11 HW_VER_REV(0x1,0x1) // NO PX4IO, Sensor Set Rev 1 +#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 +#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 +#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 +#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/ark/fmu-v6x/src/manifest.c b/boards/ark/fmu-v6x/src/manifest.c index 6b8405bcf5..f4e012bfb0 100644 --- a/boards/ark/fmu-v6x/src/manifest.c +++ b/boards/ark/fmu-v6x/src/manifest.c @@ -160,21 +160,14 @@ static const px4_hw_mft_item_t hw_mft_list_v0650[] = { static px4_hw_mft_list_entry_t mft_lists[] = { // ver_rev - {ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 - {ARKV6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 - //{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // HB CM4 base // never shipped - //{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2 HB CM4 base // never shipped - {ARKV6X43, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 - {ARKV6X44, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 - {ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X Rev 0 with HB Mini Rev 5 - //{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini // never shipped - {ARKV6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 - {ARKV6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 - {ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO - {ARKV6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {ARKV6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 - {ARKV6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 + {ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 0 + {ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 1 + {ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 0 + {ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 1 + {ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 + {ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 + {ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 + {ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped }; /************************************************************************************ diff --git a/boards/ark/fmu-v6x/src/mtd.cpp b/boards/ark/fmu-v6x/src/mtd.cpp index 3ece10aeca..80d41e0b78 100644 --- a/boards/ark/fmu-v6x/src/mtd.cpp +++ b/boards/ark/fmu-v6x/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -45,18 +45,12 @@ static const px4_mft_device_t i2c3 = { // 24LC64T on Base 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/ark/fmu-v6x/src/spi.cpp b/boards/ark/fmu-v6x/src/spi.cpp index 2d047cff1d..f83a5a91cd 100644 --- a/boards/ark/fmu-v6x/src/spi.cpp +++ b/boards/ark/fmu-v6x/src/spi.cpp @@ -59,7 +59,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(ARKV6X01, { // Placeholder + initSPIHWVersion(ARKV6X01, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X10, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -81,6 +104,121 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(ARKV6X11, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X40, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X41, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X50, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X51, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/atl/mantis-edu/nuttx-config/nsh/defconfig b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig index 3556e01746..b6e976d427 100644 --- a/boards/atl/mantis-edu/nuttx-config/nsh/defconfig +++ b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/av/x-v1/nuttx-config/nsh/defconfig b/boards/av/x-v1/nuttx-config/nsh/defconfig index 4c2bca3b4f..d5e4039047 100644 --- a/boards/av/x-v1/nuttx-config/nsh/defconfig +++ b/boards/av/x-v1/nuttx-config/nsh/defconfig @@ -120,7 +120,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_LIBC_LONG_LONG=y diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 326273dda2..e7e9705f2d 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -15,7 +15,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y -# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_GNSS is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig index ff4b7ff6ff..15c99f925e 100644 --- a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig @@ -105,7 +105,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/bitcraze/crazyflie/src/mtd.cpp b/boards/bitcraze/crazyflie/src/mtd.cpp index 2b00e231b2..d81e3e4596 100644 --- a/boards/bitcraze/crazyflie/src/mtd.cpp +++ b/boards/bitcraze/crazyflie/src/mtd.cpp @@ -42,18 +42,12 @@ static const px4_mft_device_t i2c1 = { // 24AA64FT on Base 8K 32 X static const px4_mtd_entry_t fmu_eeprom = { .device = &i2c1, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", .nblocks = 128 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 128 - } }, }; diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index a5e5244cc8..aa55b50621 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -14,7 +14,8 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y -# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_GNSS is not set +# CONFIG_EKF2_MAGNETOMETER is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y @@ -33,6 +34,7 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MTD=y diff --git a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig index df3a85dc70..a8e182bc1d 100644 --- a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig index 64f884c2db..aa13dbdb1e 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig @@ -119,6 +119,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/cuav/can-gps-v1/uavcan_board_identity b/boards/cuav/can-gps-v1/uavcan_board_identity index a547589079..0cf273dde7 100644 --- a/boards/cuav/can-gps-v1/uavcan_board_identity +++ b/boards/cuav/can-gps-v1/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/cuav/nora/nuttx-config/bootloader/defconfig b/boards/cuav/nora/nuttx-config/bootloader/defconfig index 18b0a188a1..cc5ca67d2d 100644 --- a/boards/cuav/nora/nuttx-config/bootloader/defconfig +++ b/boards/cuav/nora/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cuav/nora/nuttx-config/nsh/defconfig b/boards/cuav/nora/nuttx-config/nsh/defconfig index a99ceca7ce..b84688f9b1 100644 --- a/boards/cuav/nora/nuttx-config/nsh/defconfig +++ b/boards/cuav/nora/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig index 53f3affc7c..d0e6e6194a 100644 --- a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig +++ b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cuav/x7pro/nuttx-config/nsh/defconfig b/boards/cuav/x7pro/nuttx-config/nsh/defconfig index 56405a89fe..039d5ee9b9 100644 --- a/boards/cuav/x7pro/nuttx-config/nsh/defconfig +++ b/boards/cuav/x7pro/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cuav/x7pro/nuttx-config/test/defconfig b/boards/cuav/x7pro/nuttx-config/test/defconfig index 789a10b6c2..392e47c9b4 100644 --- a/boards/cuav/x7pro/nuttx-config/test/defconfig +++ b/boards/cuav/x7pro/nuttx-config/test/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index 521dd3bde8..91ddcda596 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_COMMON_INS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y @@ -49,7 +50,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y -CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig index 548e9761c0..59041c61c4 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig index 333702c251..192e7c713a 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig index 6def80af98..9bb0b35420 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index e9f6c2d005..837876ee7d 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -20,6 +20,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y +CONFIG_COMMON_INS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors index 61a3fbf138..2b08f29764 100644 --- a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors +++ b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors @@ -8,16 +8,22 @@ board_adc start # 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649} # 2. Isolated {ICM42688p, ICM42688p}, body-fixed {ICM20649, ICM45686, AK09918} # 3. Isolated {ICM42688p, ICM42688p}, body-fixed {ICM45686, AK09918} +# 4. Isolated {ICM45686, ICM45686}, body-fixed {ICM45686, AK09918} # SPI4 is isolated, SPI1 is body-fixed # SPI4, isolated ms5611 -s -b 4 start -icm42688p -s -b 4 -R 10 start -c 15 -if ! icm20948 -s -b 4 -R 10 -M -q start +if icm42688p -s -b 4 -R 10 -q start -c 15 then - icm42688p -s -b 4 -R 6 start -c 13 + if ! icm20948 -s -b 4 -R 10 -M -q start + then + icm42688p -s -b 4 -R 6 start -c 13 + fi +else + icm45686 -s -b 4 -R 10 start -c 15 + icm45686 -s -b 4 -R 6 start -c 13 fi # SPI1, body-fixed diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig index 22823dd3b1..3861e2c8dc 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig @@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig index 367658878f..3a3d954783 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig index 8161de188a..9b0814ae08 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/cubepilot/cubeorangeplus/src/spi.cpp b/boards/cubepilot/cubeorangeplus/src/spi.cpp index fe5ca16172..b14c9c6bcb 100644 --- a/boards/cubepilot/cubeorangeplus/src/spi.cpp +++ b/boards/cubepilot/cubeorangeplus/src/spi.cpp @@ -49,7 +49,9 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI4, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin4}), // MPU_EXT_CS initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS }), }; diff --git a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig index be2bd9abc5..c696b87e08 100644 --- a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig index 185e4c27bc..07573c0f9e 100644 --- a/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig +++ b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig @@ -62,7 +62,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y diff --git a/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig b/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig index ee3bde1eb1..ea831c0b91 100644 --- a/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig +++ b/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig @@ -109,7 +109,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y diff --git a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig index 3ea9c492e6..d295254d69 100644 --- a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig +++ b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig @@ -132,6 +132,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/freefly/can-rtk-gps/uavcan_board_identity b/boards/freefly/can-rtk-gps/uavcan_board_identity index 997b26e2ae..2531725501 100644 --- a/boards/freefly/can-rtk-gps/uavcan_board_identity +++ b/boards/freefly/can-rtk-gps/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig index 4dcee619f1..f95e8f5c0c 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig @@ -120,6 +120,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/holybro/can-gps-v1/uavcan_board_identity b/boards/holybro/can-gps-v1/uavcan_board_identity index c34b51cd77..bb7a514fd6 100644 --- a/boards/holybro/can-gps-v1/uavcan_board_identity +++ b/boards/holybro/can-gps-v1/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig index 48482d5d4d..be49e9a560 100644 --- a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig +++ b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig index 4464f881d1..19647f3b1f 100644 --- a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -149,6 +149,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 716186400e..bcaedd9cf2 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -17,11 +17,19 @@ CONFIG_DRIVERS_OSD_ATXXXX=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_MAGNETOMETER is not set +# CONFIG_EKF2_RANGE_FINDER is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOGGER=y @@ -33,6 +41,9 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set +# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_TOP=y diff --git a/boards/holybro/kakutef7/init/rc.board_defaults b/boards/holybro/kakutef7/init/rc.board_defaults index 01fd668a3d..7a3bc92e84 100644 --- a/boards/holybro/kakutef7/init/rc.board_defaults +++ b/boards/holybro/kakutef7/init/rc.board_defaults @@ -12,13 +12,11 @@ param set-default CBRK_SUPPLY_CHK 894281 # Select the Generic 250 Racer by default param set-default SYS_AUTOSTART 4050 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_W_ACC 0.4 -param set-default ATT_W_GYRO_BIAS 0 - param set-default SYS_HAS_MAG 0 +# enable gravity fusion +param set-default EKF2_IMU_CONTROL 7 + # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 diff --git a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig index 3410f97087..88d57ea784 100644 --- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig @@ -115,7 +115,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -150,6 +150,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 713c736d50..a9f38ba78c 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7/init/rc.board_defaults b/boards/holybro/kakuteh7/init/rc.board_defaults index 413b2e6ff6..47dea71be6 100644 --- a/boards/holybro/kakuteh7/init/rc.board_defaults +++ b/boards/holybro/kakuteh7/init/rc.board_defaults @@ -22,12 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281 # Select the Generic 250 Racer by default param set-default SYS_AUTOSTART 4050 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_W_ACC 0.4 -param set-default ATT_W_GYRO_BIAS 0 - +# use EKF2 without mag param set-default SYS_HAS_MAG 0 +# and enable gravity fusion +param set-default EKF2_IMU_CONTROL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 diff --git a/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig index 1ba7aaa65e..4557ffca77 100644 --- a/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig +++ b/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig index a716028e20..4dc58311a1 100644 --- a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -149,6 +149,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index b019b753f9..3310e79fbb 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -32,6 +32,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig index 77dda970ac..bc47e6b75b 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig +++ b/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig index bff4b99ca0..685ea6c247 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index d326a78797..9c060d8442 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig index 89289fd968..10011bc456 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig +++ b/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig index 338df71016..1b85f5b0fd 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/holybro/pix32v5/nuttx-config/nsh/defconfig b/boards/holybro/pix32v5/nuttx-config/nsh/defconfig index 3a834b3eec..370b24dfe6 100644 --- a/boards/holybro/pix32v5/nuttx-config/nsh/defconfig +++ b/boards/holybro/pix32v5/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig b/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig index 196b5382ec..0e5f3cf684 100644 --- a/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig +++ b/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig @@ -30,7 +30,7 @@ CONFIG_FDCLONE_STDIO=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig b/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig index 266c0e3666..f74d825a37 100644 --- a/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig +++ b/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig @@ -104,7 +104,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -138,6 +138,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=131072 CONFIG_RAM_START=0x20000000 diff --git a/boards/matek/gnss-m9n-f4/uavcan_board_identity b/boards/matek/gnss-m9n-f4/uavcan_board_identity index 70123f7d91..fb24ed9f11 100755 --- a/boards/matek/gnss-m9n-f4/uavcan_board_identity +++ b/boards/matek/gnss-m9n-f4/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/matek/h743-mini/nuttx-config/bootloader/defconfig b/boards/matek/h743-mini/nuttx-config/bootloader/defconfig index b005f7cff9..a5e157f541 100644 --- a/boards/matek/h743-mini/nuttx-config/bootloader/defconfig +++ b/boards/matek/h743-mini/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/h743-mini/nuttx-config/nsh/defconfig b/boards/matek/h743-mini/nuttx-config/nsh/defconfig index c96190d81a..600180fa6d 100644 --- a/boards/matek/h743-mini/nuttx-config/nsh/defconfig +++ b/boards/matek/h743-mini/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index fed4e6c1a7..ce5f222229 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -12,6 +12,7 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y diff --git a/boards/matek/h743-slim/init/rc.board_sensors b/boards/matek/h743-slim/init/rc.board_sensors index c37c3b3b58..e6761906ee 100644 --- a/boards/matek/h743-slim/init/rc.board_sensors +++ b/boards/matek/h743-slim/init/rc.board_sensors @@ -5,15 +5,25 @@ board_adc start -# Internal SPI bus ICM-42605 -if ! icm42605 -R 14 -s start +# Different board versions have different IMUs, so we try all known options + +# Internal SPI bus ICM-42688P (SPI1) on V3 board, PITCH180 orientation +if ! icm42688p -s -b 1 -R 12 start then - # internal SPI bus ICM-20602 - icm20602 -R 12 -s start + # Internal SPI bus MPU-6000 on V1.0 and V1.5 boards + mpu6000 -s -b 1 -R 12 start fi -# Internal SPI bus MPU-6000 -mpu6000 -R 12 -s start +# Internal SPI bus ICM-42688P (SPI4) on V3 board, PITCH180_YAW90 orientation +if ! icm42688p -s -b 4 -R 26 start +then + # Internal SPI bus ICM-42605 on V1.5 board, ROTATION_ROLL_180_YAW_270 orientation + if ! icm42605 -s -b 4 -R 14 start + then + # Internal SPI bus ICM-20602 on V1.0 board, PITCH180 orientation + icm20602 -s -b 4 -R 12 start + fi +fi # Internal baro dps310 -I start -a 118 diff --git a/boards/matek/h743-slim/nuttx-config/bootloader/defconfig b/boards/matek/h743-slim/nuttx-config/bootloader/defconfig index 7834d7b54c..636630ffd7 100644 --- a/boards/matek/h743-slim/nuttx-config/bootloader/defconfig +++ b/boards/matek/h743-slim/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/h743-slim/nuttx-config/nsh/defconfig b/boards/matek/h743-slim/nuttx-config/nsh/defconfig index 55277d0cd6..cc3124fca0 100644 --- a/boards/matek/h743-slim/nuttx-config/nsh/defconfig +++ b/boards/matek/h743-slim/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/matek/h743-slim/src/spi.cpp b/boards/matek/h743-slim/src/spi.cpp index db673bedf0..84f887aa69 100644 --- a/boards/matek/h743-slim/src/spi.cpp +++ b/boards/matek/h743-slim/src/spi.cpp @@ -38,7 +38,11 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { + // Matek H743 Slim V1.0 and V1.5 initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + + // Matek H743 Slim V3 + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), }), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), @@ -48,8 +52,14 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin2}), }), initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}), + // Matek H743 Slim V1.0 initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortE, GPIO::Pin11}), + + // Matek H743 Slim V1.5 + initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}), + + // Matek H743 Slim V3 + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), }), }; diff --git a/boards/matek/h743/nuttx-config/bootloader/defconfig b/boards/matek/h743/nuttx-config/bootloader/defconfig index b6ebc6657b..a39e59702e 100644 --- a/boards/matek/h743/nuttx-config/bootloader/defconfig +++ b/boards/matek/h743/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/h743/nuttx-config/nsh/defconfig b/boards/matek/h743/nuttx-config/nsh/defconfig index 9e54278d41..eba77a1a90 100644 --- a/boards/matek/h743/nuttx-config/nsh/defconfig +++ b/boards/matek/h743/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/modalai/fc-v1/nuttx-config/nsh/defconfig b/boards/modalai/fc-v1/nuttx-config/nsh/defconfig index e53609c01d..d1f84d56ee 100644 --- a/boards/modalai/fc-v1/nuttx-config/nsh/defconfig +++ b/boards/modalai/fc-v1/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig b/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig index 8e0c1c6a9e..1a138a659b 100644 --- a/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig +++ b/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/modalai/fc-v2/nuttx-config/nsh/defconfig b/boards/modalai/fc-v2/nuttx-config/nsh/defconfig index f35aba9035..7fb9744c18 100644 --- a/boards/modalai/fc-v2/nuttx-config/nsh/defconfig +++ b/boards/modalai/fc-v2/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y @@ -152,6 +152,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/modalai/fc-v2/src/mtd.cpp b/boards/modalai/fc-v2/src/mtd.cpp index 644b96581d..48a5bd334c 100644 --- a/boards/modalai/fc-v2/src/mtd.cpp +++ b/boards/modalai/fc-v2/src/mtd.cpp @@ -34,25 +34,19 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp index b3c020bd76..797df2e632 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -133,6 +133,15 @@ enum INT_CONFIG_BIT : uint8_t { INT1_POLARITY = Bit0, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0, +}; + // FIFO_CONFIG enum FIFO_CONFIG_BIT : uint8_t { // 7:6 FIFO_MODE diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig index 94d49e5420..1167e3eefa 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig index b8a6f0e779..0811c67f0a 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -149,6 +149,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 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 41d6586235..93af46bab6 100644 --- a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig index 764c0f3d5a..64878f212a 100644 --- a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig index f2d08b3e71..8a44a577f1 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig index e169c24027..cbfdca2b5c 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig index 7fd7bd827e..5a696370f9 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig index c533863957..c5f40e5b55 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig b/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig index c6303e9005..584011e926 100644 --- a/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig index f309a3fe3f..e8005ce5bb 100644 --- a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/mro/x21-777/nuttx-config/nsh/defconfig b/boards/mro/x21-777/nuttx-config/nsh/defconfig index 8774af0687..5a768e24ed 100644 --- a/boards/mro/x21-777/nuttx-config/nsh/defconfig +++ b/boards/mro/x21-777/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/mro/x21/nuttx-config/nsh/defconfig b/boards/mro/x21/nuttx-config/nsh/defconfig index 60e71b3c9e..681db5c36b 100644 --- a/boards/mro/x21/nuttx-config/nsh/defconfig +++ b/boards/mro/x21/nuttx-config/nsh/defconfig @@ -107,7 +107,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig index c6f55f90ce..57d0c64bb6 100644 --- a/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig @@ -61,7 +61,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_KINETIS_ADC0=y @@ -168,6 +168,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig index 947e059674..377f2cd48e 100644 --- a/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig @@ -62,7 +62,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=48 CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y @@ -171,6 +171,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig index 31c4b33b66..9ef98c951f 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig @@ -65,7 +65,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y @@ -171,6 +171,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig index adcfcee49f..e99d7c6e90 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig @@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=48 CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y @@ -170,6 +170,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig index 83ac893d6c..9818be1f54 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig @@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_KINETIS_ADC0=y @@ -171,6 +171,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig index 66968d1d24..31175960f9 100644 --- a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig @@ -115,7 +115,7 @@ CONFIG_IMXRT_USDHC1=y CONFIG_IMXRT_USDHC1_INVERT_CD=y CONFIG_IMXRT_USDHC1_WIDTH_D1_D4=y CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_LPUART2_BAUD=57600 @@ -184,6 +184,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=1048576 CONFIG_RAM_START=0x20200000 diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig index 1f9258e5ab..77861ee597 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -85,6 +85,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DEV_URANDOM=y CONFIG_ETH0_PHY_TJA1103=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -109,7 +110,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=900 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -175,6 +176,7 @@ CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_IGMP=y CONFIG_NET_NACTIVESOCKETS=16 CONFIG_NET_SOLINGER=y CONFIG_NET_TCP=y diff --git a/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig index 83287f2d3b..e9d287c2cd 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig @@ -110,7 +110,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=900 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y diff --git a/boards/nxp/mr-canhubk3/zenoh.px4board b/boards/nxp/mr-canhubk3/zenoh.px4board new file mode 100644 index 0000000000..28de5ea5cc --- /dev/null +++ b/boards/nxp/mr-canhubk3/zenoh.px4board @@ -0,0 +1,58 @@ +# CONFIG_BOARD_ROMFSROOT is not set +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_ZENOH=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index 2e52bc7b72..0cd4598d2a 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -3,7 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ -pwm_out mode_pwm1 start +pwm_out start ifup can0 diff --git a/boards/nxp/ucans32k146/uavcan_board_identity b/boards/nxp/ucans32k146/uavcan_board_identity index dd7e3fcb77..7ff76347f8 100644 --- a/boards/nxp/ucans32k146/uavcan_board_identity +++ b/boards/nxp/ucans32k146/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig index 709844f253..b154c61070 100644 --- a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig +++ b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig @@ -62,7 +62,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y diff --git a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig index 221553a723..f3d181112b 100644 --- a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig @@ -103,7 +103,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -136,6 +136,8 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_DEFAULT_PRIO_INHERIT=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig index bbf056f6a3..2c9487ddaf 100644 --- a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index 8ac74111e4..3566ccc30c 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -107,7 +107,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4/nuttx-config/test/defconfig b/boards/px4/fmu-v4/nuttx-config/test/defconfig index 2a09e687bf..3c90516de3 100644 --- a/boards/px4/fmu-v4/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/test/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4/uavcan_board_identity b/boards/px4/fmu-v4/uavcan_board_identity index 0905752e49..507d3ec578 100644 --- a/boards/px4/fmu-v4/uavcan_board_identity +++ b/boards/px4/fmu-v4/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig index 6dad97af68..e479b13646 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig @@ -107,7 +107,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=393216 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4pro/nuttx-config/test/defconfig b/boards/px4/fmu-v4pro/nuttx-config/test/defconfig index ff337efc27..a1145fe284 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v4pro/nuttx-config/test/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=393216 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4pro/src/mtd.cpp b/boards/px4/fmu-v4pro/src/mtd.cpp index 4bd93619db..f32f1dee1d 100644 --- a/boards/px4/fmu-v4pro/src/mtd.cpp +++ b/boards/px4/fmu-v4pro/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi2 = { // FM25V01A on FMUM 16K +static const px4_mft_device_t spi2 = { // FM25V01A on FMUM native: 16K X 8, emulated as (512 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -46,7 +46,7 @@ static const px4_mtd_entry_t fmum_fram = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 + .nblocks = (16384 / (1 << CONFIG_RAMTRON_EMULATE_PAGE_SHIFT)) }, }, }; diff --git a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig index cfa3c56802..171589b494 100644 --- a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig b/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig index 3f40bfd397..2c7bafc1e8 100644 --- a/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/nuttx-config/debug/defconfig b/boards/px4/fmu-v5/nuttx-config/debug/defconfig index 52cd3e1c70..08e2bb9e8a 100644 --- a/boards/px4/fmu-v5/nuttx-config/debug/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/debug/defconfig @@ -157,7 +157,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=1064 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -192,6 +192,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index ba372ae3ec..ae31f2b0af 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/nuttx-config/protected/defconfig b/boards/px4/fmu-v5/nuttx-config/protected/defconfig index bba4ab5b26..7733599a7f 100644 --- a/boards/px4/fmu-v5/nuttx-config/protected/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/protected/defconfig @@ -115,7 +115,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="px4_entry" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -152,6 +152,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 334e594a27..7350cb4532 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=878 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/nuttx-config/test/defconfig b/boards/px4/fmu-v5/nuttx-config/test/defconfig index 4e653c46ee..21babe66f6 100644 --- a/boards/px4/fmu-v5/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/test/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index de913d6ac8..e5371b2a7d 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -36,8 +36,6 @@ CONFIG_SYSTEMCMDS_MTD=n CONFIG_SYSTEMCMDS_NSHTERM=n CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SD_BENCH=n -CONFIG_SYSTEMCMDS_SD_STRESS=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_USB_CONNECTED=n CONFIG_BOARD_PROTECTED=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index fb10b4c77a..812a0f7295 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -3,6 +3,8 @@ CONFIG_COMMON_BAROMETERS=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_HYGROMETERS=n +CONFIG_COMMON_OSD=n +CONFIG_COMMON_RC=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_ADC_ADS1115=n CONFIG_DRIVERS_BATT_SMBUS=n @@ -35,7 +37,6 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_SYSTEMCMDS_REFLECT=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index 70e35abd44..c9e9823c03 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -32,8 +32,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=n CONFIG_SYSTEMCMDS_LED_CONTROL=n CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SD_BENCH=n -CONFIG_SYSTEMCMDS_SD_STRESS=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default" diff --git a/boards/px4/fmu-v5x/cmake/upload.cmake b/boards/px4/fmu-v5x/cmake/upload.cmake new file mode 100644 index 0000000000..e5c23c3e1c --- /dev/null +++ b/boards/px4/fmu-v5x/cmake/upload.cmake @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4) + +add_custom_target(upload_skynode_usb + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) + +add_custom_target(upload_skynode_wifi + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 0b6e7f6d7a..8ba73bcdcc 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -37,6 +37,7 @@ CONFIG_COMMON_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y @@ -83,6 +84,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_BSONDUMP=y diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index 50402d17b2..2ff61617b9 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -18,6 +18,12 @@ param set-default SENS_EN_INA226 1 param set-default SYS_USE_IO 1 +if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 +then + # Skynode: use the "custom participant" config for uxrce_dds_client + param set-default UXRCE_DDS_PTCFG 2 +fi + safety_button start # GPIO Expander driver on external I2C3 diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 4f7c1d078d..d6b91f4505 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -94,6 +94,9 @@ else # Internal magnetometer on I2c bmm150 -I -R 6 start + # Auto start power monitors + pm_selector_auterion start + fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index d72ba45c6d..8c21923c64 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -185,6 +185,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5x/nuttx-config/test/defconfig b/boards/px4/fmu-v5x/nuttx-config/test/defconfig index e5b66b466a..12333125f9 100644 --- a/boards/px4/fmu-v5x/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/test/defconfig @@ -116,7 +116,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -183,6 +183,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v5x/src/mtd.cpp b/boards/px4/fmu-v5x/src/mtd.cpp index fc1e97d2c8..c0139d1232 100644 --- a/boards/px4/fmu-v5x/src/mtd.cpp +++ b/boards/px4/fmu-v5x/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -50,18 +50,12 @@ static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig index 078e5aeafa..a5269376c6 100644 --- a/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig index b527b9a2a6..9e051bd1f9 100644 --- a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y @@ -151,6 +151,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v6c/src/mtd.cpp b/boards/px4/fmu-v6c/src/mtd.cpp index b315ff399d..5efe7bac64 100644 --- a/boards/px4/fmu-v6c/src/mtd.cpp +++ b/boards/px4/fmu-v6c/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi2 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi2 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -47,18 +47,12 @@ static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi2, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig index e3ce5f2c46..0ca6729cfa 100644 --- a/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig index fa9b81078c..40513352ae 100644 --- a/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y @@ -151,6 +151,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v6u/src/mtd.cpp b/boards/px4/fmu-v6u/src/mtd.cpp index 644b96581d..48a5bd334c 100644 --- a/boards/px4/fmu-v6u/src/mtd.cpp +++ b/boards/px4/fmu-v6u/src/mtd.cpp @@ -34,25 +34,19 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6x/cmake/upload.cmake b/boards/px4/fmu-v6x/cmake/upload.cmake new file mode 100644 index 0000000000..7319b46df3 --- /dev/null +++ b/boards/px4/fmu-v6x/cmake/upload.cmake @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4) + +add_custom_target(upload_skynode_usb + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) + +add_custom_target(upload_skynode_wifi + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index cb9b676b68..9dc9d9a444 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -19,20 +19,23 @@ CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_COMMON_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index ba812e4b7b..d277bd771c 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -18,4 +18,10 @@ param set-default SENS_EN_INA226 1 param set-default SYS_USE_IO 1 +if ver hwtypecmp V6X009010 V6X010010 +then + # Skynode: use the "custom participant" config for uxrce_dds_client + param set-default UXRCE_DDS_PTCFG 2 +fi + safety_button start diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 52aa8f9c78..d3ae62d8a4 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -47,53 +47,66 @@ then fi fi - -if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 +#Start Auterion Power Module selector for Skynode boards +if ver hwtypecmp V6X009010 V6X010010 then - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start + pm_selector_auterion start +fi + +if ver hwtypecmp V6X000006 V6X004006 V6X005006 +then + # Internal SPI bus ICM45686 + icm45686 -s -R 10 start + iim42652 -s -R 6 start + adis16470 -s -R 0 start else - # Internal SPI BMI088 + if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 + then + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start + else + # Internal SPI BMI088 + if ver hwtypecmp V6X009010 V6X010010 + then + bmi088 -A -R 6 -s start + bmi088 -G -R 6 -s start + else + if ver hwtypecmp V6X000010 + then + bmi088 -A -R 0 -s start + bmi088 -G -R 0 -s start + else + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + fi + fi + fi + + # Internal SPI bus ICM42688p if ver hwtypecmp V6X009010 V6X010010 then - bmi088 -A -R 6 -s start - bmi088 -G -R 6 -s start + icm42688p -R 12 -s start else if ver hwtypecmp V6X000010 then - bmi088 -A -R 0 -s start - bmi088 -G -R 0 -s start + icm42688p -R 14 -s start else - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + icm42688p -R 6 -s start fi fi -fi -# Internal SPI bus ICM42688p -if ver hwtypecmp V6X009010 V6X010010 -then - icm42688p -R 12 -s start -else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 then - icm42688p -R 14 -s start + # Internal SPI bus ICM-42670-P (hard-mounted) + icm42670p -R 10 -s start else - icm42688p -R 6 -s start - fi -fi - -if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 -then - # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 10 -s start -else - if ver hwtypecmp V6X009010 V6X010010 - then - icm20602 -R 6 -s start - else - # Internal SPI bus ICM-20649 (hard-mounted) - icm20649 -R 14 -s start + if ver hwtypecmp V6X009010 V6X010010 + then + icm20602 -R 6 -s start + else + # Internal SPI bus ICM-20649 (hard-mounted) + icm20649 -R 14 -s start + fi fi fi @@ -102,13 +115,8 @@ if ver hwtypecmp V6X002001 then rm3100 -I -b 4 start else - if ver hwtypecmp V6X009010 V6X010010 - then - # Internal magnetometer on I2C - bmm150 -I -R 0 start - else - bmm150 -I -R 6 start - fi + # Internal magnetometer on I2C + bmm150 -I -R 0 start fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) @@ -117,7 +125,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 + if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006 then icp201xx -I -a 0x64 start else diff --git a/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig index 8542ec837e..41dc164e0a 100644 --- a/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig @@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index 13191fc716..e5f02a46a7 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -118,7 +118,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -189,6 +189,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index fecd7efc9c..ff0676fab7 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -215,24 +215,28 @@ #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets // Base/FMUM #define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 #define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 #define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 #define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 +#define V6X06 HW_VER_REV(0x0,0x6) // FMUV6X, Sensor Set Rev 6 #define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 #define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 #define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 +#define V6X16 HW_VER_REV(0x1,0x6) // NO PX4IO, Sensor Set Rev 6 #define V6X21 HW_VER_REV(0x2,0x1) // FMUV6X, CUAV Sensor Set #define V6X40 HW_VER_REV(0x4,0x0) // FMUV6X, HB CM4 base Rev 0 #define V6X41 HW_VER_REV(0x4,0x1) // FMUV6X, BMI388 I2C2 HB CM4 base Rev 1 #define V6X43 HW_VER_REV(0x4,0x3) // FMUV6X, Sensor Set HB CM4 base Rev 3 #define V6X44 HW_VER_REV(0x4,0x4) // FMUV6X, Sensor Set HB CM4 base Rev 4 +#define V6X46 HW_VER_REV(0x4,0x6) // FMUV6X, Sensor Set HB CM4 base Rev 6 #define V6X50 HW_VER_REV(0x5,0x0) // FMUV6X, HB Mini Rev 0 #define V6X51 HW_VER_REV(0x5,0x1) // FMUV6X, BMI388 I2C2 HB Mini Rev 1 #define V6X53 HW_VER_REV(0x5,0x3) // FMUV6X, Sensor Set HB Mini Rev 3 #define V6X54 HW_VER_REV(0x5,0x4) // FMUV6X, Sensor Set HB Mini Rev 4 +#define V6X56 HW_VER_REV(0x5,0x6) // FMUV6X, Sensor Set HB Mini Rev 6 #define V6X90 HW_VER_REV(0x9,0x0) // Rev 0 #define V6X0910 HW_VER_REV(0x9,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver9 #define V6X1010 HW_VER_REV(0x10,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver10 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index 313d49f9bf..bda800b749 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -142,17 +142,20 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 + {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 + {V6X06, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 {V6X40, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // HB CM4 base {V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 HB CM4 base {V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 {V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 + {V6X46, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 + {V6X56, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 6 {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 {V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver9 diff --git a/boards/px4/fmu-v6x/src/mtd.cpp b/boards/px4/fmu-v6x/src/mtd.cpp index fc1e97d2c8..c0139d1232 100644 --- a/boards/px4/fmu-v6x/src/mtd.cpp +++ b/boards/px4/fmu-v6x/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -50,18 +50,12 @@ static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index d3bbdb9315..8b002e157e 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -84,6 +84,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIHWVersion(V6X06, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X21, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -132,6 +155,52 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIHWVersion(V6X44, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V6X46, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X50, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -156,75 +225,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X44, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - // never shipped - //initSPIHWVersion(V6X50, { - // initSPIBus(SPI::Bus::SPI1, { - // initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - // }, {GPIO::PortI, GPIO::Pin11}), - // initSPIBus(SPI::Bus::SPI2, { - // initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - // }, {GPIO::PortF, GPIO::Pin4}), - // initSPIBus(SPI::Bus::SPI3, { - // initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - // initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - // }, {GPIO::PortE, GPIO::Pin7}), - // // initSPIBus(SPI::Bus::SPI4, { - // // // no devices - // // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // // }, {GPIO::PortG, GPIO::Pin8}), - // initSPIBus(SPI::Bus::SPI5, { - // initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - // }), - // initSPIBusExternal(SPI::Bus::SPI6, { - // initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - // initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - // }), - //}), - initSPIHWVersion(V6X04, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // initSPIBus(SPI::Bus::SPI4, { - // // no devices - // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h - // }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), initSPIHWVersion(V6X53, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -248,6 +248,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X54, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -270,6 +271,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(V6X56, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X0910, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -293,6 +318,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X1010, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), diff --git a/boards/px4/fmu-v6x/zenoh.px4board b/boards/px4/fmu-v6x/zenoh.px4board new file mode 100644 index 0000000000..cb14fde935 --- /dev/null +++ b/boards/px4/fmu-v6x/zenoh.px4board @@ -0,0 +1,4 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_DRIVERS_UAVCAN=n +CONFIG_MODULES_UXRCE_DDS_CLIENT=n +CONFIG_MODULES_ZENOH=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index db358c3e4d..fa7276f76e 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -49,6 +49,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DYN=y diff --git a/boards/px4/sitl/src/sitl_led.c b/boards/px4/sitl/src/sitl_led.c index c9d3f4d053..f86e877764 100644 --- a/boards/px4/sitl/src/sitl_led.c +++ b/boards/px4/sitl/src/sitl_led.c @@ -50,7 +50,7 @@ __END_DECLS static bool _led_state[2] = { false, false }; -__EXPORT void led_init() +__EXPORT void led_init(void) { PX4_DEBUG("LED_INIT"); } diff --git a/boards/px4/sitl/zenoh.px4board b/boards/px4/sitl/zenoh.px4board new file mode 100644 index 0000000000..58a29ad52f --- /dev/null +++ b/boards/px4/sitl/zenoh.px4board @@ -0,0 +1,3 @@ +CONFIG_MODULES_UXRCE_DDS_CLIENT=n +CONFIG_MODULES_ZENOH=y +CONFIG_ZENOH_CONFIG_PATH="./zenoh" diff --git a/boards/raspberrypi/pico/nuttx-config/nsh/defconfig b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig index 4c73af3bb9..ebce067020 100644 --- a/boards/raspberrypi/pico/nuttx-config/nsh/defconfig +++ b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 0de2a4311a..ccb3bb3ce9 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -16,6 +16,8 @@ CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y +CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ=25000000 CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y diff --git a/boards/siyi/n7/bootloader.px4board b/boards/siyi/n7/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/siyi/n7/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/siyi/n7/default.px4board b/boards/siyi/n7/default.px4board new file mode 100644 index 0000000000..cb42f3530c --- /dev/null +++ b/boards/siyi/n7/default.px4board @@ -0,0 +1,86 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS2" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/siyi/n7/extras/px4_io-v2_default.bin b/boards/siyi/n7/extras/px4_io-v2_default.bin new file mode 100755 index 0000000000..957f0f13a1 Binary files /dev/null and b/boards/siyi/n7/extras/px4_io-v2_default.bin differ diff --git a/boards/siyi/n7/extras/siyi_n7_bootloader.bin b/boards/siyi/n7/extras/siyi_n7_bootloader.bin new file mode 100755 index 0000000000..6f0ff949f5 Binary files /dev/null and b/boards/siyi/n7/extras/siyi_n7_bootloader.bin differ diff --git a/boards/siyi/n7/firmware.prototype b/boards/siyi/n7/firmware.prototype new file mode 100644 index 0000000000..b4c1d9362a --- /dev/null +++ b/boards/siyi/n7/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1123, + "magic": "PX4FWv1", + "description": "Firmware for the N7 board", + "image": "", + "build_time": 0, + "summary": "N7", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/siyi/n7/init/rc.board_defaults b/boards/siyi/n7/init/rc.board_defaults new file mode 100644 index 0000000000..a7d3d20bf9 --- /dev/null +++ b/boards/siyi/n7/init/rc.board_defaults @@ -0,0 +1,14 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default MAV_0_RATE 100000 + +param set-default BAT1_V_DIV 18.1 + +param set-default BAT1_A_PER_V 36.367515152 + +# Enable IMU thermal control +param set-default SENS_EN_THERMAL 1 + +param set-default SYS_USE_IO 1 diff --git a/boards/siyi/n7/init/rc.board_sensors b/boards/siyi/n7/init/rc.board_sensors new file mode 100644 index 0000000000..e790785884 --- /dev/null +++ b/boards/siyi/n7/init/rc.board_sensors @@ -0,0 +1,21 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ +board_adc start + +# SPI1 +bmi088 -s -b 1 -A -R 2 start +bmi088 -s -b 1 -G -R 2 start + +# SPI1 +icm20689 -s -b 1 -R 2 start + +# I2C1 +ist8310 -X -b 1 -R 10 -a 0xE start + +# I2C3 +ist8310 -I -b 3 -R 10 -a 0xE start + +# SPI4 +ms5611 -s -b 4 start diff --git a/boards/siyi/n7/nuttx-config/Kconfig b/boards/siyi/n7/nuttx-config/Kconfig new file mode 100644 index 0000000000..bb33d3cfda --- /dev/null +++ b/boards/siyi/n7/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. diff --git a/boards/siyi/n7/nuttx-config/bootloader/defconfig b/boards/siyi/n7/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..775acfb2a0 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/bootloader/defconfig @@ -0,0 +1,93 @@ +# +# 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="../../../../boards/siyi/n7/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=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL Siyi N7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Siyi" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=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_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=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_DMA2=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_UART8=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_SIGTSTP=y +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_TXDMA=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/siyi/n7/nuttx-config/include/board.h b/boards/siyi/n7/nuttx-config/include/board.h new file mode 100644 index 0000000000..d6b690c293 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/include/board.h @@ -0,0 +1,381 @@ +/************************************************************************************ + * nuttx-config/include/board.h + * + * Copyright (C) 2016-2019 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 + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#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: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,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_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (16,000,000 / 1) * 60 = 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(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(60) +#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 / 1) * 60) +#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(48) +#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) * 48) +#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(48) +#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) * 48) +#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_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI /* I2C4 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 + +/* LED definitions ******************************************************************/ +/* The board has three, LED_GREEN a Green LED, LED_BLUE + * a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */ + +#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_UART4_RX GPIO_UART4_RX_5 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PD1 */ + + + +#define GPIO_UART7_RX GPIO_UART7_RX_4 /* PF6 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + +/* CAN + * + * CAN1 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_5 /* PI9 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_4 /* PH13 */ + +/* SPI + * SPI1 sensors + * SPI2 is FRAM. + * SPI4 is BARO + */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_3 /* PD7 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_3 /* PG11 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_4 /* PI3 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_6 /* PI1 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI4_SCK GPIO_SPI4_SCK_2 /* PE2 */ + + + +/* I2C + * + * Each I2C is associated with a U[S]ART + * hence the naming I2C2_SDA_UART4 in FMU USAGE spreadsheet + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_2 /* PF1 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_2 /* PF0 */ + +#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/siyi/n7/nuttx-config/include/board_dma_map.h b/boards/siyi/n7/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..6577106c23 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/include/board_dma_map.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 FRAM */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 FRAM */ + +#define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_0 /* DMA1:81 */ +#define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_0 /* DMA1:82 */ + diff --git a/boards/siyi/n7/nuttx-config/nsh/defconfig b/boards/siyi/n7/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..f2f14e7cc9 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/nsh/defconfig @@ -0,0 +1,252 @@ +# +# 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_DISABLE_PTHREAD 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_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/siyi/n7/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=768 +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_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 N7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Siyi" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +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_MAX_TASKS=64 +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_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +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=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=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_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +CONFIG_RAMTRON_SETSPEED=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_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +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_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=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_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=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_SPI4=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=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_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +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_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/siyi/n7/nuttx-config/scripts/bootloader_script.ld b/boards/siyi/n7/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..43d36e7dc9 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 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 + * + * The board has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.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. + * + * The bootloader uses the first sector of the flash, which is 128K in length. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (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(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > 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) } +} diff --git a/boards/siyi/n7/nuttx-config/scripts/script.ld b/boards/siyi/n7/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..d6019c0d13 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 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 + * + * The board has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.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) +EXTERN(board_get_manifest) + +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(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .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_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > 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) } +} diff --git a/boards/siyi/n7/src/CMakeLists.txt b/boards/siyi/n7/src/CMakeLists.txt new file mode 100644 index 0000000000..c1386c4181 --- /dev/null +++ b/boards/siyi/n7/src/CMakeLists.txt @@ -0,0 +1,70 @@ +############################################################################ +# +# Copyright (c) 2016 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 # sdio + nuttx_drivers # sdio + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + manifest.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/siyi/n7/src/board_config.h b/boards/siyi/n7/src/board_config.h new file mode 100644 index 0000000000..e705c87890 --- /dev/null +++ b/boards/siyi/n7/src/board_config.h @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_UART8_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_UART8_RX +#define PX4IO_SERIAL_BASE STM32_UART8_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_UART8 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_UART8_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_UART8_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB1LENR +#define PX4IO_SERIAL_RCC_EN RCC_APB1LENR_UART8EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +#define GPIO_nLED_RED /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PC7 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ + + + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PA0 */ GPIO_ADC1_INP16, \ + /* PA1 */ GPIO_ADC1_INP17, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PC2 */ GPIO_ADC123_INP12, \ + /* PC3 */ GPIO_ADC12_INP13 + + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PA0 */ 16 +#define ADC_BATTERY_CURRENT_CHANNEL /* PA1 */ 17 +#define ADC_RSSI_IN_CHANNEL /* PB0 */ 9 +#define ADC_SCALED_V5_CHANNEL /* PC0 */ 10 +#define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* PC1 */ 11 +#define ADC_HW_VER_SENSE_CHANNEL /* PC2 */ 12 +#define ADC_HW_REV_SENSE_CHANNEL /* PC3 */ 13 + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ + (1 << ADC_HW_VER_SENSE_CHANNEL) | \ + (1 << ADC_HW_REV_SENSE_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) + +/* HW Version and Revision drive signals Default to 1 to detect */ + +#define BOARD_HAS_HW_VERSIONING + +#define GPIO_HW_REV_DRIVE /* PH14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN14) +#define GPIO_HW_REV_SENSE /* PC3 */ GPIO_ADC12_INP13 +#define GPIO_HW_VER_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) +#define GPIO_HW_VER_SENSE /* PC2 */ GPIO_ADC123_INP12 +#define HW_INFO_INIT_PREFIX "VD" + + +#define VER00 HW_VER_REV(0x0,0x0) + +/* CAN Silence + * + * Silent mode control \ ESC Mux select + */ + +#define GPIO_CAN1_SILENT_S0 /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 5 + +#define BOARD_NUM_IO_TIMERS 2 + + +/* Power supply control and monitoring GPIOs */ + +#define GPIO_nPOWER_IN_A /* PG1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN1) +#define GPIO_nPOWER_IN_B /* PG3 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN3) + +#define GPIO_nVDD_BRICK_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_B /* USB Is Chosen */ + +#define GPIO_VDD_5V_PERIPH_nEN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) +#define GPIO_VDD_5V_PERIPH_nOC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) +#define GPIO_VDD_5V_HIPOWER_nEN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) +#define GPIO_VDD_5V_HIPOWER_nOC /* PG13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) +#define GPIO_VDD_3V3_SD_CARD_EN /* PG7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN7) + + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 15 /* timer 15 */ +#define TONE_ALARM_CHANNEL 1 /* PE5 TIM15_CH1 */ + +#define GPIO_BUZZER_1 /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN5) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM15_CH1OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ + +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL /* T4C2 */ 2 +#define GPIO_PWM_IN /* PD13 */ GPIO_TIM4_CH2IN + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* 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 (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) + +/* Board never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK_VALID)) + + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + + +/* 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 SDMMC_PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_2MHz)) + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_HW_REV_DRIVE, \ + GPIO_HW_VER_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_HEATER_OUTPUT, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_VDD_5V_PERIPH_nEN, \ + GPIO_VDD_5V_PERIPH_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D0), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D1), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D2), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D3), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_CMD), \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/siyi/n7/src/bootloader_main.c b/boards/siyi/n7/src/bootloader_main.c new file mode 100644 index 0000000000..cec6abd467 --- /dev/null +++ b/boards/siyi/n7/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2019-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 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_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/siyi/n7/src/can.c b/boards/siyi/n7/src/can.c new file mode 100644 index 0000000000..c71b2e8793 --- /dev/null +++ b/boards/siyi/n7/src/can.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif + +#endif /* CONFIG_CAN */ diff --git a/boards/siyi/n7/src/hw_config.h b/boards/siyi/n7/src/hw_config.h new file mode 100644 index 0000000000..a6b71126e0 --- /dev/null +++ b/boards/siyi/n7/src/hw_config.h @@ -0,0 +1,128 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 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 1123 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED // RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#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 + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/siyi/n7/src/i2c.cpp b/boards/siyi/n7/src/i2c.cpp new file mode 100644 index 0000000000..124fc2375c --- /dev/null +++ b/boards/siyi/n7/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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), + initI2CBusExternal(2), + initI2CBusInternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/siyi/n7/src/init.c b/boards/siyi/n7/src/init.c new file mode 100644 index 0000000000..46cf4ae4aa --- /dev/null +++ b/boards/siyi/n7/src/init.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019, 2022 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 + * + * PX4FMU-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. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * 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); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + VDD_5V_PERIPH_EN(false); + board_control_spi_sensors_power(false, 0xffff); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + board_control_spi_sensors_power(true, 0xffff); + VDD_5V_PERIPH_EN(true); +} + +/************************************************************************************ + * 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) +{ + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + board_control_spi_sensors_power_configgpio(); + + /* configure USB interfaces */ + + stm32_usbinitialize(); + +} + +/**************************************************************************** + * 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; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * 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 */ + VDD_5V_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + board_control_spi_sensors_power(true, 0xffff); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + // Use the default HW_VER_REV(0x0,0x0) for Ramtron + + stm32_spiinitialize(); + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + /* Configure the actual SPI interfaces (after we determined the HW version) */ + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + + /* 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); + } + + // Ensure Power is off for > 10 mS + usleep(15 * 1000); + VDD_3V3_SD_CARD_EN(true); + usleep(500 * 1000); + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +#endif /* CONFIG_MMCSD */ + + return OK; +} diff --git a/boards/siyi/n7/src/led.c b/boards/siyi/n7/src/led.c new file mode 100644 index 0000000000..df40a23dca --- /dev/null +++ b/boards/siyi/n7/src/led.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 px4fmu2_led.c + * + * PX4FMU 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 + +#ifdef CONFIG_ARCH_LEDS +static bool nuttx_owns_leds = true; +// B R S G +// 0 1 2 3 +static const uint8_t xlatpx4[] = {1, 2, 4, 0}; +# define xlat(p) xlatpx4[(p)] +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + 0, // Indexed by LED_SAFETY (defaulted to an input) + 0, // Indexed by LED_GREEN +}; + +#endif + +__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(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} + +#ifdef CONFIG_ARCH_LEDS +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/siyi/n7/src/manifest.c b/boards/siyi/n7/src/manifest.c new file mode 100644 index 0000000000..4bf33cab90 --- /dev/null +++ b/boards/siyi/n7/src/manifest.c @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_board[] = { + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {VER00, hw_mft_list_board, arraySize(hw_mft_list_board)}, +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 16; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/siyi/n7/src/sdio.c b/boards/siyi/n7/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/siyi/n7/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/siyi/n7/src/spi.cpp b/boards/siyi/n7/src/spi.cpp new file mode 100644 index 0000000000..c78c7bb250 --- /dev/null +++ b/boards/siyi/n7/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2020-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_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/siyi/n7/src/timer_config.cpp b/boards/siyi/n7/src/timer_config.cpp new file mode 100644 index 0000000000..c9bc0acd46 --- /dev/null +++ b/boards/siyi/n7/src/timer_config.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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}), +}; + +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::PortA, GPIO::Pin10}), + 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}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/siyi/n7/src/usb.c b/boards/siyi/n7/src/usb.c new file mode 100644 index 0000000000..6d42476b71 --- /dev/null +++ b/boards/siyi/n7/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__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 +} + +/************************************************************************************ + * 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_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig index edce2f773a..25c5120d57 100644 --- a/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig +++ b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig @@ -115,7 +115,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -181,6 +181,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 diff --git a/boards/sky-drones/smartap-airlink/src/mtd.cpp b/boards/sky-drones/smartap-airlink/src/mtd.cpp index 8b74a4617c..742b463041 100644 --- a/boards/sky-drones/smartap-airlink/src/mtd.cpp +++ b/boards/sky-drones/smartap-airlink/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -45,18 +45,12 @@ static const px4_mft_device_t i2c3 = { // 24LC64T on Base 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/thepeach/k1/nuttx-config/nsh/defconfig b/boards/thepeach/k1/nuttx-config/nsh/defconfig index 7d565b71aa..78c6bde8df 100644 --- a/boards/thepeach/k1/nuttx-config/nsh/defconfig +++ b/boards/thepeach/k1/nuttx-config/nsh/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/thepeach/r1/nuttx-config/nsh/defconfig b/boards/thepeach/r1/nuttx-config/nsh/defconfig index 4903f2754f..75c975f76f 100644 --- a/boards/thepeach/r1/nuttx-config/nsh/defconfig +++ b/boards/thepeach/r1/nuttx-config/nsh/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/uvify/core/nuttx-config/nsh/defconfig b/boards/uvify/core/nuttx-config/nsh/defconfig index d2ffa4f1a9..6d8e82e9f5 100644 --- a/boards/uvify/core/nuttx-config/nsh/defconfig +++ b/boards/uvify/core/nuttx-config/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=3094 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 CONFIG_RAMTRON_SETSPEED=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 1a54456194..c93b15734a 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -107,6 +107,7 @@ function(px4_add_functional_gtest) px4_daemon work_queue parameters + events perf tinybson uorb_msgs diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index ea5e48800e..eb418bf6cc 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -73,12 +73,18 @@ if(EXISTS ${BOARD_DEFCONFIG}) # Find the value string(REPLACE "${Name}=" "" Value ${NameAndValue}) - if(Value) - # remove extra quotes - string(REPLACE "\"" "" Value ${Value}) + # remove extra quotes + string(REPLACE "\"" "" Value ${Value}) - # Set the variable - set(${Name} ${Value} CACHE INTERNAL "BOARD DEFCONFIG: ${Name}" FORCE) + # Set the variable + set(${Name} ${Value} CACHE INTERNAL "BOARD DEFCONFIG: ${Name}" FORCE) + + else() + # Find boolean not set + string(REGEX MATCH " (CONFIG[^ ]+) is not set" Name ${NameAndValue}) + + if(${CMAKE_MATCH_1}) + set(${CMAKE_MATCH_1} "" CACHE INTERNAL "BOARD DEFCONFIG: ${CMAKE_MATCH_1}" FORCE) endif() endif() @@ -216,15 +222,17 @@ if(EXISTS ${BOARD_DEFCONFIG}) endforeach() - # Put every module not in userspace also to kernel list - foreach(modpath ${config_module_list}) + if (CONFIG_BOARD_PROTECTED) + # Put every module not in userspace also to kernel list + foreach(modpath ${config_module_list}) get_filename_component(module ${modpath} NAME) list(FIND config_user_list ${module} _index) if (${_index} EQUAL -1) list(APPEND config_kernel_list ${modpath}) endif() - endforeach() + endforeach() + endif() if(PLATFORM) # set OS, and append specific platform module path diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index 5c0ee1153d..19d2c41da1 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -168,10 +168,11 @@ function(px4_add_module) if(NOT DYNAMIC) target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf) if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL) - target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel) + target_link_libraries(${MODULE} PRIVATE + kernel_events_interface kernel_parameters_interface px4_kernel_layer uORB_kernel) set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE}) else() - target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB) + target_link_libraries(${MODULE} PRIVATE events_interface parameters_interface px4_layer uORB) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) endif() set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index fac011eb7d..8d7f6e6478 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -303,9 +303,12 @@ class MavrosMissionTest(MavrosTestCommon): self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res)) + self.assertTrue(res['roll_error_std'] < 5.0, str(res)) self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) - self.assertTrue(res['yaw_error_std'] < 5.0, str(res)) + + # TODO: fix by excluding initial heading init and reset preflight + self.assertTrue(res['yaw_error_std'] < 10.0, str(res)) if __name__ == '__main__': diff --git a/msg/ActuatorTest.msg b/msg/ActuatorTest.msg index f880720da0..221258f906 100644 --- a/msg/ActuatorTest.msg +++ b/msg/ActuatorTest.msg @@ -18,4 +18,4 @@ float32 value # range: [-1, 1], where 1 means maximum positive output, # and NaN maps to disarmed (stop the motors) uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) -uint8 ORB_QUEUE_LENGTH = 12 # same as MAX_NUM_MOTORS to support code in esc_calibration +uint8 ORB_QUEUE_LENGTH = 16 # >= MAX_NUM_MOTORS to support code in esc_calibration diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 70b09f9cfe..b1d787c721 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -336,3 +336,128 @@ add_custom_command( add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp) target_link_libraries(uorb_msgs PRIVATE m) add_dependencies(uorb_msgs prebuild_targets uorb_headers) + +if(CONFIG_LIB_CDRSTREAM) + set(uorb_cdr_idl) + set(uorb_cdr_msg) + set(uorb_cdr_idl_uorb) + set(idl_include_path ${PX4_BINARY_DIR}/uORB/idl) + set(idl_out_path ${idl_include_path}/px4/msg) + set(idl_uorb_path ${PX4_BINARY_DIR}/msg/px4/msg) + + # Make sure that CycloneDDS has been checkout out + execute_process(COMMAND git submodule sync src/lib/cdrstream/cyclonedds + WORKING_DIRECTORY ${PX4_SOURCE_DIR} ) + execute_process(COMMAND git submodule update --init --force src/lib/cdrstream/cyclonedds + WORKING_DIRECTORY ${PX4_SOURCE_DIR} ) + + # CycloneDDS-tools doesn't ship with the cdrstream-desc feature thus we've to compile idlc from source + MESSAGE(STATUS "Configuring idlc :" ${CMAKE_CURRENT_BINARY_DIR}/idlc) + file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc) + execute_process(COMMAND ${CMAKE_COMMAND} ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds + -DCMAKE_C_COMPILER=/usr/bin/gcc + -DBUILD_EXAMPLES=OFF + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc + RESULT_VARIABLE CMD_ERROR + OUTPUT_FILE CMD_OUTPUT ) + MESSAGE(STATUS "Building idlc :" ${CMAKE_CURRENT_BINARY_DIR}/idlc) + execute_process(COMMAND ${CMAKE_COMMAND} --build . --target idlc + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc + RESULT_VARIABLE CMD_ERROR + OUTPUT_FILE CMD_OUTPUT ) + list(APPEND CMAKE_PROGRAM_PATH "${CMAKE_CURRENT_BINARY_DIR}/idlc/bin") + + # Copy .msg files + foreach(msg_file ${msg_files}) + get_filename_component(msg ${msg_file} NAME_WE) + configure_file(${PX4_SOURCE_DIR}/msg/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) + list(APPEND uorb_cdr_idl ${idl_out_path}/${msg}.idl) + list(APPEND uorb_cdr_msg ${idl_out_path}/${msg}.msg) + list(APPEND uorb_cdr_idl_uorb ${idl_uorb_path}/${msg}.h) + endforeach() + + # Generate IDL from .msg using rosidl_adapter + # Note this a submodule inside PX4 hence no ROS2 installation required + add_custom_command( + OUTPUT ${uorb_cdr_idl} + COMMAND ${CMAKE_COMMAND} + -E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli" + ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py + ${uorb_cdr_msg} + DEPENDS + ${uorb_cdr_msg} + git_cyclonedds + COMMENT "Generating IDL from uORB topic headers" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + + # Generate C definitions from IDL + set(CYCLONEDDS_DIR ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds) + include("${CYCLONEDDS_DIR}/cmake/Modules/Generate.cmake") + idlc_generate(TARGET uorb_cdrstream + FEATURES "cdrstream-desc" + FILES ${uorb_cdr_idl} + INCLUDES ${idl_include_path} + BASE_DIR ${idl_include_path} + WARNINGS no-implicit-extensibility) + target_link_libraries(uorb_cdrstream INTERFACE cdr) + + # Generate and overwrite IDL header with custom headers for uORB operatability + # We typedef the IDL struct the uORB struct so that the IDL offset calculate + # the offset of internal uORB struct for serialization/deserialization + + # In the future we might want to turn this around let the IDL struct be the leading ABI + # However we need to remove the padding for logging and remove the re-ordering of fields + + add_custom_target( + uorb_idl_header + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + --uorb-idl-header + -f ${msg_files} + -i ${CMAKE_CURRENT_SOURCE_DIR} + -o ${idl_uorb_path} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream + DEPENDS + uorb_cdrstream + ${msg_files} + ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream/uorb_idl_header.h.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py + COMMENT "Generating uORB compatible IDL headers" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + add_dependencies(uorb_msgs uorb_idl_header) + + # Compile all CDR compatible message defnitions + target_link_libraries(uorb_msgs PRIVATE uorb_cdrstream ) +endif() + +if(CONFIG_MODULES_ZENOH) + # Update kconfig file for topics + execute_process(COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py + --zenoh-config + -f ${msg_files} + -o ${PX4_SOURCE_DIR}/src/modules/zenoh/ + -e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh + ) + add_custom_command( + OUTPUT + ${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py + --zenoh-pub-sub + -f ${msg_files} + -o ${PX4_BINARY_DIR}/src/modules/zenoh/ + -e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh + DEPENDS + ${msg_files} + ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em + ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py + COMMENT "Generating Zenoh Topic Code" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + add_library(zenoh_topics ${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp) + set_target_properties(zenoh_topics PROPERTIES LINKER_LANGUAGE CXX) +endif() diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 46faddafac..78273d6b06 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -14,7 +14,6 @@ float32 innovation float32 innovation_variance float32 test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused @@ -22,3 +21,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip # TOPICS estimator_aid_src_fake_hgt # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw +# TOPICS estimator_aid_src_terrain_range_finder diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 9fdba8fefa..67074de143 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -14,9 +14,9 @@ float32[2] innovation float32[2] innovation_variance float32[2] test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow +# TOPICS estimator_aid_src_drag diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index 5d143bc04d..deb4c05bd0 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -14,7 +14,6 @@ float32[3] innovation float32[3] innovation_variance float32[3] test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused diff --git a/msg/EstimatorInnovations.msg b/msg/EstimatorInnovations.msg index c8500d9e83..ecf637478d 100644 --- a/msg/EstimatorInnovations.msg +++ b/msg/EstimatorInnovations.msg @@ -19,7 +19,6 @@ float32 baro_vpos # barometer height innovation (m) and innovation variance (m** # Auxiliary velocity float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) -float32 aux_vvel # vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) # Optical flow float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 92aef646ec..4d747d8268 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -41,6 +41,7 @@ bool cs_fake_hgt # 33 - true when fake height measurements are be bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used +bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/Mission.msg b/msg/Mission.msg index c7740d4f7a..546959a3b3 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -4,6 +4,9 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest -int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased -int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased -int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased +int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise +int32 land_index # Index of the land item, -1 otherwise + +uint16 mission_update_counter # indicates updates to the mission, reload from dataman if increased +uint16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased +uint16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased diff --git a/msg/VehicleControlMode.msg b/msg/VehicleControlMode.msg index 1f4034369d..92324cf03f 100644 --- a/msg/VehicleControlMode.msg +++ b/msg/VehicleControlMode.msg @@ -14,3 +14,4 @@ bool flag_control_position_enabled # true if position is controlled bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_allocation_enabled # true if control allocation is enabled diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 4f9238123c..5f17cde407 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -39,6 +39,7 @@ float32 ay # East velocity derivative in NED earth-fixed frame, (metres/s float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only float32 delta_heading uint8 heading_reset_counter bool heading_good_for_control diff --git a/msg/VehicleOpticalFlowVel.msg b/msg/VehicleOpticalFlowVel.msg index c89207f4ab..d92806803a 100644 --- a/msg/VehicleOpticalFlowVel.msg +++ b/msg/VehicleOpticalFlowVel.msg @@ -10,4 +10,8 @@ float32[2] flow_compensated_integral # integrated optical flow measurement com float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) +float32[3] gyro_bias +float32[3] ref_gyro +float32[3] meas_gyro + # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a6e06b9d47..d2e08a405b 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -43,7 +43,6 @@ endif() add_library(px4_platform STATIC board_common.c board_identity.c - events.cpp external_reset_lockout.cpp i2c.cpp i2c_spi_buses.cpp 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 9bee390323..4df96fa5a9 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 @@ -72,7 +72,7 @@ static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; static constexpr wq_config_t INS2{"wq:INS2", 6000, -16}; 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 hp_default{"wq:hp_default", 2392, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 03845239ef..a3d9b02338 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -136,7 +136,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char #if defined(PX4_LOG_COLORIZED_OUTPUT) if (use_color) { - // alway reset color + // always reset color const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1); pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET); diff --git a/platforms/common/px4_work_queue/WorkItemSingleShot.cpp b/platforms/common/px4_work_queue/WorkItemSingleShot.cpp index d31d51ab7b..77d24e2c2a 100644 --- a/platforms/common/px4_work_queue/WorkItemSingleShot.cpp +++ b/platforms/common/px4_work_queue/WorkItemSingleShot.cpp @@ -41,6 +41,7 @@ WorkItemSingleShot::WorkItemSingleShot(const px4::wq_config_t &config, worker_me : px4::WorkItem("", config), _argument(argument), _method(method) { px4_sem_init(&_sem, 0, 0); + px4_sem_setprotocol(&_sem, SEM_PRIO_NONE); } WorkItemSingleShot::WorkItemSingleShot(const px4::WorkItem &work_item, worker_method method, void *argument) diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index c151d051c7..2ab49b3bd5 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -34,6 +34,10 @@ # this includes the generated topics directory include_directories(${CMAKE_CURRENT_BINARY_DIR}) +if(CONFIG_LIB_CDRSTREAM) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/cdrstream/cyclonedds/src/core/ddsc/include) +endif() + set(SRCS) set(SRCS_COMMON diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 9c17cab378..473c554866 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -76,7 +76,7 @@ void Subscription::unsubscribe() bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { + if (uORB::Manager::orb_device_node_exists(_orb_id, instance)) { // if desired new instance exists, unsubscribe from current unsubscribe(); _instance = instance; diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 7cc5c9bd3e..112170de6b 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -188,7 +188,10 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) if (nullptr == _data) { const size_t data_size = _meta->o_size * _queue_size; _data = (uint8_t *) px4_cache_aligned_alloc(data_size); - memset(_data, 0, data_size); + + if (_data) { + memset(_data, 0, data_size); + } } unlock(); diff --git a/platforms/common/work_queue/hrt_thread.c b/platforms/common/work_queue/hrt_thread.c index 8438b0ae2c..8da8b415d1 100644 --- a/platforms/common/work_queue/hrt_thread.c +++ b/platforms/common/work_queue/hrt_thread.c @@ -113,7 +113,7 @@ static void _sighandler(int sig_num) * ****************************************************************************/ -static void hrt_work_process() +static void hrt_work_process(void) { struct wqueue_s *wqueue = &g_hrt_work; volatile struct work_s *work; diff --git a/platforms/common/work_queue/work_thread.c b/platforms/common/work_queue/work_thread.c index b1e0e72355..5109b6ad8a 100644 --- a/platforms/common/work_queue/work_thread.c +++ b/platforms/common/work_queue/work_thread.c @@ -338,7 +338,7 @@ int work_usrthread(int argc, char *argv[]) #endif /* CONFIG_SCHED_USRWORK */ -uint32_t clock_systimer() +uint32_t clock_systimer(void) { //printf("clock_systimer: %0lx\n", hrt_absolute_time()); return (0x00000000ffffffff & hrt_absolute_time()); diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index a489381b49..616f7024a4 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit a489381b49835ecba6f3b873b5071d882a18152f +Subproject commit 616f7024a479bf209eadce133bba5dc8820a7f99 diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 3f77354c0d..c23b72dffe 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 3f77354c0dc88793a47ff3b57595195ab45f7ba9 +Subproject commit c23b72dffeb0de0d1a836ab561eb9169c4a9e58e diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h index 9e1654451c..b85da67e07 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h @@ -53,7 +53,8 @@ #define _CRYPTOIOCBASE IOCTL_IDX_TO_BASE(2) #define _PARAMIOCBASE IOCTL_IDX_TO_BASE(3) #define _PLATFORMIOCBASE IOCTL_IDX_TO_BASE(4) -#define MAX_IOCTL_PTRS 5 +#define _EVENTSIOCBASE IOCTL_IDX_TO_BASE(5) +#define MAX_IOCTL_PTRS 6 /* The PLATFORMIOCLAUNCH IOCTL is used to launch kernel side modules * from the user side code diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 1581ddfc35..d22905cd38 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -125,6 +126,7 @@ int px4_platform_init() #if !defined(CONFIG_BUILD_FLAT) hrt_ioctl_init(); + events_ioctl_init(); #endif /* configure CPU load estimation */ diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index 8f45bd58e3..f4b3476722 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -249,25 +249,19 @@ static const px4_mtd_manifest_t default_mtd_config = { #else -const px4_mft_device_t spifram = { // FM25V02A on FMUM 32K 512 X 64 +const px4_mft_device_t spifram = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; const px4_mtd_entry_t fram = { .device = &spifram, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 7f76502e68..9c57b488b6 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -52,6 +52,8 @@ target_link_libraries(px4_kernel_layer nuttx_kc nuttx_karch nuttx_kmm + PRIVATE + kernel_events_interface # events_ioctl_init ) target_link_libraries(px4_kernel_layer diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5a72cc9f5e..4246ea3585 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -898,11 +898,13 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann case IOTimerChanMode_Dshot: dier_bit = 0; - cr1_bit = state ? GTIM_CR1_CEN : 0; - break; - case IOTimerChanMode_PWMIn: + /* fallthrough */ case IOTimerChanMode_Capture: + cr1_bit = state ? GTIM_CR1_CEN : 0; + + /* fallthrough */ + case IOTimerChanMode_PWMIn: break; default: diff --git a/platforms/posix/include/hrt_work.h b/platforms/posix/include/hrt_work.h index 6dd2ab5a09..7799d3815c 100644 --- a/platforms/posix/include/hrt_work.h +++ b/platforms/posix/include/hrt_work.h @@ -48,13 +48,13 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usd void hrt_work_cancel(struct work_s *work); static inline void hrt_work_lock(void); -static inline void hrt_work_lock() +static inline void hrt_work_lock(void) { px4_sem_wait(&_hrt_work_lock); } static inline void hrt_work_unlock(void); -static inline void hrt_work_unlock() +static inline void hrt_work_unlock(void) { px4_sem_post(&_hrt_work_lock); } diff --git a/src/drivers/adc/ads1115/ADS1115.h b/src/drivers/adc/ads1115/ADS1115.h index 6f3c1e92be..79a31198c9 100644 --- a/src/drivers/adc/ads1115/ADS1115.h +++ b/src/drivers/adc/ads1115/ADS1115.h @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -127,7 +127,7 @@ protected: private: - uORB::Publication _to_adc_report{ORB_ID(adc_report)}; + uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; static const hrt_abstime SAMPLE_INTERVAL{50_ms}; diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 91ab266311..7bf3813542 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -226,7 +226,9 @@ void CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { - camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); + if (camera_capture::g_camera_capture) { + camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); + } } void @@ -359,6 +361,11 @@ CameraCapture::stop() work_cancel(HPWORK, &_work_publisher); + if (_capture_channel >= 0) { + up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr); + } + + if (camera_capture::g_camera_capture != nullptr) { delete (camera_capture::g_camera_capture); } diff --git a/src/drivers/differential_pressure/asp5033/ASP5033.cpp b/src/drivers/differential_pressure/asp5033/ASP5033.cpp new file mode 100644 index 0000000000..9a3a263bbd --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/ASP5033.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * + * Copyright (c) 2023 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 ASP5033.cpp + * + *@author Denislav Petrov + */ + + + +#include "ASP5033.hpp" + +ASP5033::ASP5033(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +ASP5033::~ASP5033() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_fault_perf); +} + +int ASP5033::probe() +{ + int ret = sensor_id_check(); + return ret; +} + +int ASP5033::sensor_id_check() +{ + uint8_t id[1]; + uint8_t cmd_1 = REG_ID_SET_ASP5033; + uint8_t cmd_2 = REG_WHOAMI_RECHECK_ID_ASP5033; + uint8_t cmd_3 = REG_ID_ASP5033; + uint8_t cmd_1_2[2]; + cmd_1_2[0] = static_cast(cmd_1); + cmd_1_2[1] = static_cast(cmd_2); + + + if ((transfer(&cmd_1, 1, &id[0], sizeof(id)) != PX4_OK) || (*id != REG_WHOAMI_DEFAULT_ID_ASP5033)) { return 0; } + + if (transfer(&cmd_1_2[0], 2, nullptr, 0) != PX4_OK) { return 0; } + + if ((transfer(&cmd_3, 1, &id[0], sizeof(id)) != PX4_OK) || (*id != REG_WHOAMI_RECHECK_ID_ASP5033)) { return 0; } + + return 1; + + + +} + +int ASP5033::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + ScheduleNow(); + return ret; +} + + +/** + * @brief calculation of the differential pressure in this way: + * it collect all measured pressure and store it into press_sum, + * count the value of collected times-press_count, then divide both + * and get the actual value of differential pressure - _pressure + * + * @return true if pressure is valid and no errors, false if not + */ +bool ASP5033::get_differential_pressure() +{ + if (hrt_elapsed_time(&last_sample_time) > 200_ms) { + return false; + } + + if (press_count == 0) { + return false; + } + + //calculation differential pressure + _pressure = press_sum / press_count; + + press_sum = 0.; + press_count = 0; + return true; + +} + + +void ASP5033::print_status() +{ + + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_fault_perf); +} + +void ASP5033::RunImpl() +{ + int ret = PX4_ERROR; + + // collection phase + if (_collect_phase) { + // perform collection + ret = collect(); + + if (OK != ret) { + perf_count(_comms_errors); + /* restart the measurement state machine */ + _collect_phase = false; + _sensor_ok = false; + ScheduleNow(); + return; + } + + // next phase is measurement + _collect_phase = false; + + // is there a collect->measure gap? + if (_measure_interval > CONVERSION_INTERVAL) { + + // schedule a fresh cycle call when we are ready to measure again + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + // next phase is collection + _collect_phase = true; + + // schedule a fresh cycle call when the measurement is done + ScheduleDelayed(CONVERSION_INTERVAL); +} + + +int ASP5033::measure() +{ + // Send the command to begin a measurement. + uint8_t cmd_1 = CMD_MEASURE_ASP5033; + uint8_t cmd_2 = REG_CMD_ASP5033;; + + //write to driver to start + uint8_t cmd[2]; + cmd[0] = static_cast(cmd_2); + cmd[1] = static_cast(cmd_1); + int ret = transfer(&cmd[0], 2, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int ASP5033::collect() +{ + perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + + // Read pressure and temperature as one block + uint8_t val[5] {0, 0, 0, 0, 0}; + uint8_t cmd = REG_PRESS_DATA_ASP5033; + transfer(&cmd, 1, &val[0], sizeof(val)); + + //Pressure is a signed 24-bit value + int32_t press = (val[0] << 24) | (val[1] << 16) | (val[2] << 8); + // convert back to 24 bit + press >>= 8; + + // k is a shift based on the pressure range of the device. See + // table in the datasheet + constexpr uint8_t k = 7; + constexpr float press_scale = 1.0f / (1U << k); //= 1.0f / (1U << k); + press_sum += press * press_scale; + press_count++; + + // temperature is 16 bit signed in units of 1/256 C + const int16_t temp = (val[3] << 8) | val[4]; + constexpr float temp_scale = 1.0f / 256; + _temperature = temp * temp_scale; + last_sample_time = hrt_absolute_time(); + bool status = get_differential_pressure(); + + if (status == true && (int)_temperature != 0) { + // publish values + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = _pressure; + differential_pressure.temperature = _temperature ; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = timestamp_sample; + _differential_pressure_pub.publish(differential_pressure); + + } + + perf_end(_sample_perf); + + return PX4_OK; +} + + + + + diff --git a/src/drivers/differential_pressure/asp5033/ASP5033.hpp b/src/drivers/differential_pressure/asp5033/ASP5033.hpp new file mode 100644 index 0000000000..37a9fdeb8f --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/ASP5033.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 ASP5033.hpp + * + * Driver for ASP5033 connected via I2C. + * + * Supported sensors: + * + * - ASP5033 + * + * Interface application notes: + * + * + *@author Denislav Petrov + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include +#include + + + + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + + +/* Configuration Constants */ +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x6D; /* 0x6D 0xE4 */ +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface + + +#define REG_CMD_ASP5033 0x30 +#define REG_PRESS_DATA_ASP5033 0X06 +#define REG_TEMP_DATA_ASP5033 0X09 +#define CMD_MEASURE_ASP5033 0X0A +#define REG_WHOAMI_DEFAULT_ID_ASP5033 0X00 +#define REG_WHOAMI_RECHECK_ID_ASP5033 0X66 +#define REG_ID_ASP5033 0x01 +#define REG_ID_SET_ASP5033 0xa4 + +using namespace time_literals; + + +class ASP5033 : public device::I2C, public I2CSPIDriver +{ +public: + ASP5033(const I2CSPIDriverConfig &config); + ~ASP5033() override; + + static void print_usage(); + void print_status() override; + + + void RunImpl(); + + int init() override; + + + + float press_sum; + uint32_t press_count; + + +private: + + float _pressure = 0.f; + float _temperature = 0.f; + float _pressure_prev = 0.f; + float _temperaute_prev = 0.f; + + int probe() override; + + int measure(); + int collect(); + int sensor_id_check(); + + bool get_differential_pressure(); + hrt_abstime last_sample_time = hrt_absolute_time(); + orb_advert_t _mavlink_log_pub {nullptr}; //log send to + + + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; + perf_counter_t _fault_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": fault detected")}; +}; + diff --git a/src/drivers/differential_pressure/asp5033/CMakeLists.txt b/src/drivers/differential_pressure/asp5033/CMakeLists.txt new file mode 100644 index 0000000000..3ca04b9d67 --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2023 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 drivers__differential_pressure__asp5033 + MAIN asp5033 + COMPILE_FLAGS + SRCS + asp5033_main.cpp + ASP5033.cpp + ASP5033.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/asp5033/Kconfig b/src/drivers/differential_pressure/asp5033/Kconfig new file mode 100644 index 0000000000..6bf415af2e --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033 + bool "asp5033" + default n + ---help--- + Enable support for asp5033 diff --git a/src/drivers/differential_pressure/asp5033/asp5033_main.cpp b/src/drivers/differential_pressure/asp5033/asp5033_main.cpp new file mode 100644 index 0000000000..f34597ef37 --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/asp5033_main.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "ASP5033.hpp" +#include +#include + + + + +void ASP5033::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver to enable an external [ASP5033] +(https://www.qio-tek.com/index.php/product/qiotek-asp5033-dronecan-airspeed-and-compass-module/) +TE connected via I2C. +This is not included by default in firmware. It can be included with terminal command: "make boardconfig" +or in default.px4board with adding the line: "CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y" +It can be enabled with the "SENS_EN_ASP5033" parameter set to 1. +)DESCR_STR"); + PRINT_MODULE_USAGE_NAME("asp5033", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x6D); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int asp5033_main(int argc, char *argv[]) +{ + + using ThisDriver = ASP5033; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ASP5033); + + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/asp5033/parameters.c b/src/drivers/differential_pressure/asp5033/parameters.c new file mode 100644 index 0000000000..5b50f628f6 --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/parameters.c @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + + +/** + * ASP5033 differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_ASP5033, 0); + + + + diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index b66eeeba3d..4419530515 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -33,6 +33,7 @@ /* Include Files */ #include "AFBRS50.hpp" +#include "argus_hal_test.h" #include @@ -42,9 +43,6 @@ /*! Define the SPI baud rate (to be used in the SPI module). */ #define SPI_BAUD_RATE 5000000 -#define LONG_RANGE_MODE_HZ 25 -#define SHORT_RANGE_MODE_HZ 50 - #include "s2pi.h" #include "timer.h" #include "argus_hal_test.h" @@ -52,6 +50,7 @@ AFBRS50 *g_dev{nullptr}; AFBRS50::AFBRS50(uint8_t device_orientation): + ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _px4_rangefinder(0, device_orientation) { @@ -61,6 +60,7 @@ AFBRS50::AFBRS50(uint8_t device_orientation): device_id.devid_s.devtype = DRV_DIST_DEVTYPE_AFBRS50; _px4_rangefinder.set_device_id(device_id.devid); + _px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); } AFBRS50::~AFBRS50() @@ -70,12 +70,12 @@ AFBRS50::~AFBRS50() perf_free(_sample_perf); } -status_t AFBRS50::measurement_ready_callback(status_t status, void *data) +status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd) { if (!up_interrupt_context()) { if (status == STATUS_OK) { if (g_dev) { - g_dev->ProcessMeasurement(data); + g_dev->ProcessMeasurement(hnd); } } else { @@ -86,35 +86,33 @@ status_t AFBRS50::measurement_ready_callback(status_t status, void *data) return status; } -void AFBRS50::ProcessMeasurement(void *data) +void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd) { - if (data != nullptr) { - perf_count(_sample_perf); + perf_count(_sample_perf); - argus_results_t res{}; - status_t evaluate_status = Argus_EvaluateData(_hnd, &res, data); + argus_results_t res{}; + status_t evaluate_status = Argus_EvaluateData(hnd, &res); - if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) { - uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); - float result_m = static_cast(result_mm) / 1000.f; - int8_t quality = res.Bin.SignalQuality; + if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) { + uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); + float result_m = static_cast(result_mm) / 1000.f; + int8_t quality = res.Bin.SignalQuality; - // Signal quality indicates 100% for good signals, 50% and lower for weak signals. - // 1% is an errored signal (not reliable). Signal Quality of 0% is unknown. - if (quality == 1) { - quality = 0; - } - - // distance quality check - if (result_m > _max_distance) { - result_m = 0.0; - quality = 0; - } - - _current_distance = result_m; - _current_quality = quality; - _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); + // Signal quality indicates 100% for good signals, 50% and lower for weak signals. + // 1% is an errored signal (not reliable). Signal Quality of 0% is unknown. + if (quality == 1) { + quality = 0; } + + // distance quality check + if (result_m > _max_distance) { + result_m = 0.0; + quality = 0; + } + + _current_distance = result_m; + _current_quality = quality; + _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); } } @@ -137,7 +135,37 @@ int AFBRS50::init() // Initialize the S2PI hardware required by the API. S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE); - status_t status = Argus_Init(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS); + int32_t mode_param = _p_sens_afbr_mode.get(); + + if (mode_param < 0 || mode_param > 3) { + PX4_ERR("Invalid mode parameter: %li", mode_param); + return PX4_ERROR; + } + + argus_mode_t mode = ARGUS_MODE_LONG_RANGE; + + switch (mode_param) { + case 0: + mode = ARGUS_MODE_SHORT_RANGE; + break; + + case 1: + mode = ARGUS_MODE_LONG_RANGE; + break; + + case 2: + mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE; + break; + + case 3: + mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE; + break; + + default: + break; + } + + status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, mode); if (status == STATUS_OK) { uint32_t id = Argus_GetChipID(_hnd); @@ -148,7 +176,6 @@ int AFBRS50::init() PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c); argus_module_version_t mv = Argus_GetModuleVersion(_hnd); - argus_laser_type_t lt = Argus_GetLaserType(_hnd); switch (mv) { case AFBR_S50MV85G_V1: @@ -168,19 +195,20 @@ int AFBRS50::init() case AFBR_S50LV85D_V1: _min_distance = 0.08f; - - if (lt == LASER_H_V2X) { - _max_distance = 50.f; - PX4_INFO_RAW("AFBR-S50LX85D (v2)\n"); - - } else { - _max_distance = 30.f; - PX4_INFO_RAW("AFBR-S50LV85D (v1)\n"); - } - + _max_distance = 30.f; _px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50LV85D\n"); + break; + + case AFBR_S50LX85D_V1: + _min_distance = 0.08f; + _max_distance = 50.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50LX85D\n"); break; case AFBR_S50MV68B_V1: @@ -223,6 +251,9 @@ int AFBRS50::init() ScheduleDelayed(_measure_interval); return PX4_OK; + + } else { + PX4_ERR("Argus_InitMode failed: %ld", status); } return PX4_ERROR; @@ -230,6 +261,15 @@ int AFBRS50::init() void AFBRS50::Run() { + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // update parameters from storage + ModuleParams::updateParams(); + } + switch (_state) { case STATE::TEST: { if (_testing) { @@ -243,7 +283,8 @@ void AFBRS50::Run() break; case STATE::CONFIGURE: { - status_t status = set_rate(SHORT_RANGE_MODE_HZ); + _current_rate = (uint32_t)_p_sens_afbr_s_rate.get(); + status_t status = set_rate(_current_rate); if (status != STATUS_OK) { PX4_ERR("CONFIGURE status not okay: %i", (int)status); @@ -251,24 +292,18 @@ void AFBRS50::Run() ScheduleNow(); } - status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X); + status = Argus_SetConfigurationDFMMode(_hnd, DFM_MODE_8X); if (status != STATUS_OK) { PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); + _state = STATE::STOP; + ScheduleNow(); } - status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, DFM_MODE_8X); + status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false); if (status != STATUS_OK) { - PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); - } - - // start in short range mode - _mode = ARGUS_MODE_B; - set_mode(_mode); - - if (status != STATUS_OK) { - PX4_ERR("CONFIGURE status not okay: %i", (int)status); + PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status); ScheduleNow(); } else { @@ -288,7 +323,7 @@ void AFBRS50::Run() } } - UpdateMode(); + Evaluate_rate(); } break; @@ -306,49 +341,41 @@ void AFBRS50::Run() ScheduleDelayed(_measure_interval); } -void AFBRS50::UpdateMode() +void AFBRS50::Evaluate_rate() { - // only update mode if _current_distance is a valid measurement - if ((_current_distance > 0) && (_current_quality > 0)) { + // only update mode if _current_distance is a valid measurement and if the last rate switch was more than 1 second ago + if ((_current_distance > 0) && (_current_quality > 0) && ((hrt_absolute_time() - _last_rate_switch) > 1_s)) { - if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) { - // change to long range mode - argus_mode_t mode = ARGUS_MODE_A; - status_t status = set_mode(mode); + status_t status = STATUS_OK; - if (status != STATUS_OK) { - PX4_ERR("set_mode status not okay: %i", (int)status); - } + if ((_current_distance >= (_p_sens_afbr_thresh.get() + _p_sens_afbr_hyster.get())) + && (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) { - status = set_rate(LONG_RANGE_MODE_HZ); + _current_rate = (uint32_t)_p_sens_afbr_l_rate.get(); + status = set_rate(_current_rate); if (status != STATUS_OK) { PX4_ERR("set_rate status not okay: %i", (int)status); + + } else { + PX4_INFO("switched to long range rate: %i", (int)_current_rate); + _last_rate_switch = hrt_absolute_time(); } - status = set_rate(LONG_RANGE_MODE_HZ); + } else if ((_current_distance <= (_p_sens_afbr_thresh.get() - _p_sens_afbr_hyster.get())) + && (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) { + + _current_rate = (uint32_t)_p_sens_afbr_s_rate.get(); + status = set_rate(_current_rate); if (status != STATUS_OK) { PX4_ERR("set_rate status not okay: %i", (int)status); - } - } else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) { - // change to short range mode - argus_mode_t mode = ARGUS_MODE_B; - status_t status = set_mode(mode); - - if (status != STATUS_OK) { - PX4_ERR("set_mode status not okay: %i", (int)status); - } - - status = set_rate(SHORT_RANGE_MODE_HZ); - - if (status != STATUS_OK) { - PX4_ERR("set_rate status not okay: %i", (int)status); + } else { + PX4_INFO("switched to short range rate: %i", (int)_current_rate); + _last_rate_switch = hrt_absolute_time(); } } - - ScheduleDelayed(1000_ms); // don't switch again for at least 1 second } } @@ -373,33 +400,6 @@ void AFBRS50::print_info() get_info(); } -status_t AFBRS50::set_mode(argus_mode_t mode) -{ - while (Argus_GetStatus(_hnd) != STATUS_IDLE) { - px4_usleep(1_ms); - } - - status_t status = Argus_SetConfigurationMeasurementMode(_hnd, mode); - - if (status != STATUS_OK) { - PX4_ERR("Argus_SetConfigurationMeasurementMode status not okay: %i", (int)status); - return status; - } - - argus_mode_t current_mode; - status = Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); - - if (status != STATUS_OK) { - PX4_ERR("Argus_GetConfigurationMeasurementMode status not okay: %i", (int)status); - return status; - - } else { - _mode = current_mode; - } - - return status; -} - status_t AFBRS50::set_rate(uint32_t rate_hz) { while (Argus_GetStatus(_hnd) != STATUS_IDLE) { @@ -429,13 +429,10 @@ status_t AFBRS50::set_rate(uint32_t rate_hz) void AFBRS50::get_info() { - argus_mode_t current_mode; argus_dfm_mode_t dfm_mode; - Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); - Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode); + Argus_GetConfigurationDFMMode(_hnd, &dfm_mode); PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance); - PX4_INFO_RAW("mode: %d\n", current_mode); PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval)); } diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index f7503b321b..2ad767b2fa 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -44,13 +44,16 @@ #include #include #include -#include #include +#include +#include #include +#include +#include using namespace time_literals; -class AFBRS50 : public px4::ScheduledWorkItem +class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem { public: AFBRS50(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); @@ -75,18 +78,16 @@ public: private: void Run() override; - void UpdateMode(); + void Evaluate_rate(); - void ProcessMeasurement(void *data); + void ProcessMeasurement(argus_hnd_t *hnd); - static status_t measurement_ready_callback(status_t status, void *data); + static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd); void get_info(); - status_t set_mode(argus_mode_t mode); status_t set_rate(uint32_t rate_hz); argus_hnd_t *_hnd{nullptr}; - argus_mode_t _mode{ARGUS_MODE_B}; // Short-Range enum class STATE : uint8_t { TEST, @@ -98,14 +99,24 @@ private: PX4Rangefinder _px4_rangefinder; hrt_abstime _measurement_time{0}; + hrt_abstime _last_rate_switch{0}; perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; uint32_t _measure_interval{1000000 / 50}; // 50Hz float _current_distance{0}; int8_t _current_quality{0}; - const float _short_range_threshold = 4.0; //meters - const float _long_range_threshold = 6.0; //meters float _max_distance; float _min_distance; + uint32_t _current_rate{0}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + DEFINE_PARAMETERS( + (ParamInt) _p_sens_afbr_mode, + (ParamInt) _p_sens_afbr_s_rate, + (ParamInt) _p_sens_afbr_l_rate, + (ParamInt) _p_sens_afbr_thresh, + (ParamInt) _p_sens_afbr_hyster + ); }; diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c index 61950a12b8..015249f859 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c @@ -85,6 +85,8 @@ static int gpio_falling_edge(int irq, void *context, void *arg) status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps) { + (void)defaultSlave; + px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_CS); s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS); @@ -107,11 +109,24 @@ status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps) * - #STATUS_BUSY: An SPI transfer is in progress. * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. *****************************************************************************/ -status_t S2PI_GetStatus(void) +status_t S2PI_GetStatus(s2pi_slave_t slave) { + (void)slave; + return s2pi_.Status; } +status_t S2PI_TryGetMutex(s2pi_slave_t slave) +{ + (void) slave; + return STATUS_OK; +} + +void S2PI_ReleaseMutex(s2pi_slave_t slave) +{ + (void) slave; +} + /*!*************************************************************************** * @brief Sets the SPI baud rate in bps. * @param baudRate_Bps The default SPI baud rate in bauds-per-second. @@ -135,8 +150,10 @@ status_t S2PI_SetBaudRate(uint32_t baudRate_Bps) * switch back to ordinary SPI functionality. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_CaptureGpioControl(void) +status_t S2PI_CaptureGpioControl(s2pi_slave_t slave) { + (void)slave; + /* Check if something is ongoing. */ IRQ_LOCK(); status_t status = s2pi_.Status; @@ -165,8 +182,10 @@ status_t S2PI_CaptureGpioControl(void) * the #S2PI_CaptureGpioControl function. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_ReleaseGpioControl(void) +status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave) { + (void)slave; + /* Check if something is ongoing. */ IRQ_LOCK(); status_t status = s2pi_.Status; @@ -202,6 +221,8 @@ status_t S2PI_ReleaseGpioControl(void) *****************************************************************************/ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value) { + (void)slave; + /* Check if pin is valid. */ if (pin > S2PI_IRQ || value > 1) { return ERROR_INVALID_ARGUMENT; @@ -228,6 +249,8 @@ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value) *****************************************************************************/ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value) { + (void)slave; + /* Check if pin is valid. */ if (pin > S2PI_IRQ || !value) { return ERROR_INVALID_ARGUMENT; @@ -255,6 +278,8 @@ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value) *****************************************************************************/ status_t S2PI_CycleCsPin(s2pi_slave_t slave) { + (void)slave; + /* Check the driver status. */ IRQ_LOCK(); status_t status = s2pi_.Status; @@ -372,8 +397,10 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8 * invoked with the #ERROR_ABORTED error byte. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_Abort(void) +status_t S2PI_Abort(s2pi_slave_t slave) { + (void)slave; + status_t status = s2pi_.Status; /* Check if something is ongoing. */ @@ -405,6 +432,8 @@ status_t S2PI_Abort(void) *****************************************************************************/ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData) { + (void)slave; + s2pi_.IrqCallback = callback; s2pi_.IrqCallbackData = callbackData; @@ -430,5 +459,7 @@ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, v *****************************************************************************/ uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave) { + (void)slave; + return px4_arch_gpioread(s2pi_.GPIOs[S2PI_IRQ]); } diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h index a933976370..44cdc92168 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides generic functionality belonging to all - * devices from the AFBR-S50 product family. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides generic functionality belonging to all + * devices from the AFBR-S50 product family. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,21 +37,21 @@ #ifndef ARGUS_API_H #define ARGUS_API_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argusapi AFBR-S50 API + * @defgroup argus_api AFBR-S50 API + * @ingroup argus * - * @brief The main module of the API from the AFBR-S50 SDK. + * @brief The main module of the API from the AFBR-S50 SDK. * - * @details General API for the AFBR-S50 time-of-flight sensor device family.\n - * See the \ref getting_started Guide for a detailed description - * on how to use the module/API. + * @details General API for the AFBR-S50 time-of-flight sensor device family.\n + * See the \ref getting_started Guide for a detailed description + * on how to use the module/API. * - * @addtogroup argusapi + * @addtogroup argus_api * @{ *****************************************************************************/ @@ -61,213 +61,301 @@ extern "C" { #include "argus_dfm.h" #include "argus_snm.h" #include "argus_xtalk.h" - -/*! The data structure for the API representing a AFBR-S50 device instance. */ -typedef void argus_hnd_t; +#include "argus_offset.h" /*! The S2PI slave identifier. */ typedef int32_t s2pi_slave_t; /*!*************************************************************************** - * @brief Initializes the API modules and the device with default parameters. + * @brief Initializes the device with default measurement mode. * * @details The function that needs to be called once after power up to - * initialize the modules state (i.e. the corresponding handle) and the - * dedicated Time-of-Flight device. In order to obtain a handle, - * reference the #Argus_CreateHandle method. + * initialize the modules state (i.e. the corresponding handle) and the + * dedicated Time-of-Flight device. In order to obtain a handle, + * reference the #Argus_CreateHandle method. * - * Prior to calling the function, the required peripherals (i.e. S2PI, - * GPIO w/ IRQ and Timers) must be initialized and ready to use. + * Prior to calling the function, the required peripherals (i.e. S2PI, + * GPIO w/ IRQ and Timers) must be initialized and ready to use. * - * The function executes the following tasks: - * - Initialization of the internal state represented by the handle - * object. - * - Setup the device such that an safe configuration is present in - * the registers. - * - Initialize sub modules such as calibration or measurement modules. - * . + * The function executes the following tasks: + * - Initialization of the internal state represented by the handle + * object. + * - Setup the device such that an safe configuration is present in + * the registers. + * - Initialize sub modules such as calibration or measurement modules. + * . * - * The modules configuration is initialized with reasonable default values. + * The modules configuration is initialized with reasonable default + * values. Note that the default measurement mode depends on the + * given device. * - * @param hnd The API handle; contains all internal states and data. + * Also refer to #Argus_InitMode, which uses an specified measurement + * mode instead of the dedicated default measurement mode. * - * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ - * lines. This is actually just a number that is passed - * to the SPI interface to distinct for multiple SPI slave - * devices. Note that the slave must be not equal to 0, - * since is reserved for error handling. + * @param hnd The API handle; contains all internal states and data. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Init(argus_hnd_t *hnd, s2pi_slave_t spi_slave); /*!*************************************************************************** - * @brief Reinitializes the API modules and the device with default parameters. + * @brief Initializes the device with specified measurement mode. * - * @details The function reinitializes the device with default configuration. - * Can be used as reset sequence for the device. See #Argus_Init for - * more information on the initialization. + * @details The function that needs to be called once after power up to + * initialize the modules state (i.e. the corresponding handle) and the + * dedicated Time-of-Flight device. In order to obtain a handle, + * reference the #Argus_CreateHandle method. * - * Note that the #Argus_Init function must be called first! Otherwise, - * the function will return an error if it is called for an yet - * uninitialized device/handle. + * Prior to calling the function, the required peripherals (i.e. S2PI, + * GPIO w/ IRQ and Timers) must be initialized and ready to use. * - * @param hnd The API handle; contains all internal states and data. + * The function executes the following tasks: + * - Initialization of the internal state represented by the handle + * object. + * - Setup the device such that an safe configuration is present in + * the registers. + * - Initialize sub modules such as calibration or measurement modules. + * . * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * The modules configuration is initialized with reasonable default values. + * + * Also refer to #Argus_Init, which uses the dedicated default measurement + * mode instead of an user specified measurement mode. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @param mode The specified measurement mode to be initialized. + * Pass 0 as special value to select default measurement mode + * (see #Argus_Init). + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_InitMode(argus_hnd_t *hnd, s2pi_slave_t spi_slave, argus_mode_t mode); + +/*!*************************************************************************** + * @brief Reinitializes the device with the current measurement mode. + * + * @details The function reinitializes the device with the currently active + * measurement mode. + * + * This can be used as a soft reset for the device and API. + * See #Argus_Init for more information on the initialization. + * + * Note that the #Argus_Init or #Argus_InitMode function must be called + * first! Otherwise, the function will return an error if it is called + * for an yet uninitialized device/handle. + * + * Also refer to #Argus_ReinitMode, which uses a specified measurement + * mode instead of the currently active measurement mode. + * + * @param hnd The API handle; contains all internal states and data. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Reinit(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Deinitializes the API modules and the device. + * @brief Reinitializes the device with a specified measurement mode. + * + * @details The function reinitializes the device with a specified (/p mode) + * measurement mode. + * + * This can be used as a soft reset for the device and API. + * See #Argus_InitMode for more information on the initialization. + * + * Note that the #Argus_Init or #Argus_InitMode function must be called + * first! Otherwise, the function will return an error if it is called + * for an yet uninitialized device/handle. + * + * Also refer to #Argus_Reinit, which re-uses the currently active + * measurement mode instead of an user specified measurement mode. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param mode The specified measurement mode to be initialized. + * Pass 0 as special value to select the current measurement mode + * (see #Argus_Init). + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ReinitMode(argus_hnd_t *hnd, argus_mode_t mode); + +/*!*************************************************************************** + * @brief Deinitializes the API modules and the device. * * @details The function deinitializes the device and clear all internal states. - * Can be used to cleanup before releasing the memory. The device - * can not be used any more and must be initialized again prior to next - * usage. + * Can be used to cleanup before releasing the memory. The device + * can not be used any more and must be initialized again prior to next + * usage. * - * Note that the #Argus_Init function must be called first! Otherwise, - * the function will return an error if it is called for an yet - * uninitialized device/handle. + * Note that the #Argus_Init function must be called first! Otherwise, + * the function will return an error if it is called for an yet + * uninitialized device/handle. * - * @param hnd The API handle; contains all internal states and data. + * @param hnd The API handle; contains all internal states and data. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Deinit(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Creates a new device data handle object to store all internal states. + * @brief Creates a new device data handle object to store all internal states. * * @details The function must be called to obtain a new device handle object. - * The handle is basically an abstract object in memory that contains - * all the internal states and settings of the API module. The handle - * is passed to all the API methods in order to address the specified - * device. This allows to use the API with more than a single measurement - * device. + * The handle is basically an abstract object in memory that contains + * all the internal states and settings of the API module. The handle + * is passed to all the API methods in order to address the specified + * device. This allows to use the API with more than a single measurement + * device. * - * The handler is created by calling the memory allocation method from - * the standard library: @code void * malloc(size_t size) @endcode - * In order to implement an individual memory allocation method, - * define and implement the following weakly binded method and return - * a pointer to the newly allocated memory. * + * The handler is created by calling the memory allocation method from + * the standard library: @code void * malloc(size_t size) @endcode + * In order to implement an individual memory allocation method, + * define and implement the following weakly binded method and return + * a pointer to the newly allocated memory. * * - * @code void * Argus_Malloc (size_t size) @endcode + * @code void * Argus_Malloc (size_t size) @endcode * - * Also see the #Argus_DestroyHandle method for the corresponding - * deallocation of the allocated memory. + * Also see the #Argus_DestroyHandle method for the corresponding + * deallocation of the allocated memory. * - * @note Although the method is using memory allocated on the heap, it - * is eventually no dynamic memory allocation, since the block of - * memory is kept all the time and no memory blocks are dynamically - * freed and re-allocated. If the usage of heap must be avoided, one - * can always implement its own version of the `Argus_Malloc` function - * to create the memory elsewhere. + * @note Although the method is using memory allocated on the heap, it + * is eventually no dynamic memory allocation, since the block of + * memory is kept all the time and no memory blocks are dynamically + * freed and re-allocated. If the usage of heap must be avoided, one + * can always implement its own version of the `Argus_Malloc` function + * to create the memory elsewhere. * - * @return Returns a pointer to the newly allocated device handler object. - * Returns a null pointer if the allocation failed! + * @return Returns a pointer to the newly allocated device handler object. + * Returns a null pointer if the allocation failed! *****************************************************************************/ argus_hnd_t *Argus_CreateHandle(void); /*!*************************************************************************** - * @brief Destroys a given device data handle object. + * @brief Destroys a given device data handle object. * * @details The function can be called to free the previously created device - * data handle object in order to save memory when the device is not - * used any more. + * data handle object in order to save memory when the device is not + * used any more. * - * Please refer to the #Argus_CreateHandle method for the corresponding - * allocation of the memory. + * Note that the handle must be deinitialized before it can be + * destroyed. The function returns #ERROR_FAIL if the handle is not + * yet deinitialized. * - * The handler is destroyed by freeing the corresponding memory with the - * method from the standard library, @code void free(void * ptr) @endcode. - * In order to implement an individual memory deallocation method, define - * and implement the following weakly binded method and free the memory - * object passed to the method by a pointer. + * Please refer to the #Argus_CreateHandle method for the corresponding + * allocation of the memory. * - * @code void Argus_Free (void * ptr) @endcode + * The handler is destroyed by freeing the corresponding memory with the + * method from the standard library, @code void free(void * ptr) @endcode. + * In order to implement an individual memory deallocation method, define + * and implement the following weakly binded method and free the memory + * object passed to the method by a pointer. * - * Also see the #Argus_CreateHandle method for the corresponding - * allocation of the required memory. + * @code void Argus_Free (void * ptr) @endcode * - * @param hnd The device handle object to be deallocated. + * Also see the #Argus_CreateHandle method for the corresponding + * allocation of the required memory. + * + * @param hnd The device handle object to be deallocated. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -void Argus_DestroyHandle(argus_hnd_t *hnd); +status_t Argus_DestroyHandle(argus_hnd_t *hnd); /*!************************************************************************** * Generic API ****************************************************************************/ /*!*************************************************************************** - * @brief Gets the version number of the current API library. + * @brief Gets the version number of the current API library. * - * @details The version is compiled of a major (a), minor (b) and bugfix (c) - * number: a.b.c. + * @details The version is compiled of a major (a), minor (b) and bugfix (c) + * number: a.b.c. * - * The values are encoded into a 32-bit value: + * The values are encoded into a 32-bit value: * - * - [ 31 .. 24 ] - Major Version Number - * - [ 23 .. 16 ] - Minor Version Number - * - [ 15 .. 0 ] - Bugfix Version Number - * . + * - [ 31 .. 24 ] - Major Version Number + * - [ 23 .. 16 ] - Minor Version Number + * - [ 15 .. 0 ] - Bugfix Version Number + * . * - * To obtain the parts from the returned uin32_t value: + * To obtain the parts from the returned uin32_t value: * - * @code - * uint32_t value = Argus_GetAPIVersion(); - * uint8_t a = (value >> 24) & 0xFFU; - * uint8_t b = (value >> 16) & 0xFFU; - * uint8_t c = value & 0xFFFFU; - * @endcode + * @code + * uint32_t value = Argus_GetAPIVersion(); + * uint8_t a = (value >> 24) & 0xFFU; + * uint8_t b = (value >> 16) & 0xFFU; + * uint8_t c = value & 0xFFFFU; + * @endcode * - * @return Returns the current version number. + * @return Returns the current version number. *****************************************************************************/ uint32_t Argus_GetAPIVersion(void); /*!*************************************************************************** - * @brief Gets the build number of the current API library. + * @brief Gets the build number of the current API library. * - * @return Returns the current build number as a C-string. + * @return Returns the current build number as a C-string. *****************************************************************************/ char const *Argus_GetBuildNumber(void); /*!*************************************************************************** - * @brief Gets the version/variant of the module. + * @brief Gets the version/variant of the module. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the current module number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current module number. *****************************************************************************/ argus_module_version_t Argus_GetModuleVersion(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the version number of the chip. + * @brief Gets the name string of the module. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the current version number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current module name. + *****************************************************************************/ +char const *Argus_GetModuleName(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the version number of the chip. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current version number. *****************************************************************************/ argus_chip_version_t Argus_GetChipVersion(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the type number of the device laser. + * @brief Gets the type number of the device laser. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the current device laser type number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current device laser type number. *****************************************************************************/ argus_laser_type_t Argus_GetLaserType(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the unique identification number of the chip. + * @brief Gets the unique identification number of the chip. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the unique identification number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the unique identification number. *****************************************************************************/ uint32_t Argus_GetChipID(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the SPI hardware slave identifier. + * @brief Gets the SPI hardware slave identifier. * - * @param hnd The API handle; contains all internal states and data. - * @return The SPI hardware slave identifier. + * @param hnd The API handle; contains all internal states and data. + * @return The SPI hardware slave identifier. *****************************************************************************/ s2pi_slave_t Argus_GetSPISlave(argus_hnd_t *hnd); @@ -276,323 +364,425 @@ s2pi_slave_t Argus_GetSPISlave(argus_hnd_t *hnd); /*!************************************************************************** * Measurement/Device Operation **************************************************************************** - * @addtogroup argusmeas + * @addtogroup argus_meas * @{ ****************************************************************************/ /*!*************************************************************************** - * @brief Starts the timer based measurement cycle asynchronously. + * @brief Starts the timer based measurement cycle asynchronously. * * @details This function starts a timer based measurement cycle asynchronously - * in the background. A periodic timer interrupt triggers the measurement - * frames on the ASIC and the data readout afterwards. + * in the background. A periodic timer interrupt triggers the measurement + * frames on the ASIC and the data readout afterwards. * - * When the measurement has finished, a callback (which is passed as - * a parameter to the function) is invoked in order to inform the - * main thread to call the \link #Argus_EvaluateData data evaluation - * method\endlink. This call is mandatory to release the data buffer - * for the next measurement and it must not be invoked directly from - * the callback since it is currently within an interrupt service - * routine. Rather a flag should inform the main thread or task - * scheduler to invoke the evaluation as soon as possible in order - * to not introduce any unwanted delays to the next measurement frame. + * When the measurement has finished, a callback (which is passed as + * a parameter to the function) is invoked in order to inform the + * main thread to call the \link #Argus_EvaluateData data evaluation + * method\endlink. This call is mandatory to release the data buffer + * for the next measurement and it must not be invoked directly from + * the callback since it is currently within an interrupt service + * routine. Rather a flag should inform the main thread or task + * scheduler to invoke the evaluation as soon as possible in order + * to not introduce any unwanted delays to the next measurement frame. * - * The next measurement frame will be started as soon as the pre- - * conditions are meet. These are: - * 1. timer flag set (i.e. a certain time has passed since the last - * measurement in order to fulfill eye-safety), - * 2. device idle (i.e. no measurement currently ongoing) and - * 3. data buffer ready (i.e. the previous data has been evaluated). + * The next measurement frame will be started as soon as the pre- + * conditions are meet. These are: + * 1. timer flag set (i.e. a certain time has passed since the last + * measurement in order to fulfill eye-safety), + * 2. device idle (i.e. no measurement currently ongoing) and + * 3. data buffer ready (i.e. the previous data has been evaluated). * - * Usually, the device idle and data buffer ready conditions are met - * before the timer tick occurs and thus the timer dictates the frame - * rate. + * Usually, the device idle and data buffer ready conditions are met + * before the timer tick occurs and thus the timer dictates the frame + * rate. * - * The callback function pointer will be invoked when the measurement - * frame has finished successfully or whenever an error, that cannot - * be handled internally, occurs. + * The callback function pointer will be invoked when the measurement + * frame has finished successfully or whenever an error, that cannot + * be handled internally, occurs. * - * The periodic timer interrupts are used to check the measurement status - * for timeouts. An error is invoked when a measurement cycle have not - * finished within the specified time. + * The periodic timer interrupts are used to check the measurement status + * for timeouts. An error is invoked when a measurement cycle have not + * finished within the specified time. * - * Use #Argus_StopMeasurementTimer to stop the measurements. + * Use #Argus_StopMeasurementTimer to stop the measurements. * - * @note In order to use this function, the periodic interrupt timer module - * (see @ref argus_timer) must be implemented! + * @note In order to use this function, the periodic interrupt timer module + * (see @ref argus_timer) must be implemented! * - * @param hnd The API handle; contains all internal states and data. - * @param cb Callback function that will be invoked when the measurement - * is completed. Its parameters are the \link #status_t status - * \endlink and a pointer to the \link #argus_results_t results - * \endlink structure. If an error occurred, the status differs - * from #STATUS_OK and the second parameter is null. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * + * @param cb Callback function that will be invoked when the measurement + * is completed. Its parameters are the \link #status_t status + * \endlink of the finished measurement cycle and the pointer to + * the calling \link #argus_hnd_t API handle\endlink, i.e. the + * /p hnd value. The latter must be passed to the + * #Argus_EvaluateData function. + * If an error occurred, the status differs from #STATUS_OK. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_StartMeasurementTimer(argus_hnd_t *hnd, argus_callback_t cb); +status_t Argus_StartMeasurementTimer(argus_hnd_t *hnd, + argus_measurement_ready_callback_t cb); /*!*************************************************************************** - * @brief Stops the timer based measurement cycle. + * @brief Stops the timer based measurement cycle. * * @details This function stops the ongoing timer based measurement cycles - * that have been started using the #Argus_StartMeasurementTimer - * function. + * that have been started using the #Argus_StartMeasurementTimer + * function. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_StopMeasurementTimer(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Triggers a single measurement frame asynchronously. + * @brief Triggers a single measurement frame asynchronously. * * @details This function immediately triggers a single measurement frame - * asynchronously if all the pre-conditions are met. Otherwise it - * returns with a corresponding status (e.g. #STATUS_BUSY or - * #STATUS_ARGUS_POWERLIMIT). + * asynchronously if all the pre-conditions are met. Otherwise it + * returns with a corresponding status (e.g. #STATUS_BUSY or + * #STATUS_ARGUS_POWERLIMIT). * - * When the measurement has finished, a callback (which is passed as - * a parameter to the function) is invoked in order to inform the - * main thread to call the \link #Argus_EvaluateData data evaluation - * method\endlink. This call is mandatory to release the data buffer - * for the next measurement and it must not be invoked directly from - * the callback since it is currently within an interrupt service - * routine. Rather a flag should inform the main thread or task - * scheduler to invoke the evaluation task. + * When the measurement has finished, a callback (which is passed as + * a parameter to the function) is invoked in order to inform the + * main thread to call the \link #Argus_EvaluateData data evaluation + * method\endlink. This call is mandatory to release the data buffer + * for the next measurement and it must not be invoked directly from + * the callback since it is currently within an interrupt service + * routine. Rather a flag should inform the main thread or task + * scheduler to invoke the evaluation task. * - * The pre-conditions for starting a measurement frame are: - * 1. timer flag set (i.e. a certain time has passed since the last - * measurement in order to fulfill eye-safety), - * 2. device idle (i.e. no measurement currently ongoing) and - * 3. data buffer ready (i.e. the previous data has been evaluated). + * The pre-conditions for starting a measurement frame are: + * 1. timer flag set (i.e. a certain time has passed since the last + * measurement in order to fulfill eye-safety), + * 2. device idle (i.e. no measurement currently ongoing) and + * 3. data buffer ready (i.e. the previous data has been evaluated). * - * The callback function pointer will be invoked when the measurement - * frame has finished successfully or whenever an error, that cannot - * be handled internally, occurs. + * The callback function pointer will be invoked when the measurement + * frame has finished successfully or whenever an error, that cannot + * be handled internally, occurs. * - * The successful finishing of the measurement frame is not checked - * for timeouts! Instead, the user can call the #Argus_GetStatus() - * function on a regular function to do so. + * The successful finishing of the measurement frame is not checked + * for timeouts! Instead, the user can call the #Argus_GetStatus() + * function on a regular function to do so. * - * @param hnd The API handle; contains all internal states and data. - * @param cb Callback function that will be invoked when the measurement - * is completed. Its parameters are the \link #status_t status - * \endlink and a pointer to the \link #argus_results_t results - * \endlink structure. If an error occurred, the status differs - * from #STATUS_OK and the second parameter is null. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @note Despite this function triggers a new measurement cycle upon its + * invocation, the frame time parameter is still active for this + * measurement mode. Basically, the first pre-condition mentioned + * above is controlled via the frame time parameter. This means + * that measurements cannot be triggered faster than the frame + * timer parameters specifies. The maximum integration time (i.e. + * exposure time) is also determined by the frame time such that + * new measurements are finished with the specified frame time and + * the device is ready to trigger a new measurement after the + * frame time has elapse. + * See #Argus_SetConfigurationFrameTime function for more information + * on the frame time. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param cb Callback function that will be invoked when the measurement + * is completed. Its parameters are the \link #status_t status + * \endlink of the finished measurement cycle and the pointer to + * the calling \link #argus_hnd_t API handle\endlink, i.e. the + * /p hnd value. The latter must be passed to the + * #Argus_EvaluateData function. + * If an error occurred, the status differs from #STATUS_OK. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_TriggerMeasurement(argus_hnd_t *hnd, argus_callback_t cb); +status_t Argus_TriggerMeasurement(argus_hnd_t *hnd, + argus_measurement_ready_callback_t cb); /*!*************************************************************************** - * @brief Stops the currently ongoing measurements and SPI activity immediately. + * @brief Determines whether a data evaluation is pending. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details If the function returns true, a raw buffer is required to be + * evaluated to the #Argus_EvaluateData function. The raw data buffer + * is filled with raw data from the measurement tasks which need to + * be evaluated and the buffer must be freed in order to restart a + * new measurement task. + * + * Note that no configuration parameters can be update until all raw + * buffers are evaluated. + * + * @note See also the #Argus_GetStatus function to obtain the current device + * status and error code if any. + * + * @param hnd The API handle; contains all internal states and data. + * @return True if any raw buffer is filled with data that must be evaluated. + *****************************************************************************/ +bool Argus_IsDataEvaluationPending(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Determines if the device if active with timer based measurements. + * @details If the function returns true, the device is active with timer + * scheduled measurements that have been started via the + * #Argus_StartMeasurementTimer. + * + * Note that the active state is independent of the busy state that + * is set when the device is actually busy. The active state is also + * true if the device is currently idle but waits for the next timer + * event to trigger a new measurement cycle. + * + * @note See also the #Argus_GetStatus function to obtain the current device + * status and error code if any. + * + * @param hnd The API handle; contains all internal states and data. + * @return True if the device is operating in timer triggered measurement mode. + *****************************************************************************/ +bool Argus_IsTimerMeasurementActive(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Stops the currently ongoing measurements and SPI activity immediately. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Abort(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Checks the state of the device/driver. + * @brief Checks the state of the device/driver. * - * @details Returns the current module status or error if any. - * See the following for a list of errors: + * @details Returns the current module status or error if any. * - * Status: - * - Idle/OK: Device and SPI interface are idle (== #STATUS_IDLE). - * - Busy: Device or SPI interface are busy (== #STATUS_BUSY). - * - Initializing: The modules and devices are currently initializing - * (== #STATUS_INITIALIZING). - * . + * See the following for a list of errors: * - * Error: - * - Not Initialized: The modules (or any submodule) has not been - * initialized yet (== #ERROR_NOT_INITIALIZED). - * - Not Connected: No device has been connected (or connection errors - * have occurred) (== #ERROR_ARGUS_NOT_CONNECTED). - * - Timeout: A previous frame measurement has not finished within a - * specified time (== #ERROR_TIMEOUT). - * . + * Status: + * - Idle/OK: Device and SPI interface are idle (== #STATUS_IDLE). + * - Busy: Device or SPI interface are busy (== #STATUS_BUSY). + * - Initializing: The modules and devices are currently initializing + * (== #STATUS_INITIALIZING). + * . * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * Error: + * - Not Initialized: The modules (or any submodule) has not been + * initialized yet (== #ERROR_NOT_INITIALIZED). + * - Not Connected: No device has been connected (or connection errors + * have occurred) (== #ERROR_ARGUS_NOT_CONNECTED). + * - Timeout: A previous frame measurement has not finished within a + * specified time (== #ERROR_TIMEOUT). + * . + * + * @note Note that this function returns the actual busy state. This means + * that it will return #STATUS_IDLE during the pause between two + * consecutive measurement frames. If the device is active with timer + * based measurements (i.e. started via the #Argus_StartMeasurementTimer + * function), the return state switches from idle to busy and back + * periodically. Use the #Argus_IsTimerMeasurementActive function in + * order to determine if the device is active with timer based + * measurements. + * + * @note Note also that the device might reject configuration parameter + * update despite the status is #STATUS_IDLE. This is due to the fact + * that the internal raw data buffers are still busy and require to + * be freed by passing them to the #Argus_EvaluateData function. Use + * the #Argus_IsDataEvaluationPending function to see whether any of + * the raw data buffers is busy or the configuration can be changed. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetStatus(argus_hnd_t *hnd); /*!***************************************************************************** - * @brief Tests the connection to the device by sending a ping message. + * @brief Tests the connection to the device by sending a ping message. * - * @details A ping is transfered to the device in order to check the device and - * SPI connection status. Returns #STATUS_OK on success and - * #ERROR_ARGUS_NOT_CONNECTED elsewise. + * @details A ping is transferred to the device in order to check the device and + * SPI connection status. Returns #STATUS_OK on success and + * #ERROR_ARGUS_NOT_CONNECTED else-wise. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). ******************************************************************************/ status_t Argus_Ping(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Evaluate useful information from the raw measurement data. + * @brief Evaluates measurement data from the raw sensor readout data. * - * @details This function is called with a pointer to the raw results obtained - * from the measurement cycle. It evaluates this data and creates - * useful information from it. Furthermore, calibration is applied to - * the data. Finally, the results are used in order to adapt the device - * configuration to the ambient conditions in order to achieve optimal - * device performance. + * @details This function must be called after each completion of a measurement + * cycle. The completion of a measurement cycle is communicated by the + * API via the invocation of the measurement data ready callback. The + * callback is installed in the API when new measurements are started + * either via the #Argus_TriggerMeasurement or via the + * #Argus_StartMeasurementTimer functions. * - * Therefore, it consists of the following sub-functions: - * - Apply pre-calibration: Applies calibration steps before evaluating - * the data, i.e. calculations that are to the integration results - * directly. - * - Evaluate data: Calculates measurement parameters such as range, - * amplitude or ambient light intensity, depending on the configurations. - * - Apply post-calibration: Applies calibrations after evaluation of - * measurement data, i.e. calibrations applied to the calculated - * values such as range. - * - Dynamic Configuration Adaption: checks if the configuration needs - * to be adjusted before the next measurement cycle in order to - * achieve optimum performance. Note that the configuration might not - * applied directly but before the next measurement starts. This is - * due to the fact that the device could be busy measuring already - * the next frame and thus no SPI activity is allowed. - * . - * However, if the device is idle, the configuration will be written - * immediately. + * This function evaluates measurement values like distances, amplitudes + * states and auxiliary values like temperature or voltage values from + * the raw sensor readout data obtained from the device during the + * measurement cycle. A pointer to a #argus_results_t data structure + * must be passed where all the evaluated values will be written to. + * The structure must persist during the whole execution of the + * #Argus_EvaluateData function. * - * @param hnd The API handle; contains all internal states and data. - * @param res A pointer to the results structure that will be populated - * with evaluated data. - * @param raw The pointer to the raw data that has been obtained by the - * measurement finished callback. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * In addition to the evaluation of measurement data, the function + * feeds back the obtained information to the device in order to + * optimize its performance with respect to the ambient conditions, + * utilizing the so called Dynamic Configuration Adaption (DCA) + * feature. + * + * Furthermore, several calibration algorithm are applied to the data. + * + * If the function is called without any data ready to be evaluated + * from the measurement module, the error code #ERROR_ARGUS_BUFFER_EMPTY + * is returned and not data is written to the passed #argus_results_t + * data structure. + * + * @note The call to this function is mandatory for each finished measurement + * cycle, i.e. for each call to the measurement data ready callback. + * If the function is not called, the data is not evaluated and the + * internal raw data buffers are not freed. In that case, they can not + * be reused for the next measurement and the API can not start new + * measurements. + * There are up to two internal buffers available, the to callback + * is called twice before the API must wait for the data evaluation + * to finish. + * + * @param hnd The API handle; contains all internal states and data. + * @param res A pointer to the results structure that will be populated + * with evaluated data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_EvaluateData(argus_hnd_t *hnd, argus_results_t *res, void *raw); +status_t Argus_EvaluateData(argus_hnd_t *hnd, argus_results_t *res); /*!*************************************************************************** - * @brief Executes a crosstalk calibration measurement. + * @brief Evaluates measurement data from the raw sensor readout data. + * + * @details This function enhances the #Argus_EvaluateData by adding additional + * debug data into a specified debug data structure (\p dbg). If the + * \p dbg is null, the function is eqivalent to the #Argus_EvaluateData + * function. This, see #Argus_EvaluateData for reference. + * + * @param hnd The API handle; contains all internal states and data. + * @param res A pointer to the results structure that will be populated + * with evaluated data. + * @param dbg An optional pointer (can be null) to the debug data structure. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_EvaluateDataDebug(argus_hnd_t *hnd, argus_results_t *res, + argus_results_debug_t *dbg); + +/*!*************************************************************************** + * @brief Executes a crosstalk calibration measurement. * * @details This function immediately triggers a crosstalk vector calibration - * measurement sequence. The ordinary measurement activity is suspended - * while the calibration is ongoing. + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. * - * In order to perform a crosstalk calibration, the reflection of the - * transmitted signal must be kept from the receiver side, by either - * covering the TX completely (or RX respectively) or by setting up - * an absorbing target at far distance. + * In order to perform a crosstalk calibration, the reflection of the + * transmitted signal must be kept from the receiver side, by either + * covering the TX completely (or RX respectively) or by setting up + * an absorbing target at far distance. * - * After calibration has finished successfully, the obtained data is - * applied immediately and can be read from the API using the - * #Argus_GetCalibrationCrosstalkVectorTable function. + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationCrosstalkVectorTable function. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd, argus_mode_t mode); - +status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Executes a relative range offset calibration measurement. + * @brief Executes a relative range offset calibration measurement. * * @details This function immediately triggers a relative range offset calibration - * measurement sequence. The ordinary measurement activity is suspended - * while the calibration is ongoing. + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. * - * In order to perform a relative range offset calibration, a flat - * calibration target must be setup perpendicular to the sensors - * field-of-view. + * In order to perform a relative range offset calibration, a flat + * calibration target must be setup perpendicular to the sensors + * field-of-view. * - * \code + * \code * AFBR-S50 ToF Sensor - * #| - * #| | - * #|-----+ | - * #| Rx | | - * Reference #|----++ | Calibration - * Plane #| Tx | | Target - * #|----+ | - * #| | - * #| <------- targetRange -----------------> | - * \endcode + * #| + * #| | + * #|-----+ | + * #| RX | | + * Reference #|----++ | Calibration + * Plane #| TX | | Target + * #|----+ | + * #| | + * #| <------- targetRange -----------------> | + * \endcode * - * There are two options to run the offset calibration: relative and - * absolute. - * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): - * when the absolute distance is not essential or the distance to - * the calibration target is not known, the relative method can be - * used to compensate the relative pixel range offset w.r.t. the - * average range. The absolute or global range offset is not changed. - * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): - * when the absolute distance is essential and the distance to the - * calibration target is known, the absolute method can be used to - * calibrate the absolute measured distance. Additionally, the - * relative pixel offset w.r.t. the average range is also compensated. - * . + * There are two options to run the offset calibration: relative and + * absolute. * - * After calibration has finished successfully, the obtained data is - * applied immediately and can be read from the API using the - * #Argus_GetCalibrationPixelRangeOffsets or - * #Argus_GetCalibrationGlobalRangeOffset function. + * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): + * when the absolute distance is not essential or the distance to + * the calibration target is not known, the relative method can be + * used to compensate the relative pixel range offset w.r.t. the + * average range. The absolute or global range offset is not changed. + * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): + * when the absolute distance is essential and the distance to the + * calibration target is known, the absolute method can be used to + * calibrate the absolute measured distance. Additionally, the + * relative pixel offset w.r.t. the average range is also compensated. + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationPixelRangeOffsets or + * #Argus_GetCalibrationGlobalRangeOffset function. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd, - argus_mode_t mode); +status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Executes an absolute range offset calibration measurement. + * @brief Executes an absolute range offset calibration measurement. * * @details This function immediately triggers an absolute range offset calibration - * measurement sequence. The ordinary measurement activity is suspended - * while the calibration is ongoing. + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. * - * In order to perform a relative range offset calibration, a flat - * calibration target must be setup perpendicular to the sensors - * field-of-view. + * In order to perform a relative range offset calibration, a flat + * calibration target must be setup perpendicular to the sensors + * field-of-view. * - * \code + * \code * AFBR-S50 ToF Sensor - * #| - * #| | - * #|-----+ | - * #| Rx | | - * Reference #|----++ | Calibration - * Plane #| Tx | | Target - * #|----+ | - * #| | - * #| <------- targetRange -----------------> | - * \endcode + * #| + * #| | + * #|-----+ | + * #| RX | | + * Reference #|----++ | Calibration + * Plane #| TX | | Target + * #|----+ | + * #| | + * #| <------- targetRange -----------------> | + * \endcode * - * There are two options to run the offset calibration: relative and - * absolute. - * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): - * when the absolute distance is not essential or the distance to - * the calibration target is not known, the relative method can be - * used to compensate the relative pixel range offset w.r.t. the - * average range. The absolute or global range offset is not changed. - * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): - * when the absolute distance is essential and the distance to the - * calibration target is known, the absolute method can be used to - * calibrate the absolute measured distance. Additionally, the - * relative pixel offset w.r.t. the average range is also compensated. - * . + * There are two options to run the offset calibration: relative and + * absolute. * - * After calibration has finished successfully, the obtained data is - * applied immediately and can be read from the API using the - * #Argus_GetCalibrationPixelRangeOffsets or - * #Argus_GetCalibrationGlobalRangeOffset function. + * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): + * when the absolute distance is not essential or the distance to + * the calibration target is not known, the relative method can be + * used to compensate the relative pixel range offset w.r.t. the + * average range. The absolute or global range offset is not changed. + * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): + * when the absolute distance is essential and the distance to the + * calibration target is known, the absolute method can be used to + * calibrate the absolute measured distance. Additionally, the + * relative pixel offset w.r.t. the average range is also compensated. + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param targetRange The absolute range between the reference plane and the - * calibration target in meter an Q9.22 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationPixelRangeOffsets or + * #Argus_GetCalibrationGlobalRangeOffset function. + * + * @param hnd The API handle; contains all internal states and data. + * @param targetRange The absolute range between the reference plane and the + * calibration target in meter an Q9.22 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence(argus_hnd_t *hnd, - argus_mode_t mode, q9_22_t targetRange); /*! @} */ @@ -600,188 +790,245 @@ status_t Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence(argus_hnd_t *hnd, /*!************************************************************************** * Configuration API **************************************************************************** - * @addtogroup arguscfg + * @addtogroup argus_cfg * @{ ****************************************************************************/ /*!*************************************************************************** - * @brief Sets the measurement mode to a specified device. + * @brief Gets the default measurement mode for a specified module type. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param module The specified module type. + * @return Returns the default measurement mode for the specified module type. *****************************************************************************/ -status_t Argus_SetConfigurationMeasurementMode(argus_hnd_t *hnd, - argus_mode_t value); +argus_mode_t Argus_GetDefaultMeasurementMode(argus_module_version_t module); /*!*************************************************************************** - * @brief Gets the measurement mode from a specified device. + * @brief Sets the measurement mode to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details This generates a new default configuration and calibration for the + * specified measurement mode and applies it to the device. + * + * See #argus_mode_t for a list of all available measurement modes. + * + * @warning The function overwrites all made changes to the configuration or + * calibration parameters with the default values. So this function + * must be called before any other changes to the configuration or + * calibration parameters are made! + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The new measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetConfigurationMeasurementMode(argus_hnd_t *hnd, - argus_mode_t *value); +status_t Argus_SetMeasurementMode(argus_hnd_t *hnd, argus_mode_t mode); /*!*************************************************************************** - * @brief Sets the frame time to a specified device. + * @brief Resets the measurement mode to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The measurement frame time in microseconds. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details This generates a new default configuration and calibration for the + * current measurement mode and applies it to the device. + * + * @warning The function overwrites all made changes to the configuration or + * calibration parameters with the default values. So this function + * must be called before any other changes to the configuration or + * calibration parameters are made! + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ResetMeasurementMode(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the measurement mode from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The current measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetMeasurementMode(argus_hnd_t *hnd, argus_mode_t *mode); + +/*!*************************************************************************** + * @brief Sets the frame time to a specified device. + * + * @details The frame time determines the measurement rate of the device. + * Usually, this controller the periodicity of measurements to be + * triggered via the timer based measurement mode that can be started + * via the #Argus_StartMeasurementTimer function. But also the + * behavior of the #Argus_TriggerMeasurement function is influenced + * by the frame rate parameter. + * + * The frame time parameter handles the maximum frame rate by limiting + * the trigger of a new measurement frame to the specified value. + * On the other hand, the accuracy of measurement results it also + * influenced since the frame time specifies the maximum integration + * depth (i.e. exposure time) along with the laser safety limitations. + * This means, the measurement speed can be increased by decreasing + * the frame time parameter and the accuracy can be improved by + * increasing the frame time parameter. + * + * Note the additional factor will limit the maximum frame rate on the + * one hand and the accuracy on the other hand: + * - High CPU load (or slow CPU in general) will lead to delays due + * to long data evaluation task (#Argus_EvaluateData) or long user + * application code. Reduce CPU load or increase CPU power to + * increase maximum frame rate. + * - The dual frequency mode (DFM, see #Argus_SetConfigurationDFMMode) + * will additionally limit the maximum frame rate to approximately + * 100 frames per second. Disable the DFM to increase maximum frame + * rates. + * - The smart power save (SPS, see + * #Argus_SetConfigurationSmartPowerSaveEnabled) mode will decrease + * the maximum possible frame rate slightly. Disable it to increase + * the maximum frame rate. + * - The dynamic configuration adaption with its specific power saving + * ratio parameter (see #Argus_SetConfigurationDynamicAdaption) + * will limit the maximum integration depth along with the laser + * safety limitations. Increase the power saving ratio to increase + * accuracy. Note that laser safety limitations might already limit + * the maximum integration depth such that the power saving ratio + * is ineffective. + * . + * + * @param hnd The API handle; contains all internal states and data. + * @param value The measurement frame time in microseconds. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationFrameTime(argus_hnd_t *hnd, uint32_t value); /*!*************************************************************************** - * @brief Gets the frame time from a specified device. + * @brief Gets the frame time from a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current frame time in microseconds. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current frame time in microseconds. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationFrameTime(argus_hnd_t *hnd, uint32_t *value); /*!*************************************************************************** - * @brief Sets the smart power save enabled flag to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new smart power save enabled flag. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the smart power save enabled flag to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new smart power save enabled flag. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationSmartPowerSaveEnabled(argus_hnd_t *hnd, - argus_mode_t mode, bool value); /*!*************************************************************************** - * @brief Gets the smart power save enabled flag from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current smart power save enabled flag. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the smart power save enabled flag from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current smart power save enabled flag. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationSmartPowerSaveEnabled(argus_hnd_t *hnd, - argus_mode_t mode, bool *value); /*!*************************************************************************** - * @brief Sets the Dual Frequency Mode (DFM) to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new DFM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the Dual Frequency Mode (DFM) to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new DFM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationDFMMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_dfm_mode_t value); /*!*************************************************************************** - * @brief Gets the Dual Frequency Mode (DFM) from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current DFM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the Dual Frequency Mode (DFM) from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current DFM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationDFMMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_dfm_mode_t *value); /*!*************************************************************************** - * @brief Sets the Shot Noise Monitor (SNM) mode to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new SNM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the Shot Noise Monitor (SNM) mode to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new SNM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_snm_mode_t value); /*!*************************************************************************** - * @brief Gets the Shot Noise Montor (SNM) mode from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current SNM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the Shot Noise Monitor (SNM) mode from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current SNM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_snm_mode_t *value); -#if 0 -///*!*************************************************************************** -// * @brief Sets the Crosstalk Monitor (XTM) mode to a specified device. -// * @param hnd The API handle; contains all internal states and data. -// * @param mode The targeted measurement mode. -// * @param value The new XTM mode value (true: enabled; false: disabled). -// * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). -// *****************************************************************************/ -//status_t Argus_SetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd, -// argus_mode_t mode, -// bool value); -// -///*!*************************************************************************** -// * @brief Gets the Crosstalk Monitor (XTM) mode from a specified device. -// * @param hnd The API handle; contains all internal states and data. -// * @param mode The targeted measurement mode. -// * @param value The current XTM mode value (true: enabled; false: disabled). -// * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). -// *****************************************************************************/ -//status_t Argus_GetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd, -// argus_mode_t mode, -// bool * value); -#endif /*!*************************************************************************** - * @brief Sets the full DCA module configuration to a specified device. +* @brief Sets the Crosstalk Monitor (XTM) mode to a specified device. +* +* @param hnd The API handle; contains all internal states and data. +* @param value The new XTM mode value (true: enabled; false: disabled). +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t Argus_SetConfigurationCrosstalkMonitorMode(argus_hnd_t *hnd, + bool value); + +/*!*************************************************************************** +* @brief Gets the Crosstalk Monitor (XTM) mode from a specified device. +* +* @param hnd The API handle; contains all internal states and data. +* @param value The current XTM mode value (true: enabled; false: disabled). +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t Argus_GetConfigurationCrosstalkMonitorMode(argus_hnd_t *hnd, + bool *value); + +/*!*************************************************************************** + * @brief Sets the full DCA module configuration to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new DCA configuration set. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new DCA configuration set. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationDynamicAdaption(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_dca_t const *value); /*!*************************************************************************** - * @brief Gets the # from a specified device. + * @brief Gets the # from a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current DCA configuration set value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current DCA configuration set value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationDynamicAdaption(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_dca_t *value); /*!*************************************************************************** - * @brief Sets the pixel binning configuration parameters to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new pixel binning configuration parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the pixel binning configuration parameters to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new pixel binning configuration parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationPixelBinning(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_pba_t const *value); /*!*************************************************************************** - * @brief Gets the pixel binning configuration parameters from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current pixel binning configuration parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the pixel binning configuration parameters from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current pixel binning configuration parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationPixelBinning(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_pba_t *value); /*!*************************************************************************** - * @brief Gets the current unambiguous range in mm. - * @param hnd The API handle; contains all internal states and data. - * @param range_mm The returned range in mm. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the current unambiguous range in mm. + * + * @param hnd The API handle; contains all internal states and data. + * @param range_mm The returned range in mm. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd, uint32_t *range_mm); @@ -791,458 +1038,398 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd, /*!************************************************************************** * Calibration API **************************************************************************** - * @addtogroup arguscal + * @addtogroup argus_cal * @{ ****************************************************************************/ /*!*************************************************************************** - * @brief Sets the global range offset value to a specified device. + * @brief Sets the global range offset value to a specified device. * - * @details The global range offset is subtracted from the raw range values. + * @details The global range offset is subtracted from the raw range values. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new global range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new global range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd, - argus_mode_t mode, q0_15_t value); /*!*************************************************************************** - * @brief Gets the global range offset value from a specified device. + * @brief Gets the global range offset value from a specified device. * - * @details The global range offset is subtracted from the raw range values. + * @details The global range offset is subtracted from the raw range values. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current global range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current global range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationGlobalRangeOffset(argus_hnd_t *hnd, - argus_mode_t mode, q0_15_t *value); /*!*************************************************************************** - * @brief Sets the relative pixel offset table to a specified device. + * @brief Sets the relative pixel offset table to a specified device. * * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. The relative pixel offset values are meant - * to be with respect to the average range of all pixels, i.e. the - * average of all relative offsets should be 0! + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! * - * The crosstalk vector table is a two dimensional array of type - * #q0_15_t. + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t, wrapped within the #argus_cal_offset_table_t structure. * - * The dimensions are: - * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * Its recommended to use the built-in pixel offset calibration - * sequence (see #Argus_ExecuteRelativeRangeOffsetCalibrationSequence) - * to determine the offset table for the current device. + * Its recommended to use the built-in pixel offset calibration + * sequence (see #Argus_ExecuteRelativeRangeOffsetCalibrationSequence) + * to determine the offset table for the current device. * - * If a constant offset table for all device needs to be incorporated - * into the sources, the #Argus_GetExternalPixelRangeOffsets_Callback - * should be used. + * If a constant offset table for all device needs to be incorporated + * into the sources, the #Argus_GetPixelRangeOffsets_Callback + * should be used. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new relative range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_SetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, - q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); +status_t Argus_SetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, + argus_cal_offset_table_t const *value); /*!*************************************************************************** - * @brief Gets the relative pixel offset table from a specified device. + * @brief Gets the relative pixel offset table from a specified device. * * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. The relative pixel offset values are meant - * to be with respect to the average range of all pixels, i.e. the - * average of all relative offsets should be 0! + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! * - * The crosstalk vector table is a two dimensional array of type - * #q0_15_t. + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t, wrapped within the #argus_cal_offset_table_t structure. * - * The dimensions are: - * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current relative range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, - q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); +status_t Argus_GetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, + argus_cal_offset_table_t *value); /*!*************************************************************************** - * @brief Gets the relative pixel offset table from a specified device. + * @brief Resets the relative pixel offset values for the specified device to + * the factory calibrated default values. * * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. The relative pixel offset values are meant - * to be with respect to the average range of all pixels, i.e. the - * average of all relative offsets should be 0! + * values for each individual pixel. Note that a global range offset + * is applied additionally. * - * The crosstalk vector table is a two dimensional array of type - * #q0_15_t. + * The factory defaults are device specific values. * - * The dimensions are: - * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * - * The total offset table consists of the custom pixel offset values - * (set via #Argus_SetCalibrationPixelRangeOffsets) and the internal, - * factory calibrated device specific offset values. - * This is informational only! - * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current total relative range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetCalibrationTotalPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, - q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); - +status_t Argus_ResetCalibrationPixelRangeOffsets(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Resets the relative pixel offset values for the specified device to - * the factory calibrated default values. - * - * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. - * - * The factory defaults are device specific values. - * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t Argus_ResetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode); - -/*!*************************************************************************** - * @brief A callback that returns the external pixel range offsets. + * @brief A callback that returns the external pixel range offsets. * * @details The function needs to be implemented by the host application in - * order to set the external pixel range offsets values upon system - * initialization. If not defined in user code, the default - * implementation will return an all zero offset table, assuming there - * is no (additional) external pixel range offset values. + * order to set the external pixel range offsets values upon system + * initialization. If not defined in user code, the default + * implementation will return an all zero offset table, assuming there + * is no (additional) external pixel range offset values. * - * If defined in user code, the function must fill all offset values - * in the provided \par offsets parameter with external range offset - * values. - * The values can be obtained by the calibration routine. + * If defined in user code, the function must fill all offset values + * in the provided \par offsets parameter with external range offset + * values. + * The values can be obtained by the calibration routine. * - * Example usage: + * Example usage: * - * @code - * status_t Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - * argus_mode_t mode) - * { - * (void) mode; // Ignore mode; use same values for all modes. - * memset(offsets, 0, sizeof(q0_15_t) * ARGUS_PIXELS); + * @code + * status_t Argus_GetPixelRangeOffsets_Callback(argus_cal_offset_table_t offsets) + * { + * memset(offsets, 0, sizeof(argus_cal_offset_t)); * - * // Set offset values in meter and Q0.15 format. - * offsets[0][0].dS = -16384; offsets[0][0].dC = -32768; - * offsets[0][1].dS = -32768; offsets[0][1].dC = 0; - * offsets[0][2].dS = 16384; offsets[0][2].dC = -16384; - * // etc. - * } - * @endcode + * // Set offset values in meter and Q0.15 format. + * offsets.Table[0][0] = -3542; + * offsets.Table[0][1] = -4385; + * offsets.Table[0][2] = 2953; + * // etc. + * } + * @endcode * - * @param offsets The pixel range offsets in meter and Q0.15 format; to be - * filled with data. - * @param mode Determines the current measurement mode; can be ignored if - * only a single measurement mode is utilized. + * @param offsets The pixel range offsets in meter and Q0.15 format; to be + * filled with data. + * @param mode The current measurement mode. *****************************************************************************/ -void Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - argus_mode_t mode); +void Argus_GetPixelRangeOffsets_Callback(argus_cal_offset_table_t *offsets, + argus_mode_t const mode); /*!*************************************************************************** - * @brief Sets the sample time for the range offset calibration sequence. + * @brief Sets the sample time for the range offset calibration sequence. * - * @details Gets the measurement sample acquisition time for executing the - * range offset calibration sequence and generate the offset data.\n - * Units: msec. + * @details Gets the measurement sample acquisition time for executing the + * range offset calibration sequence and generate the offset data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new range offset calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new range offset calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t value); /*!*************************************************************************** - * @brief Gets the sample time for the range offset calibration sequence. + * @brief Gets the sample time for the range offset calibration sequence. * - * @details Gets the measurement sample acquisition time for executing the - * range offset calibration sequence and generate the ooffset data.\n - * Units: msec. + * @details Gets the measurement sample acquisition time for executing the + * range offset calibration sequence and generate the offset data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current range offset calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current range offset calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t *value); /*!*************************************************************************** - * @brief Sets the pixel-to-pixel crosstalk compensation parameters to a specified device. + * @brief Sets the pixel-to-pixel crosstalk compensation parameters to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new pixel-to-pixel crosstalk compensation parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new pixel-to-pixel crosstalk compensation parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkPixel2Pixel(argus_hnd_t *hnd, - argus_mode_t mode, argus_cal_p2pxtalk_t const *value); /*!*************************************************************************** - * @brief Gets the pixel-to-pixel crosstalk compensation parameters from a specified device. + * @brief Gets the pixel-to-pixel crosstalk compensation parameters from a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current pixel-to-pixel crosstalk compensation parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current pixel-to-pixel crosstalk compensation parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkPixel2Pixel(argus_hnd_t *hnd, - argus_mode_t mode, argus_cal_p2pxtalk_t *value); /*!*************************************************************************** - * @brief Sets the custom crosstalk vector table to a specified device. + * @brief Sets the custom crosstalk vector table to a specified device. * * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. + * in the data evaluation phase. * - * The crosstalk vector table is a three dimensional array of type - * #xtalk_t. + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. The #argus_cal_xtalk_table_t is the corresponding + * typedef for the required data. * - * The dimensions are: - * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) - * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * Its recommended to use the built-in crosstalk calibration sequence - * (see #Argus_ExecuteXtalkCalibrationSequence) to determine the - * crosstalk vector table. + * Its recommended to use the built-in crosstalk calibration sequence + * (see #Argus_ExecuteXtalkCalibrationSequence) to determine the + * crosstalk vector table. * - * If a constant table for all device needs to be incorporated into - * the sources, the #Argus_GetExternalCrosstalkVectorTable_Callback - * should be used. + * If a constant table for all device needs to be incorporated into + * the sources, the #Argus_GetCrosstalkVectorTable_Callback + * should be used. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new crosstalk vector table. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode, - xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + argus_cal_xtalk_table_t const *value); /*!*************************************************************************** - * @brief Gets the custom crosstalk vector table from a specified device. + * @brief Gets the custom crosstalk vector table from a specified device. * * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. + * in the data evaluation phase. * - * The crosstalk vector table is a three dimensional array of type - * #xtalk_t. + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. The #argus_cal_xtalk_table_t is the corresponding + * typedef for the required data. * - * The dimensions are: - * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) - * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current crosstalk vector table. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode, - xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + argus_cal_xtalk_table_t *value); /*!*************************************************************************** - * @brief Gets the factory calibrated default crosstalk vector table for the - * specified device. + * @brief Resets the crosstalk vector table for the specified device to the + * factory calibrated default values. * * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. - * - * The crosstalk vector table is a three dimensional array of type - * #xtalk_t. - * - * The dimensions are: - * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) - * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . - * - * The total vector table consists of the custom crosstalk vector - * table (set via #Argus_SetCalibrationCrosstalkVectorTable) and - * an internal, factory calibrated device specific vector table. - * This is informational only! - * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current total crosstalk vector table. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t Argus_GetCalibrationTotalCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode, - xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); - -/*!*************************************************************************** - * @brief Resets the crosstalk vector table for the specified device to the - * factory calibrated default values. - * - * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. + * in the data evaluation phase. * * - * The factory defaults are device specific calibrated values. + * The factory defaults are device specific calibrated values. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_ResetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode); +status_t Argus_ResetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Sets the sample time for the crosstalk calibration sequence. + * @brief Sets the sample time for the crosstalk calibration sequence. * - * @details Sets the measurement sample acquisition time for executing the - * crosstalk calibration sequence and generate the crosstalk data.\n - * Units: msec. + * @details Sets the measurement sample acquisition time for executing the + * crosstalk calibration sequence and generate the crosstalk data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new crosstalk calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd, uint16_t value); /*!*************************************************************************** - * @brief Gets the sample time for the crosstalk calibration sequence. + * @brief Gets the sample time for the crosstalk calibration sequence. * - * @details Gets the measurement sample acquisition time for executing the - * crosstalk calibration sequence and generate the crosstalk data.\n - * Units: msec. + * @details Gets the measurement sample acquisition time for executing the + * crosstalk calibration sequence and generate the crosstalk data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current crosstalk calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current crosstalk calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd, uint16_t *value); /*!*************************************************************************** - * @brief Sets the max. amplitude threshold for the crosstalk calibration sequence. + * @brief Sets the max. amplitude threshold for the crosstalk calibration sequence. * - * @details The maximum amplitude threshold defines a maximum crosstalk vector - * amplitude before causing an error message. If the crosstalk is - * too high, there is usually an issue with the measurement setup, i.e. - * there is still a measurement signal detected. + * @details The maximum amplitude threshold defines a maximum crosstalk vector + * amplitude before causing an error message. If the crosstalk is + * too high, there is usually an issue with the measurement setup, i.e. + * there is still a measurement signal detected. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new crosstalk calibration sequence maximum amplitude - * threshold value in UQ12.4 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk calibration sequence maximum amplitude + * threshold value in UQ12.4 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkSequenceAmplitudeThreshold(argus_hnd_t *hnd, uq12_4_t value); /*!*************************************************************************** - * @brief Gets the max. amplitude threshold for the crosstalk calibration sequence. + * @brief Gets the max. amplitude threshold for the crosstalk calibration sequence. * - * @details The maximum amplitude threshold defines a maximum crosstalk vector - * amplitude before causing an error message. If the crosstalk is - * too high, there is usually an issue with the measurement setup, i.e. - * there is still a measurement signal detected. + * @details The maximum amplitude threshold defines a maximum crosstalk vector + * amplitude before causing an error message. If the crosstalk is + * too high, there is usually an issue with the measurement setup, i.e. + * there is still a measurement signal detected. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current max. amplitude threshold value in UQ12.4 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current max. amplitude threshold value in UQ12.4 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkSequenceAmplitudeThreshold(argus_hnd_t *hnd, uq12_4_t *value); -/*!*************************************************************************** - * @brief Sets the sample count for the substrate voltage calibration sequence. - * - * @param hnd The API handle; contains all internal states and data. - * @param value The new substrate voltage calibration sequence sample count. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t Argus_SetCalibrationVsubSequenceSampleCount(argus_hnd_t *hnd, - uint16_t value); /*!*************************************************************************** - * @brief Gets the sample count for the substrate voltage calibration sequence. + * @brief Clears all user calibration values from NVM for the specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current substrate voltage calibration sequence sample count. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details The user calibration values are stored in the non-volatile memory + * (NVM) if corresponding \link #argus_nvm NVM hardware layer\endlink + * is implemented. This method clears the user calibration data from + * the non-volatile memory. + * + * @warning This does not reset the currently set calibration values to + * factory defaults! + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetCalibrationVsubSequenceSampleCount(argus_hnd_t *hnd, - uint16_t *value); +status_t Argus_ClearUserCalibration(argus_hnd_t *hnd); + /*!*************************************************************************** - * @brief A callback that returns the external crosstalk vector table. + * @brief A callback that returns the external crosstalk vector table. * * @details The function needs to be implemented by the host application in - * order to set the external crosstalk vector table upon system - * initialization. If not defined in user code, the default - * implementation will return an all zero vector table, assuming there - * is no (additional) external crosstalk. + * order to set the external crosstalk vector table upon system + * initialization. If not defined in user code, the default + * implementation will return an all zero vector table, assuming there + * is no (additional) external crosstalk. * - * If defined in user code, the function must fill all vector values - * in the provided \par xtalk parameter with external crosstalk values. - * The values can be obtained by the calibration routine. + * If defined in user code, the function must fill all vector values + * in the provided \par crosstalk parameter with external crosstalk + * values. The values can be obtained by the calibration routine. * - * Example usage: + * Example usage: * - * @code - * status_t Argus_GetExternalCrosstalkVectorTable_Callback(xtalk_t xtalk[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - * argus_mode_t mode) - * { - * (void) mode; // Ignore mode; use same values for all modes. - * memset(&xtalk, 0, sizeof(xtalk)); + * @code + * status_t Argus_GetCrosstalkVectorTable_Callback( + * argus_cal_xtalk_table_t * xtalk) + * { + * memset(xtalk, 0, sizeof(argus_cal_xtalk_table_t)); * - * // Set crosstalk vectors in Q11.4 format. - * // Note on dual-frequency frame index: 0 = A-Frame; 1 = B-Frame - * xtalk[0][0][0].dS = -9; xtalk[0][0][0].dC = -11; - * xtalk[0][0][1].dS = -13; xtalk[0][0][1].dC = -16; - * xtalk[0][0][2].dS = 6; xtalk[0][0][2].dC = -18; - * // etc. - * } - * @endcode + * // Set crosstalk vectors in Q11.4 format. + * // Note on dual-frequency frame index: 0 = A-Frame; 1 = B-Frame + * xtalk.FrameA[0][0].dS = -9; xtalk.FrameB[0][0].dC = -11; + * xtalk.FrameA[0][1].dS = -13; xtalk.FrameB[0][1].dC = -16; + * xtalk.FrameA[0][2].dS = 6; xtalk.FrameB[0][2].dC = -18; + * // etc. + * } + * @endcode * - * @param xtalk The crosstalk vector array; to be filled with data. - * @param mode Determines the current measurement mode; can be ignored if - * only a single measurement mode is utilized. + * @param xtalk The crosstalk vector array; to be filled with data. + * @param mode The current measurement mode. *****************************************************************************/ -void Argus_GetExternalCrosstalkVectorTable_Callback(xtalk_t - xtalk[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - argus_mode_t mode); +void Argus_GetCrosstalkVectorTable_Callback(argus_cal_xtalk_table_t *xtalk, + argus_mode_t const mode); -#ifdef __cplusplus -} -#endif +/*!*************************************************************************** + * @brief Gets the currently calibrated Golden Pixel coordinates. + * + * @details The Golden Pixel is the pixel that is located at the center of the + * receiving light beam. Thus it it the one that receives the most + * signal and plays a central role in 1D measurement systems. + * + * The function fills the provided \p x and \p y parameters with + * the Golden Pixel coordinates. Typical values are x = 5 and y = 1 + * or 2. But the actual values depend on the specific sensor. + * + * Please also note the utility functions provided in the \ref argus_map + * module to convert between pixel coordinates and channel numbers or + * shift pixel maps by a position offset (#ShiftSelectedPixels) or + * generate pixel masks centered around the Golden Pixel + * (#FillPixelMask). + * + * @param hnd The API handle; contains all internal states and data. + * @param x The Golden Pixel x-coordinate. + * @param y The Golden Pixel y-coordinate. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationGoldenPixel(argus_hnd_t const *hnd, uint8_t *x, uint8_t *y); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_API_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h index f23d117648..8f6b40bdc5 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the dynamic configuration adaption (DCA) setup parameters - * and data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the dynamic configuration adaption (DCA) setup parameters + * and data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,34 +37,37 @@ #ifndef ARGUS_DCA_H #define ARGUS_DCA_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusdca Dynamic Configuration Adaption - * @ingroup argusapi + * @defgroup argus_dca Dynamic Configuration Adaption + * @ingroup argus_api * - * @brief Dynamic Configuration Adaption (DCA) parameter definitions and API functions. + * @brief Dynamic Configuration Adaption (DCA) parameter definitions and API functions. * - * @details The DCA contains an algorithms that detect ambient conditions - * and adopt the device configuration to the changing parameters - * dynamically while operating the sensor. This is achieved by - * rating the currently received signal quality and changing the - * device configuration accordingly to the gathered information - * from the current measurement frame results before the next - * integration cycle starts. + * @details The DCA contains an algorithms that detect ambient conditions + * and adopt the device configuration to the changing parameters + * dynamically while operating the sensor. This is achieved by + * rating the currently received signal quality and changing the + * device configuration accordingly to the gathered information + * from the current measurement frame results before the next + * integration cycle starts. * - * The DCA consists of the following features: - * - Static or Dynamic mode. The first is utilizing the nominal - * values while the latter is dynamically adopting between min. - * and max. value and starting from the nominal values. - * - Analog Integration Depth Adaption (from multiple patterns down to single pulses) - * - Optical Output Power Adaption - * - Pixel Input Gain Adaption (w/ ambient light rejection) - * - ADC Sensitivity (i.e. ADC Range) Adaption - * - Power Saving Ratio (to decrease the average output power and thus the current consumption) - * - All that features are heeding the Laser Safety limits. - * . + * The DCA consists of the following features: + * - Static or Dynamic mode. The first is utilizing the nominal + * values while the latter is dynamically adopting between min. + * and max. value and starting from the nominal values. + * - Analog Integration Depth Adaption (from multiple patterns down to single pulses) + * - Optical Output Power Adaption + * - Pixel Input Gain Adaption (w/ ambient light rejection) + * - ADC Sensitivity (i.e. ADC Range) Adaption + * - Power Saving Ratio (to decrease the average output power and thus the current consumption) + * - All that features are heeding the Laser Safety limits. + * . * - * @addtogroup argusdca + * @addtogroup argus_dca * @{ *****************************************************************************/ @@ -73,39 +76,26 @@ /*! The minimum amplitude threshold value. */ -#define ARGUS_CFG_DCA_ATH_MIN (1U << 6U) +#define ARGUS_CFG_DCA_ATH_MIN (1U << 6U) /*! The maximum amplitude threshold value. */ -#define ARGUS_CFG_DCA_ATH_MAX (0xFFFFU) +#define ARGUS_CFG_DCA_ATH_MAX (0xFFFFU) /*! The minimum saturated pixel threshold value. */ -#define ARGUS_CFG_DCA_PXTH_MIN (1U) +#define ARGUS_CFG_DCA_PXTH_MIN (1U) /*! The maximum saturated pixel threshold value. */ -#define ARGUS_CFG_DCA_PXTH_MAX (33U) +#define ARGUS_CFG_DCA_PXTH_MAX (33U) /*! The maximum analog integration depth in UQ10.6 format, * i.e. the maximum pattern count per sample. */ -#define ARGUS_CFG_DCA_DEPTH_MAX ((uq10_6_t)(ADS_SEQCT_N_MASK << (6U - ADS_SEQCT_N_SHIFT))) +#define ARGUS_CFG_DCA_DEPTH_MAX ((uq10_6_t)(0xFFC0U)) /*! The minimum analog integration depth in UQ10.6 format, * i.e. the minimum pattern count per sample. */ -#define ARGUS_CFG_DCA_DEPTH_MIN ((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble - - -/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */ -#define ARGUS_CFG_DCA_POWER_MAX_LSB (ADS_LASET_VCSEL_HC1_MASK >> ADS_LASET_VCSEL_HC1_SHIFT) - -/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */ -#define ARGUS_CFG_DCA_POWER_MIN_LSB (1) - -/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */ -#define ARGUS_CFG_DCA_POWER_MAX (ADS0032_HIGH_CURRENT_LSB2MA(ARGUS_CFG_DCA_POWER_MAX_LSB + 1)) - -/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */ -#define ARGUS_CFG_DCA_POWER_MIN (1) +#define ARGUS_CFG_DCA_DEPTH_MIN ((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble /*! The dynamic configuration algorithm Pixel Input Gain stage count. */ @@ -139,9 +129,9 @@ /*!*************************************************************************** - * @brief The dynamic configuration algorithm enable flags. + * @brief The dynamic configuration algorithm enable flags. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_enable_t { /*! @internal * * DCA is disabled and will be completely skipped. @@ -160,9 +150,9 @@ typedef enum { } argus_dca_enable_t; /*!*************************************************************************** - * @brief The DCA amplitude evaluation method. + * @brief The DCA amplitude evaluation method. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_amplitude_mode_t { /*! Evaluate the DCA amplitude as the maximum of all valid amplitudes. */ DCA_AMPLITUDE_MAX = 1U, @@ -172,9 +162,9 @@ typedef enum { } argus_dca_amplitude_mode_t; /*!*************************************************************************** - * @brief The dynamic configuration algorithm Optical Output Power stages enumerator. + * @brief The dynamic configuration algorithm Optical Output Power stages enumerator. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_power_t { /*! Use low output power stage. */ DCA_POWER_LOW = 0, @@ -187,9 +177,9 @@ typedef enum { } argus_dca_power_t; /*!*************************************************************************** - * @brief The dynamic configuration algorithm Pixel Input Gain stages enumerator. + * @brief The dynamic configuration algorithm Pixel Input Gain stages enumerator. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_gain_t { /*! Low gain stage. */ DCA_GAIN_LOW = 0, @@ -206,113 +196,113 @@ typedef enum { /*!*************************************************************************** - * @brief State flags for the current frame. - * @details State flags determine the current state of the measurement frame: - * - [0]: #ARGUS_STATE_MEASUREMENT_MODE - * - [1]: #ARGUS_STATE_DUAL_FREQ_MODE - * - [2]: #ARGUS_STATE_MEASUREMENT_FREQ - * - [3]: #ARGUS_STATE_DEBUG_MODE - * - [4]: #ARGUS_STATE_WEAK_SIGNAL - * - [5]: #ARGUS_STATE_BGL_WARNING - * - [6]: #ARGUS_STATE_BGL_ERROR - * - [7]: #ARGUS_STATE_PLL_LOCKED - * - [8]: #ARGUS_STATE_LASER_WARNING - * - [9]: #ARGUS_STATE_LASER_ERROR - * - [10]: #ARGUS_STATE_HAS_DATA - * - [11]: #ARGUS_STATE_HAS_AUX_DATA - * - [12]: #ARGUS_STATE_DCA_MAX - * - [13]: DCA Power Stage - * - [14-15]: DCA Gain Stages - * . + * @brief State flags for the current frame. + * @details State flags determine the current state of the measurement frame: + * - [0]: #ARGUS_STATE_XTALK_MONITOR_ACTIVE + * - [1]: #ARGUS_STATE_DUAL_FREQ_MODE + * - [2]: #ARGUS_STATE_MEASUREMENT_FREQ + * - [3]: #ARGUS_STATE_DEBUG_MODE + * - [4]: #ARGUS_STATE_WEAK_SIGNAL + * - [5]: #ARGUS_STATE_BGL_WARNING + * - [6]: #ARGUS_STATE_BGL_ERROR + * - [7]: #ARGUS_STATE_PLL_LOCKED + * - [8]: #ARGUS_STATE_LASER_WARNING + * - [9]: #ARGUS_STATE_LASER_ERROR + * - [10]: #ARGUS_STATE_HAS_DATA + * - [11]: #ARGUS_STATE_HAS_AUX_DATA + * - [12]: #ARGUS_STATE_DCA_MAX + * - [13]: DCA Power Stage + * - [14-15]: DCA Gain Stages + * . *****************************************************************************/ -typedef enum { +typedef enum argus_state_t { /*! No state flag set. */ ARGUS_STATE_NONE = 0, - /*! 0x0001: Measurement Mode. - * - 0: Mode A: Long Range / Medium Precision - * - 1: Mode B: Short Range / High Precision */ - ARGUS_STATE_MEASUREMENT_MODE = 1U << 0U, + /*! 0x0001: Crosstalk Monitor is enabled and updating. + * - 0: Inactive: crosstalk monitor values are not updated, + * - 1: Active: crosstalk monitor values are updated. */ + ARGUS_STATE_XTALK_MONITOR_ACTIVE = 1U << 0U, /*! 0x0002: Dual Frequency Mode Enabled. - * - 0: Disabled: measurements with base frequency, - * - 1: Enabled: measurement with detuned frequency. */ + * - 0: Disabled: measurements with base frequency, + * - 1: Enabled: measurement with detuned frequency. */ ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U, /*! 0x0004: Measurement Frequency for Dual Frequency Mode * (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set). - * - 0: A-Frame w/ detuned frequency, - * - 1: B-Frame w/ detuned frequency */ + * - 0: A-Frame w/ detuned frequency, + * - 1: B-Frame w/ detuned frequency */ ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U, /*! 0x0008: Debug Mode. If set, the range value of erroneous pixels - * are not cleared or reset. - * - 0: Disabled (default). - * - 1: Enabled. */ + * are not cleared or reset. + * - 0: Disabled (default). + * - 1: Enabled. */ ARGUS_STATE_DEBUG_MODE = 1U << 3U, /*! 0x0010: Weak Signal Flag. - * Set whenever the Pixel Binning Algorithm is detecting a - * weak signal, i.e. if the amplitude dies not reach its - * (absolute) threshold. If the Golden Pixel is enabled, - * this also indicates that the Pixel Binning Algorithm - * falls back to the Golden Pixel. - * - 0: Normal Signal. - * - 1: Weak Signal or Golden Pixel Mode. */ + * Set whenever the Pixel Binning Algorithm is detecting a + * weak signal, i.e. if the amplitude dies not reach its + * (absolute) threshold. If the Golden Pixel is enabled, + * this also indicates that the Pixel Binning Algorithm + * falls back to the Golden Pixel. + * - 0: Normal Signal. + * - 1: Weak Signal or Golden Pixel Mode. */ ARGUS_STATE_WEAK_SIGNAL = 1U << 4U, /*! 0x0020: Background Light Warning Flag. - * Set whenever the background light is very high and the - * measurement data might be unreliable. - * - 0: No Warning: Background Light is within valid range. - * - 1: Warning: Background Light is very high. */ + * Set whenever the background light is very high and the + * measurement data might be unreliable. + * - 0: No Warning: Background Light is within valid range. + * - 1: Warning: Background Light is very high. */ ARGUS_STATE_BGL_WARNING = 1U << 5U, /*! 0x0040: Background Light Error Flag. - * Set whenever the background light is too high and the - * measurement data is unreliable or invalid. - * - 0: No Error: Background Light is within valid range. - * - 1: Error: Background Light is too high. */ + * Set whenever the background light is too high and the + * measurement data is unreliable or invalid. + * - 0: No Error: Background Light is within valid range. + * - 1: Error: Background Light is too high. */ ARGUS_STATE_BGL_ERROR = 1U << 6U, /*! 0x0080: PLL_LOCKED bit. - * - 0: PLL not locked at start of integration. - * - 1: PLL locked at start of integration. */ + * - 0: PLL not locked at start of integration. + * - 1: PLL locked at start of integration. */ ARGUS_STATE_PLL_LOCKED = 1U << 7U, /*! 0x0100: Laser Failure Warning Flag. - * Set whenever the an invalid system condition is detected. - * (i.e. DCA at max state but no amplitude on any (incl. reference) - * pixel, not amplitude but any saturated pixel). - * - 0: No Warning: Laser is operating properly. - * - 1: Warning: Invalid laser conditions detected. If the invalid - * condition stays, a laser malfunction error is raised. */ + * Set whenever the an invalid system condition is detected. + * (i.e. DCA at max state but no amplitude on any (incl. reference) + * pixel, not amplitude but any saturated pixel). + * - 0: No Warning: Laser is operating properly. + * - 1: Warning: Invalid laser conditions detected. If the invalid + * condition stays, a laser malfunction error is raised. */ ARGUS_STATE_LASER_WARNING = 1U << 8U, /*! 0x0200: Laser Failure Error Flag. - * Set whenever a laser malfunction error is raised and the - * system is put into a safe state. - * - 0: No Error: Laser is operating properly. - * - 1: Error: Invalid laser conditions are detected for a certain - * soak time and the system is put into a safe state. */ + * Set whenever a laser malfunction error is raised and the + * system is put into a safe state. + * - 0: No Error: Laser is operating properly. + * - 1: Error: Invalid laser conditions are detected for a certain + * soak time and the system is put into a safe state. */ ARGUS_STATE_LASER_ERROR = 1U << 9U, /*! 0x0400: Set if current frame has distance measurement data available. - * - 0: No measurement data available, all values are 0 or stalled. - * - 1: Measurement data is available and correctly evaluated. */ + * - 0: No measurement data available, all values are 0 or stalled. + * - 1: Measurement data is available and correctly evaluated. */ ARGUS_STATE_HAS_DATA = 1U << 10U, /*! 0x0800: Set if current frame has auxiliary measurement data available. - * - 0: No auxiliary data available, all values are 0 or stalled. - * - 1: Auxiliary data is available and correctly evaluated. */ + * - 0: No auxiliary data available, all values are 0 or stalled. + * - 1: Auxiliary data is available and correctly evaluated. */ ARGUS_STATE_HAS_AUX_DATA = 1U << 11U, /*! 0x1000: DCA Maximum State Flag. - * Set whenever the DCA has extended all its parameters to their - * maximum values and can not increase the integration energy any - * further. - * - 0: DCA has not yet reached its maximum state. - * - 1: DCA has reached its maximum state and can not increase any further. */ + * Set whenever the DCA has extended all its parameters to their + * maximum values and can not increase the integration energy any + * further. + * - 0: DCA has not yet reached its maximum state. + * - 1: DCA has reached its maximum state and can not increase any further. */ ARGUS_STATE_DCA_MAX = 1U << 12U, /*! 0x2000: DCA is in high Optical Output Power stage. */ @@ -333,20 +323,20 @@ typedef enum { } argus_state_t; /*!*************************************************************************** - * @brief Dynamic Configuration Adaption (DCA) Parameters. - * @details DCA contains: - * - Static or dynamic mode. The first is utilizing the nominal values - * while the latter is dynamically adopting between min. and max. - * value and starting form the nominal values. - * - Analog Integration Depth Adaption down to single pulses. - * - Optical Output Power Adaption - * - Pixel Input Gain Adaption - * - Digital Integration Depth Adaption - * - Dynamic Global Phase Shift Injection. - * - All that features are heeding the Laser Safety limits. - * . + * @brief Dynamic Configuration Adaption (DCA) Parameters. + * @details DCA contains: + * - Static or dynamic mode. The first is utilizing the nominal values + * while the latter is dynamically adopting between min. and max. + * value and starting form the nominal values. + * - Analog Integration Depth Adaption down to single pulses. + * - Optical Output Power Adaption + * - Pixel Input Gain Adaption + * - Digital Integration Depth Adaption + * - Dynamic Global Phase Shift Injection. + * - All that features are heeding the Laser Safety limits. + * . *****************************************************************************/ -typedef struct { +typedef struct argus_cfg_dca_t { /*! Enables the automatic configuration adaption features. * Enables the dynamic part if #DCA_ENABLE_DYNAMIC and the static only if * #DCA_ENABLE_STATIC. */ @@ -494,4 +484,7 @@ typedef struct { } argus_cfg_dca_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_DCA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h index c639922a7d..6d9dd3e9d3 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 hardware API. - * @details This file provides generic definitions belonging to all - * devices from the AFBR-S50 product family. + * @brief This file is part of the AFBR-S50 hardware API. + * @details This file provides generic definitions belonging to all + * devices from the AFBR-S50 product family. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,6 +37,9 @@ #ifndef ARGUS_DEF_H #define ARGUS_DEF_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** * Include files @@ -52,36 +55,41 @@ #include /*!*************************************************************************** - * @addtogroup argusapi + * @addtogroup argus_api * @{ *****************************************************************************/ /*!*************************************************************************** - * @brief Maximum number of phases per measurement cycle. - * @details The actual phase number is defined in the register configuration. - * However the software does only support a fixed value of 4 yet. + * @brief Maximum number of phases per measurement cycle. + * @details The actual phase number is defined in the register configuration. + * However the software does only support a fixed value of 4 yet. *****************************************************************************/ -#define ARGUS_PHASECOUNT 4U +#define ARGUS_PHASECOUNT 4 /*!*************************************************************************** - * @brief The device pixel field size in x direction (long edge). + * @brief The device pixel field size in x direction (long edge). *****************************************************************************/ -#define ARGUS_PIXELS_X 8U +#define ARGUS_PIXELS_X 8 /*!*************************************************************************** - * @brief The device pixel field size in y direction (short edge). + * @brief The device pixel field size in y direction (short edge). *****************************************************************************/ -#define ARGUS_PIXELS_Y 4U +#define ARGUS_PIXELS_Y 4 /*!*************************************************************************** - * @brief The total device pixel count. + * @brief The total device pixel count. *****************************************************************************/ -#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) +#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) /*!*************************************************************************** - * @brief The AFBR-S50 module types. + * @brief A flag indicating that the device is a extended range device. *****************************************************************************/ -typedef enum { +#define MODULE_EXTENDED_FLAG (0x40U) + +/*!*************************************************************************** + * @brief The AFBR-S50 module types. + *****************************************************************************/ +typedef enum argus_module_version_t { /*! No device connected or not recognized. */ MODULE_NONE = 0, @@ -89,54 +97,80 @@ typedef enum { * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * medium range 3D applications. * Version 1 - legacy version! */ - AFBR_S50MV85G_V1 = 1, + AFBR_S50MV85G_V1 = 0x01, /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * medium range 3D applications. * Version 2 - legacy version! */ - AFBR_S50MV85G_V2 = 2, - - /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device - * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for - * medium range 3D applications. - * Version 7 - current version! */ - AFBR_S50MV85G_V3 = 7, + AFBR_S50MV85G_V2 = 0x02, /*! AFBR-S50LV85D: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * long range 1D applications. * Version 1 - current version! */ - AFBR_S50LV85D_V1 = 3, + AFBR_S50LV85D_V1 = 0x03, /*! AFBR-S50MV68B: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and red, 680 nm, laser source for * medium range 1D applications. * Version 1 - current version! */ - AFBR_S50MV68B_V1 = 4, + AFBR_S50MV68B_V1 = 0x04, /*! AFBR-S50MV85I: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * medium range 3D applications. * Version 1 - current version! */ - AFBR_S50MV85I_V1 = 5, + AFBR_S50MV85I_V1 = 0x05, /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * short range 3D applications. * Version 1 - current version! */ - AFBR_S50SV85K_V1 = 6, + AFBR_S50SV85K_V1 = 0x06, + /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * medium range 3D applications. + * Version 3 - current version! */ + AFBR_S50MV85G_V3 = 0x07, - /*! Reserved for future extensions. */ - Reserved = 0x3F + /*! AFBR-S50LX85D: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended long range 1D applications. + * Version 1 - current version! */ + AFBR_S50LX85D_V1 = AFBR_S50LV85D_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX68B: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and red, 680 nm, laser source for + * extended medium range 1D applications. + * Version 1 - current version! */ + AFBR_S50MX68B_V1 = AFBR_S50MV68B_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX85I: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended medium range 3D applications. + * Version 1 - current version! */ + AFBR_S50MX85I_V1 = AFBR_S50MV85I_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended short range 3D applications. + * Version 1 - current version! */ + AFBR_S50SX85K_V1 = AFBR_S50SV85K_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended medium range 3D applications. + * Version 1 - current version! */ + AFBR_S50MX85G_V1 = AFBR_S50MV85G_V3 | MODULE_EXTENDED_FLAG, } argus_module_version_t; /*!*************************************************************************** - * @brief The AFBR-S50 laser configurations. + * @brief The AFBR-S50 laser configurations. *****************************************************************************/ -typedef enum { +typedef enum argus_laser_type_t { /*! No laser connected. */ LASER_NONE = 0, @@ -152,12 +186,15 @@ typedef enum { /*! 850nm Infra-Red VCSEL v2 /w extended mode. */ LASER_H_V2X = 4, + /*! 680nm Red VCSEL v1 w/ extended mode. */ + LASER_R_V1X = 5, + } argus_laser_type_t; /*!*************************************************************************** - * @brief The AFBR-S50 chip versions. + * @brief The AFBR-S50 chip versions. *****************************************************************************/ -typedef enum { +typedef enum argus_chip_version_t { /*! No device connected or not recognized. */ ADS0032_NONE = 0, @@ -178,37 +215,102 @@ typedef enum { } argus_chip_version_t; -/*!*************************************************************************** - * @brief The number of measurement modes with distinct configuration and - * calibration records. - *****************************************************************************/ -#define ARGUS_MODE_COUNT (2) /*!*************************************************************************** - * @brief The measurement modes. + * @brief The measurement mode flags. + * @details The measurement mode flags that can be combined to a measurement + * mode, e.g. high speed short range mode. See #argus_mode_t for + * a complete list of available measurement modes. + * + * - Bit 0: Short Range Mode + * - Bit 1: Long Range Mode + * - Bit 2: High Speed Mode + * + * Note that the Long and Short Range Flags are mutual exclusive but + * any of those 2 must be set. Thus the value 0 is invalid! + * All other flags enhance the base configurations, e.g. the High + * Speed flag create the high speed mode of the selected base + * measurement mode. *****************************************************************************/ -typedef enum { - /*! Measurement Mode A: Long Range Mode. */ - ARGUS_MODE_A = 1, +typedef enum argus_mode_flags_t { + /*! Measurement Mode Flag for Short Range Base Mode. */ + ARGUS_MODE_FLAG_SHORT_RANGE = 0x01 << 0, - /*! Measurement Mode B: Short Range Mode. */ - ARGUS_MODE_B = 2, + /*! Measurement Mode Flag for Long Range Base Mode. */ + ARGUS_MODE_FLAG_LONG_RANGE = 0x01 << 1, + + /*! Measurement Mode Flag for High Speed Mode. */ + ARGUS_MODE_FLAG_HIGH_SPEED = 0x01 << 2 + +} argus_mode_flags_t; + +/*!*************************************************************************** + * @brief The measurement modes. + * @details The measurement modes are composed in binary from of the flags + * define in #argus_mode_flags_t, i.e. each bit has a special meaning: + * + * - Bit 0: Short Range Mode + * - Bit 1: Long Range Mode + * - Bit 2: High Speed Mode + * + * Note that the Long and Short Range Bits are mutual exclusive but any + * of those 2 must be set. Thus the value 0 is invalid! + *****************************************************************************/ +typedef enum argus_mode_t { + /*! Measurement Mode: Short Range Mode. */ + ARGUS_MODE_SHORT_RANGE = // = 0x01 = 0b0001 + ARGUS_MODE_FLAG_SHORT_RANGE, + + /*! Measurement Mode: Long Range Mode. */ + ARGUS_MODE_LONG_RANGE = // = 0x02 = 0b0010 + ARGUS_MODE_FLAG_LONG_RANGE, + + /*! Measurement Mode: High Speed Short Range Mode. */ + ARGUS_MODE_HIGH_SPEED_SHORT_RANGE = // = 0x05 = 0b0101 + ARGUS_MODE_FLAG_SHORT_RANGE | ARGUS_MODE_FLAG_HIGH_SPEED, + + /*! Measurement Mode: High Speed Long Range Mode. */ + ARGUS_MODE_HIGH_SPEED_LONG_RANGE = // = 0x06 = 0b0110 + ARGUS_MODE_FLAG_LONG_RANGE | ARGUS_MODE_FLAG_HIGH_SPEED, } argus_mode_t; +/*! The data structure for the API representing a AFBR-S50 device instance. */ +typedef struct argus_hnd_t argus_hnd_t; + /*!*************************************************************************** - * @brief Generic API callback function. - * @details Invoked by the API. The content of the abstract data pointer - * depends upon the context. - * @param status The module status that caused the callback. #STATUS_OK if - * everything was as expected. - * @param data An abstract pointer to an user defined data. This will usually - * be passed to the function that also takes the callback as an - * parameter. Otherwise it has a special meaning such as - * configuration or calibration data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Measurement Ready API callback function. + * + * @details Invoked by the API whenever a measurement cycle is finished and + * new data is ready to be evaluated via the #Argus_EvaluateData API + * function. + * The callback is passed to the API via the #Argus_TriggerMeasurement + * or #Argus_StartMeasurementTimer API functions. + * The API passes the status of the currently finished measurement + * cycle to the callback as first parameters. The second parameter is + * a pointer the API handle structure. The latter is used to identify + * the calling instance of the API in case of multiple devices. + * Further it can be passed to the #Argus_EvaluateData function. + * + * @warning Since the callback is called from an interrupt context, the + * callback execution must return as fast as possible. The usual task + * in the callback is to post an event to the main thread to inform it + * about the new data and that is must call the #Argus_EvaluateData + * function. + * + * @param status The module status that caused the callback. #STATUS_OK if + * everything was as expected. + * + * @param hnd The API handle pointer to the calling instance. Identifies the + * instance of the API that was invoking the callback and thus the + * instance that must call the #Argus_EvaluateData for. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -typedef status_t (*argus_callback_t)(status_t status, void *data); +typedef status_t (*argus_measurement_ready_callback_t)(status_t status, argus_hnd_t *hnd); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_DEF_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h index b2517182f8..4c0182a0b3 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the dual frequency mode (DFM) setup parameters. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the dual frequency mode (DFM) setup parameters. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,24 +36,27 @@ #ifndef ARGUS_DFM_H #define ARGUS_DFM_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusdfm Dual Frequency Mode - * @ingroup argusapi + * @defgroup argus_dfm Dual Frequency Mode + * @ingroup argus_api * - * @brief Dual Frequency Mode (DFM) parameter definitions and API functions. + * @brief Dual Frequency Mode (DFM) parameter definitions and API functions. * - * @details The DFM is an algorithm to extend the unambiguous range of the - * sensor by utilizing two detuned measurement frequencies. + * @details The DFM is an algorithm to extend the unambiguous range of the + * sensor by utilizing two detuned measurement frequencies. * - * The AFBR-S50 API provides three measurement modes: - * - 1X: Single Frequency Measurement - * - 4X: Dual Frequency Measurement w/ 4 times the unambiguous - * range of the Single Frequency Measurement - * - 8X: Dual Frequency Measurement w/ 8 times the unambiguous - * range of the Single Frequency Measurement + * The AFBR-S50 API provides three measurement modes: + * - 1X: Single Frequency Measurement + * - 4X: Dual Frequency Measurement w/ 4 times the unambiguous + * range of the Single Frequency Measurement + * - 8X: Dual Frequency Measurement w/ 8 times the unambiguous + * range of the Single Frequency Measurement * - * @addtogroup argusdfm + * @addtogroup argus_dfm * @{ *****************************************************************************/ @@ -61,10 +64,10 @@ #define ARGUS_DFM_FRAME_COUNT (2U) /*! The Dual Frequency Mode measurement modes count. Excluding the disabled mode. */ -#define ARGUS_DFM_MODE_COUNT (2U) // expect off-mode! +#define ARGUS_DFM_MODE_COUNT (2U) // except off-mode! /*! The Dual Frequency Mode measurement modes enumeration. */ -typedef enum { +typedef enum argus_dfm_mode_t { /*! Single Frequency Measurement Mode (w/ 1x Unambiguous Range). */ DFM_MODE_OFF = 0U, @@ -78,4 +81,7 @@ typedef enum { /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_DFM_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h index 64588d25f1..4ffa55656b 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines macros to work with pixel and ADC channel masks. + * @brief This file is part of the AFBR-S50 API. + * @details Defines macros to work with pixel and ADC channel masks. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,217 +37,409 @@ #ifndef ARGUS_MAP_H #define ARGUS_MAP_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusmap ADC Channel Mapping - * @ingroup argusres + * @defgroup argus_map Pixel Channel Mapping + * @ingroup argus_api * - * @brief Pixel ADC Channel Mapping + * @brief Pixel Channel Mapping * - * @details The ADC Channels of each pixel or auxiliary channel on the device - * are numbered in a way that is convenient on the chip architecture. - * The macros in this module are defined in order to map between the - * chip internal channel number (ch) to the two-dimensional - * x-y-indices or one-dimensional n-index representation. + * @details The ADC Channels of each pixel or auxiliary channel on the device + * are numbered in a way that is convenient on the chip architecture. + * The macros in this module are defined in order to map between the + * chip internal channel number (ch) to the two-dimensional + * x-y-indices or one-dimensional n-index representation. * - * @addtogroup argusmap + * @addtogroup argus_map * @{ *****************************************************************************/ -#include "api/argus_def.h" #include "utility/int_math.h" +#include +/*!*************************************************************************** + * @brief The device pixel field size in x direction (long edge). + *****************************************************************************/ +#define ARGUS_PIXELS_X 8 + +/*!*************************************************************************** + * @brief The device pixel field size in y direction (short edge). + *****************************************************************************/ +#define ARGUS_PIXELS_Y 4 + +/*!*************************************************************************** + * @brief The total device pixel count. + *****************************************************************************/ +#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) /*!***************************************************************************** - * @brief Macro to determine the pixel ADC channel number from the x-z-indices. - * @param x The x-index of the pixel. - * @param y The y-index of the pixel. - * @return The ADC channel number of the pixel. + * @brief Macro to determine the pixel ADC channel number from the x-z-indices. + * @param x The x-index of the pixel. + * @param y The y-index of the pixel. + * @return The ADC channel number of the pixel. ******************************************************************************/ #define PIXEL_XY2CH(x, y) ((((y) << 3U) & 0x10U) | (((x) ^ 0x07U) << 1U) | ((y) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine the pixel x-index from the ADC channel number. - * @param c The ADC channel number of the pixel. - * @return The x-index of the pixel. + * @brief Macro to determine the pixel x-index from the ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The x-index of the pixel. ******************************************************************************/ #define PIXEL_CH2X(c) ((((c) >> 1U) ^ 0x07U) & 0x07U) /*!***************************************************************************** - * @brief Macro to determine the pixel y-index from the ADC channel number. - * @param c The ADC channel number of the pixel. - * @return The y-index of the pixel. + * @brief Macro to determine the pixel y-index from the ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The y-index of the pixel. ******************************************************************************/ #define PIXEL_CH2Y(c) ((((c) >> 3U) & 0x02U) | ((c) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine the n-index from the x-y-indices. - * @param x The x-index of the pixel. - * @param y The y-index of the pixel. - * @return The n-index of the pixel. + * @brief Macro to determine the n-index from the x-y-indices. + * @param x The x-index of the pixel. + * @param y The y-index of the pixel. + * @return The n-index of the pixel. ******************************************************************************/ #define PIXEL_XY2N(x, y) (((x) << 2U) | (y)) /*!***************************************************************************** - * @brief Macro to determine the pixel x-index from the n-index. - * @param n The n-index of the pixel. - * @return The x-index number of the pixel. + * @brief Macro to determine the pixel x-index from the n-index. + * @param n The n-index of the pixel. + * @return The x-index number of the pixel. ******************************************************************************/ #define PIXEL_N2X(n) ((n) >> 2U) /*!***************************************************************************** - * @brief Macro to determine the pixel y-index from the n-index. - * @param n The n-index of the pixel. - * @return The y-index number of the pixel. + * @brief Macro to determine the pixel y-index from the n-index. + * @param n The n-index of the pixel. + * @return The y-index number of the pixel. ******************************************************************************/ #define PIXEL_N2Y(n) ((n) & 0x03U) /*!***************************************************************************** - * @brief Macro to determine the pixel n-index from the ADC channel number. - * @param n The n-index of the pixel. - * @return The ADC channel number of the pixel. + * @brief Macro to determine the pixel n-index from the ADC channel number. + * @param n The n-index of the pixel. + * @return The ADC channel number of the pixel. ******************************************************************************/ #define PIXEL_N2CH(n) ((((n) << 3U) & 0x10U) | ((((n) >> 1U) ^ 0x0EU) & 0x0EU) | ((n) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine the pixel - * @param c The ADC channel number of the pixel. - * @return The n-index of the pixel. + * @brief Macro to determine the pixel + * @param c The ADC channel number of the pixel. + * @return The n-index of the pixel. ******************************************************************************/ #define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask. - * @param msk 32-bit pixel mask - * @param n n-index of the pixel. - * @return True if the pixel (n) is enabled. + * @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel. + * @return True if the pixel (n) is enabled. ******************************************************************************/ #define PIXELN_ISENABLED(msk, n) (((msk) >> (n)) & 0x01U) /*!***************************************************************************** - * @brief Macro to enable a pixel given by the n-index in a pixel mask. - * @param msk 32-bit pixel mask - * @param n n-index of the pixel to enable. + * @brief Macro to enable a pixel given by the n-index in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel to enable. ******************************************************************************/ #define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n))) /*!***************************************************************************** - * @brief Macro disable a pixel given by the n-index in a pixel mask. - * @param msk 32-bit pixel mask - * @param n n-index of the pixel to disable. + * @brief Macro disable a pixel given by the n-index in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel to disable. ******************************************************************************/ #define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n)))) /*!***************************************************************************** - * @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask. - * @param msk The 32-bit pixel mask - * @param c The ADC channel number of the pixel. - * @return True if the specified pixel ADC channel is enabled. + * @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The ADC channel number of the pixel. + * @return True if the specified pixel ADC channel is enabled. ******************************************************************************/ #define PIXELCH_ISENABLED(msk, c) (PIXELN_ISENABLED(msk, PIXEL_CH2N(c))) /*!***************************************************************************** - * @brief Macro to enable an ADC pixel channel in a pixel mask. - * @param msk The 32-bit pixel mask - * @param c The pixel ADC channel number to enable. + * @brief Macro to enable an ADC pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The pixel ADC channel number to enable. ******************************************************************************/ #define PIXELCH_ENABLE(msk, c) (PIXELN_ENABLE(msk, PIXEL_CH2N(c))) /*!***************************************************************************** - * @brief Macro to disable an ADC pixel channel in a pixel mask. - * @param msk The 32-bit pixel mask - * @param c The pixel ADC channel number to disable. + * @brief Macro to disable an ADC pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The pixel ADC channel number to disable. ******************************************************************************/ #define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c))) /*!***************************************************************************** - * @brief Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x-index of the pixel. - * @param y y-index of the pixel. - * @return True if the pixel (x,y) is enabled. + * @brief Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel. + * @param y y-index of the pixel. + * @return True if the pixel (x,y) is enabled. ******************************************************************************/ #define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y))) /*!***************************************************************************** - * @brief Macro to enable a pixel given by the x-y-indices in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x-index of the pixel to enable. - * @param y y-index of the pixel to enable. + * @brief Macro to enable a pixel given by the x-y-indices in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel to enable. + * @param y y-index of the pixel to enable. ******************************************************************************/ #define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y))) /*!***************************************************************************** - * @brief Macro disable a pixel given by the x-y-indices in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x-index of the pixel to disable. - * @param y y-index of the pixel to disable. + * @brief Macro disable a pixel given by the x-y-indices in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel to disable. + * @param y y-index of the pixel to disable. ******************************************************************************/ #define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y))) /*!***************************************************************************** - * @brief Macro to determine if an ADC channel is enabled in a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel. - * @return True if the ADC channel is enabled. + * @brief Macro to determine if an ADC channel is enabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel. + * @return True if the ADC channel is enabled. ******************************************************************************/ #define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U) /*!***************************************************************************** - * @brief Macro to determine if an ADC channel is enabled in a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel to enabled. + * @brief Macro to determine if an ADC channel is enabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel to enabled. ******************************************************************************/ #define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U))) /*!***************************************************************************** - * @brief Macro to determine if an ADC channel is disabled in a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel to disable. + * @brief Macro to determine if an ADC channel is disabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel to disable. ******************************************************************************/ #define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U)))) /*!***************************************************************************** - * @brief Macro to determine the number of enabled pixel/channels in a mask - * via a popcount algorithm. - * @param pxmsk 32-bit pixel mask - * @return The count of enabled pixel channels. + * @brief Macro to determine the number of enabled pixel/channels in a mask + * via a popcount algorithm. + * @param pxmsk 32-bit pixel mask + * @return The count of enabled pixel channels. ******************************************************************************/ #define PIXEL_COUNT(pxmsk) popcount(pxmsk) /*!***************************************************************************** - * @brief Macro to determine the number of enabled channels via a popcount - * algorithm. - * @param pxmsk 32-bit pixel mask - * @param chmsk 32-bit channel mask - * @return The count of enabled ADC channels. + * @brief Macro to determine the number of enabled channels via a popcount + * algorithm. + * @param pxmsk 32-bit pixel mask + * @param chmsk 32-bit channel mask + * @return The count of enabled ADC channels. ******************************************************************************/ #define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk)) /*!***************************************************************************** - * @brief Converts a raw ADC channel mask to a x-y-sorted pixel mask. - * @param msk The raw ADC channel mask to be converted. - * @return The converted x-y-sorted pixel mask. + * @brief Converts a raw ADC channel mask to a x-y-sorted pixel mask. + * @param msk The raw ADC channel mask to be converted. + * @return The converted x-y-sorted pixel mask. ******************************************************************************/ static inline uint32_t ChannelToPixelMask(uint32_t msk) { uint32_t res = 0; for (uint_fast8_t n = 0; n < 32; n += 2) { - res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n; + res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n; // sets 2 bits at once } return res; } +/*!***************************************************************************** + * @brief Converts a x-y-sorted pixel mask to a raw ADC channel mask. + * @param msk The x-y-sorted pixel channel mask to be converted. + * @return The converted raw ADC channel mask. + ******************************************************************************/ +static inline uint32_t PixelToChannelMask(uint32_t msk) +{ + uint32_t res = 0; + + for (uint_fast8_t ch = 0; ch < 32; ch += 2) { + res |= ((msk >> PIXEL_CH2N(ch)) & 0x3U) << ch; // sets 2 bits at once + } + + return res; +} + + +/*!***************************************************************************** + * @brief Shifts a pixel mask by a given offset. + * + * @details This moves the selected pixel pattern by a specified number of + * pixels in x and y direction. + * If the shift in y direction is odd (e.g +1), the pattern will be + * shifted by +0.5 or -0.5 in x direction due to the hexagonal shape + * of the pixel field. Thus, a center pixel (usually the Golden Pixel) + * is determined that is used to determine if the pattern is shifted + * by +0.5 or -0.5 pixels in x direction. The center pixel is then + * always shifted without changing the x index and the surrounding + * pixels are adopting its x index accordingly. + * + * Example: Consider the flower pattern, i.e. the Golden Pixel (e.g. + * 5/2) is selected and all is direct neighbors (i.e. 5/1, 6/1, 6/2, + * 6/3, 5/3, 4/2). If the pattern is shifted by -1 in y direction, the + * new Golden Pixel would be 5/1. Now all surrounding pixels are + * selected, namely 4/0, 4/1, 4/2, 5/0, 5/2, 6/1). This yields again + * the flower around the Golden Pixel. + * + * Thus, the pixels can not all be shifted by the same dx/dy values due + * to the hexagonal shape of the pixel field, e.g. the upper right + * neighbor of 5/2 is 5/1 but the upper right neighbor of 5/1 is NOT + * 5/0 but 4/0! + * This happens only if the shift in y direction is an odd number. + * The algorithm to determine new indices is as follows: + * - If the shift in y direction is even (e.g. +2, -2), no compensation + * of the hexagonal shape is needed; skip compensation, simply + * add/subtract indices. + * - If the center pixel y index is even, pixels that will have even y + * index after the shift will be additionally shifted by -1 in x + * direction. + * - If the center pixel y index is odd, pixel that will have odd y + * index after the shift will be additionally shifted by +1 in x + * direction. + * + * @see Please also refer to the function #Argus_GetCalibrationGoldenPixel + * to obtain the current Golden Pixel location. + * + * @param pixel_mask The x-y-sorted pixel mask to be shifted. + * @param dx The number of pixel to shift in x direction. + * @param dy The number of pixel to shift in y direction. + * @param center_y The center y index of the pattern that is shifted. + * @return The shifted pixel mask. + ******************************************************************************/ +static inline uint32_t ShiftSelectedPixels(const uint32_t pixel_mask, + const int8_t dx, + const int8_t dy, + const uint8_t center_y) +{ + if (dx == 0 && dy == 0) { return pixel_mask; } + + uint32_t shifted_mask = 0; + + for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) { + for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) { + int8_t x_src = x - dx; + int8_t y_src = y - dy; + + if (dy & 0x1) { + /* Compensate for hexagonal pixel shape. */ + if ((center_y & 0x1) && (y & 0x1)) { + x_src--; + } + + if (!(center_y & 0x1) && !(y & 0x1)) { + x_src++; + } + } + + if (x_src < 0 || x_src >= ARGUS_PIXELS_X) { continue; } + + if (y_src < 0 || y_src >= ARGUS_PIXELS_Y) { continue; } + + if (PIXELXY_ISENABLED(pixel_mask, x_src, y_src)) { + PIXELXY_ENABLE(shifted_mask, x, y); + } + } + } + + return shifted_mask; +} + +/*!***************************************************************************** + * @brief Fills a pixel mask to a specified number of pixels around a center pixel. + * + * @details The pixel mask is iteratively filled with the nearest pixel to a + * specified center pixel until a specified number of pixels is achieved. + * The distance between two pixel is determined via a quadratic metric, + * i.e. dx^2 + dy^2. Pixels towards the lower x indices are preferred. + * + * Note that the distance of only calculated approximately, e.g. the + * y distance of pixels is considered to be 2 instead of cos(60)*2. + * + * Nothing is done if the number of pixels already exceeds the specified + * /p pixel_count parameter. + * + * @see Please also refer to the function #Argus_GetCalibrationGoldenPixel + * to obtain the current Golden Pixel location. + * + * @param pixel_mask The x-y-sorted pixel mask to be filled with pixels. + * @param pixel_count The final number of pixels in the pixel mask. + * @param center_x The center pixel x-index. + * @param center_y The center pixel y-index. + * @return The filled pixel mask with at least /p pixel_count pixels selected. + ******************************************************************************/ +static inline uint32_t FillPixelMask(uint32_t pixel_mask, + const uint8_t pixel_count, + const uint8_t center_x, + const uint8_t center_y) +{ + assert(pixel_count <= ARGUS_PIXELS); + assert(center_x < ARGUS_PIXELS_X); + assert(center_y < ARGUS_PIXELS_Y); + + if (pixel_count == ARGUS_PIXELS) { return 0xFFFFFFFFU; } + + /* If the pattern was shifted towards boundaries, the pixel count may have + * decreased. In this case, the pixels closest to the reference pixel are + * selected. Pixel towards lower x index are prioritized. */ + while (pixel_count > PIXEL_COUNT(pixel_mask)) { + int32_t min_dist = INT32_MAX; + int8_t min_x = -1; + int8_t min_y = -1; + + /* Find nearest not selected pixel. */ + for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) { + for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) { + if (!PIXELXY_ISENABLED(pixel_mask, x, y)) { + int32_t distx = (x - center_x) << 1; + + if (!(y & 0x1)) { distx++; } + + if (!(center_y & 0x1)) { distx--; } + + const int32_t disty = (y - center_y) << 1; + int32_t dist = distx * distx + disty * disty; + + if (dist < min_dist) { + min_dist = dist; + min_x = x; + min_y = y; + } + } + } + } + + assert(min_x >= 0 && min_x < ARGUS_PIXELS_X); + assert(min_y >= 0 && min_y < ARGUS_PIXELS_Y); + assert(!PIXELXY_ISENABLED(pixel_mask, min_x, min_y)); + PIXELXY_ENABLE(pixel_mask, min_x, min_y); + } + + return pixel_mask; +} /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_MAP_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h index 0e074c6a8b..7a0fa31aa5 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 hardware API. - * @details Defines the generic measurement parameters and data structures. + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic measurement parameters and data structures. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,17 +36,20 @@ #ifndef ARGUS_MEAS_H #define ARGUS_MEAS_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusmeas Measurement/Device Control - * @ingroup argusapi + * @defgroup argus_meas Measurement/Device Control + * @ingroup argus_api * - * @brief Measurement/Device control module + * @brief Measurement/Device control module * - * @details This module contains measurement and device control specific - * definitions and methods. + * @details This module contains measurement and device control specific + * definitions and methods. * - * @addtogroup argusmeas + * @addtogroup argus_meas * @{ *****************************************************************************/ @@ -66,11 +69,11 @@ #define ARGUS_AUX_DATA_SIZE (3U * ARGUS_AUX_CHANNEL_COUNT) // 3 bytes * x channels * 1 phase /*!*************************************************************************** - * @brief The device measurement configuration structure. - * @details The portion of the configuration data that belongs to the - * measurement cycle. I.e. the data that defines a measurement frame. + * @brief The device measurement configuration structure. + * @details The portion of the configuration data that belongs to the + * measurement cycle. I.e. the data that defines a measurement frame. *****************************************************************************/ -typedef struct { +typedef struct argus_meas_frame_t { /*! Frame integration time in microseconds. * The integration time determines the measured time between * the start signal and the IRQ. Note that this value will be @@ -82,13 +85,13 @@ typedef struct { /*! Pixel enabled mask for the 32 pixels sorted * by x-y-indices. - * See [pixel mapping](@ref argusmap) for more + * See [pixel mapping](@ref argus_map) for more * details on the pixel mask. */ uint32_t PxEnMask; /*! ADS channel enabled mask for the remaining * channels 31 .. 63 (miscellaneous values). - * See [pixel mapping](@ref argusmap) for more + * See [pixel mapping](@ref argus_map) for more * details on the ADC channel mask. */ uint32_t ChEnMask; @@ -113,9 +116,6 @@ typedef struct { * Determines the optical output power. */ uq12_4_t OutputPower; - /*! The amplitude that is evaluated and used in the DCA module. */ - uq12_4_t DCAAmplitude; - /*! Laser Bias Current Settings in LSB. */ uint8_t BiasCurrent; @@ -133,4 +133,7 @@ typedef struct { } argus_meas_frame_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_MEAS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h new file mode 100644 index 0000000000..7a41440f39 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h @@ -0,0 +1,59 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic device calibration API. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 ARGUS_OFFSET_H +#define ARGUS_OFFSET_H + +/*!*************************************************************************** + * @addtogroup argus_cal + * @{ + *****************************************************************************/ + +#include "argus_def.h" + +/*!*************************************************************************** + * @brief Pixel Range Offset Table. + * @details Contains pixel range offset values for all 32 active pixels. + *****************************************************************************/ +typedef struct argus_cal_offset_table_t { + /*! The offset values per pixel in meter and Q0.15 format. */ + q0_15_t Table[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + +} argus_cal_offset_table_t; + + +/*! @} */ +#endif /* ARGUS_OFFSET_T */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h index 07b4853bda..f28576500d 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the pixel binning algorithm (PBA) setup parameters and - * data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the pixel binning algorithm (PBA) setup parameters and + * data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,86 +37,98 @@ #ifndef ARGUS_PBA_H #define ARGUS_PBA_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup arguspba Pixel Binning Algorithm - * @ingroup argusapi + * @defgroup argus_pba Pixel Binning Algorithm + * @ingroup argus_api * - * @brief Pixel Binning Algorithm (PBA) parameter definitions and API functions. + * @brief Pixel Binning Algorithm (PBA) parameter definitions and API functions. * - * @details Defines the generic pixel binning algorithm (PBA) setup parameters - * and data structure. + * @details Defines the generic Pixel Binning Algorithm (PBA) setup parameters + * and data structure. * - * The PBA module contains filter algorithms that determine the - * pixels with the best signal quality and extract an 1d distance - * information from the filtered pixels. + * The PBA module contains filter algorithms that determine the + * pixels with the best signal quality and extract an 1D distance + * information from the filtered pixels by averaging them in a + * specified way. * - * The pixel filter algorithm is a three-stage filter with a - * fallback value: + * The Pixel Binning Algorithm is a three-stage filter with a + * fallback value: * - * -# A fixed pre-filter mask is applied to statically disable - * specified pixels. - * -# A relative and absolute amplitude filter is applied in the - * second stage. The relative filter is determined by a ratio - * of the maximum amplitude off all available (i.e. not filtered - * in stage 1) pixels. Pixels that have an amplitude below the - * relative threshold are dismissed. The same holds true for - * the absolute amplitude threshold. All pixel with smaller - * amplitude are dismissed.\n - * The relative threshold is useful to setup a distance - * measurement scenario. All well illuminated pixels are - * selected and considered for the final 1d distance. The - * absolute threshold is used to dismiss pixels that are below - * the noise level. The latter would be considered for the 1d - * result if the maximum amplitude is already very low. - * -# A distance filter is used to distinguish pixels that target - * the actual object from pixels that see the brighter background, - * e.g. white walls. Thus, the pixel with the minimum distance - * is referenced and all pixel that have a distance between - * the minimum and the given minimum distance scope are selected - * for the 1d distance result. The minimum distance scope is - * determined by an relative (to the current minimum distance) - * and an absolute value. The larger scope value is the - * relevant one, i.e. the relative distance scope can be used - * to heed the increasing noise at larger distances. - * -# If all of the above filters fail to determine a single valid - * pixel, the golden pixel is used as a fallback value. The - * golden pixel is the pixel that sits right at the focus point - * of the optics at large distances. - * . + * -# A fixed pre-filter mask is applied to statically disable + * specified pixels. + * -# A relative and absolute amplitude filter is applied in the + * second stage. The relative filter is determined by a ratio + * of the maximum amplitude off all available (i.e. not filtered + * in stage 1) pixels. Pixels that have an amplitude below the + * relative threshold are dismissed. The same holds true for + * the absolute amplitude threshold. All pixel with smaller + * amplitude are dismissed.\n + * Note that the absolute amplitude threshold is disabled if + * the Golden Pixel (see below) is also disabled in order to + * prevent invalid filtering for multi-pixel devices.\n + * The relative threshold is useful to setup a distance + * measurement scenario. All well illuminated pixels are + * selected and considered for the final 1D distance. The + * absolute threshold is used to dismiss pixels that are below + * the noise level. The latter would be considered for the 1D + * result if the maximum amplitude is already very low. + * -# An absolute minimum distance filter is applied in addition + * to the amplitude filter. This removes all pixel that have + * a lower distance than the specified threshold. This is used + * to remove invalid pixels that can be detected by a physically + * not correct negative distance. + * -# A distance filter is used to distinguish pixels that target + * the actual object from pixels that see the brighter background, + * e.g. white walls. Thus, the pixel with the minimum distance + * is referenced and all pixel that have a distance between + * the minimum and the given minimum distance scope are selected + * for the 1D distance result. The minimum distance scope is + * determined by an relative (to the current minimum distance) + * and an absolute value. The larger scope value is the + * relevant one, i.e. the relative distance scope can be used + * to heed the increasing noise at larger distances. + * -# If all of the above filters fail to determine a single valid + * pixel, the Golden Pixel is used as a fallback value. The + * Golden Pixel is the pixel that sits right at the focus point + * of the optics at large distances. + * . * - * After filtering is done, there may be more than a single pixel - * left to determine the 1d signal. Therefore several averaging - * methods are implemented to obtain the best 1d result from many - * pixels. See #argus_pba_averaging_mode_t for details. + * After filtering is done, there may be more than a single pixel + * left to determine the 1D signal. Therefore several averaging + * methods are implemented to obtain the best 1D result from many + * pixels. See #argus_pba_averaging_mode_t for details. * * - * @addtogroup arguspba + * @addtogroup argus_pba * @{ *****************************************************************************/ #include "argus_def.h" /*!*************************************************************************** - * @brief Enable flags for the pixel binning algorithm. + * @brief Enable flags for the pixel binning algorithm. * * @details Determines the pixel binning algorithm feature enable status. - * - [0]: #PBA_ENABLE: Enables the pixel binning feature. - * - [1]: reserved - * - [2]: reserved - * - [3]: reserved - * - [4]: reserved - * - [5]: #PBA_ENABLE_GOLDPX: Enables the golden pixel feature. - * - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope - * feature. - * - [7]: reserved - * . + * - [0]: #PBA_ENABLE: Enables the pixel binning feature. + * - [1]: reserved + * - [2]: reserved + * - [3]: reserved + * - [4]: reserved + * - [5]: #PBA_ENABLE_GOLDPX: Enables the Golden Pixel feature. + * - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope + * feature. + * - [7]: reserved + * . *****************************************************************************/ -typedef enum { +typedef enum argus_pba_flags_t { /*! Enables the pixel binning feature. */ PBA_ENABLE = 1U << 0U, - /*! Enables the golden pixel. */ + /*! Enables the Golden Pixel. */ PBA_ENABLE_GOLDPX = 1U << 5U, /*! Enables the minimum distance scope filter. */ @@ -125,9 +137,9 @@ typedef enum { } argus_pba_flags_t; /*!*************************************************************************** - * @brief The averaging modes for the pixel binning algorithm. + * @brief The averaging modes for the pixel binning algorithm. *****************************************************************************/ -typedef enum { +typedef enum argus_pba_averaging_mode_t { /*! Evaluate the 1D range from all available pixels using * a simple average. */ PBA_SIMPLE_AVG = 1U, @@ -140,11 +152,12 @@ typedef enum { } argus_pba_averaging_mode_t; /*!*************************************************************************** - * @brief The pixel binning algorithm settings data structure. - * @details Describes the pixel binning algorithm settings. + * @brief The pixel binning algorithm settings data structure. + * @details Describes the pixel binning algorithm settings. *****************************************************************************/ typedef struct { - /*! Enables the pixel binning features. + /*! Enables the Pixel Binning Algorithm. + * * Each bit may enable a different feature. See #argus_pba_flags_t * for details about the enabled flags. */ argus_pba_flags_t Enabled; @@ -156,6 +169,7 @@ typedef struct { argus_pba_averaging_mode_t AveragingMode; /*! The Relative amplitude threshold value (in %) of the max. amplitude. + * * Pixels with amplitude below this threshold value are dismissed. * * All available values from the 8-bit representation are valid. @@ -165,22 +179,27 @@ typedef struct { uq0_8_t RelAmplThreshold; /*! The relative minimum distance scope value in %. - * Pixels that have a range value within [x0, x0 + dx] are considered - * for the pixel binning, where x0 is the minimum distance of all - * amplitude picked pixels and dx is the minimum distance scope value. - * The minimum distance scope value will be the maximum of relative - * and absolute value. + * + * Pixels that have a range value within [x0, x0 + dx] are considered + * for the pixel binning, where x0 is the minimum distance of all + * amplitude picked pixels and dx is the minimum distance scope value. + * The minimum distance scope value will be the maximum of relative + * and absolute value. * * All available values from the 8-bit representation are valid. * The actual percentage value is determined by 100%/256*x. * - * Special values: + * Special values: * - 0: Use 0 for absolute value only or to choose the pixel with the - * minimum distance only (of also the absolute value is 0)! */ + * minimum distance only (of also the absolute value is 0)! */ uq0_8_t RelMinDistanceScope; - /*! The Absolute amplitude threshold value in LSB. - * Pixels with amplitude below this threshold value are dismissed. + /*! The absolute amplitude threshold value in LSB. + * + * Pixels with amplitude below this threshold value are dismissed. + * + * The absolute amplitude threshold is only valid if the Golden Pixel + * mode is enabled. Otherwise, the threshold is set to 0 LSB internally. * * All available values from the 16-bit representation are valid. * The actual LSB value is determined by x/16. @@ -189,33 +208,42 @@ typedef struct { uq12_4_t AbsAmplThreshold; /*! The absolute minimum distance scope value in m. - * Pixels that have a range value within [x0, x0 + dx] are considered - * for the pixel binning, where x0 is the minimum distance of all - * amplitude picked pixels and dx is the minimum distance scope value. - * The minimum distance scope value will be the maximum of relative - * and absolute value. + * + * Pixels that have a range value within [x0, x0 + dx] are considered + * for the pixel binning, where x0 is the minimum distance of all + * amplitude picked pixels and dx is the minimum distance scope value. + * The minimum distance scope value will be the maximum of relative + * and absolute value. * * All available values from the 16-bit representation are valid. * The actual LSB value is determined by x/2^15. * - * Special values: + * Special values: * - 0: Use 0 for relative value only or to choose the pixel with the - * minimum distance only (of also the relative value is 0)! */ + * minimum distance only (of also the relative value is 0)! */ uq1_15_t AbsMinDistanceScope; - /*! The pre-filter pixel mask determines the pixel channels that are - * statically excluded from the pixel binning (i.e. 1D distance) result. + /*! The absolute minimum distance threshold value in m. * - * The pixel enabled mask is an 32-bit mask that determines the - * device internal channel number. It is recommended to use the - * - #PIXELXY_ISENABLED(msk, x, y) - * - #PIXELXY_ENABLE(msk, x, y) - * - #PIXELXY_DISABLE(msk, x, y) - * . - * macros to work with the pixel enable masks. */ + * Pixels with distance below this threshold value are dismissed. */ + q9_22_t AbsMinDistanceThreshold; + + /*! The pre-filter pixel mask determines the pixel channels that are + * statically excluded from the pixel binning (i.e. 1D distance) result. + * + * The pixel enabled mask is an 32-bit mask that determines the + * device internal channel number. It is recommended to use the + * - #PIXELXY_ISENABLED(msk, x, y) + * - #PIXELXY_ENABLE(msk, x, y) + * - #PIXELXY_DISABLE(msk, x, y) + * . + * macros to work with the pixel enable masks. */ uint32_t PrefilterMask; } argus_cfg_pba_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_PBA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h index faa031aeb7..a739cea7f3 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the device pixel measurement results data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the device pixel measurement results data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,34 +36,40 @@ #ifndef ARGUS_PX_H #define ARGUS_PX_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @addtogroup argusres + * @addtogroup argus_res * @{ *****************************************************************************/ +#include "argus_def.h" + + /*! Maximum amplitude value in UQ12.4 format. */ -#define ARGUS_AMPLITUDE_MAX (0xFFF0U) +#define ARGUS_AMPLITUDE_MAX (0xFFF0U) /*! Maximum range value in Q9.22 format. * Also used as a special value to determine no object detected or infinity range. */ #define ARGUS_RANGE_MAX (Q9_22_MAX) /*!*************************************************************************** - * @brief Status flags for the evaluated pixel structure. + * @brief Status flags for the evaluated pixel structure. * * @details Determines the pixel status. 0 means OK (#PIXEL_OK). - * - [0]: #PIXEL_OFF: Pixel was disabled and not read from the device. - * - [1]: #PIXEL_SAT: The pixel was saturated. - * - [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result. - * - [3]: #PIXEL_AMPL_MIN: The pixel amplitude has evaluated to 0. - * - [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask. - * - [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal. - * - [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace. - * - [7]: #PIXEL_STALLED: The pixel value is stalled due to errors. - * . + * - [0]: #PIXEL_OFF: Pixel was disabled and not read from the device. + * - [1]: #PIXEL_SAT: The pixel was saturated. + * - [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result. + * - [3]: #PIXEL_INVALID: The pixel data is invalid. + * - [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask. + * - [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal. + * - [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace. + * - [7]: #PIXEL_STALLED: The pixel value is stalled due to errors. + * . *****************************************************************************/ -typedef enum { +typedef enum argus_px_status_t { /*! 0x00: Pixel status OK. */ PIXEL_OK = 0, @@ -77,43 +83,45 @@ typedef enum { /*! 0x04: Pixel is excluded from the pixel binning (1d) result. */ PIXEL_BIN_EXCL = 1U << 2U, - /*! 0x08: Pixel amplitude minimum underrun - * (i.e. the amplitude calculation yields 0). */ - PIXEL_AMPL_MIN = 1U << 3U, + /*! 0x08: Pixel has invalid data due to miscellaneous reasons, e.g. + * - Amplitude calculates to 0 (i.e. division by 0) + * - Golden Pixel is invalid due to other saturated pixel. + * - Range/distance is negative. */ + PIXEL_INVALID = 1U << 3U, /*! 0x10: Pixel is pre-filtered by the static pixel binning pre-filter mask, - * i.e. the pixel is disabled by software. */ + * i.e. the pixel is disabled by software. */ PIXEL_PREFILTERED = 1U << 4U, /*! 0x20: Pixel amplitude is below its threshold value. The received signal - * strength is too low to evaluate a valid signal. The range value is - * set to the maximum possible value (approx. 512 m). */ + * strength is too low to evaluate a valid signal. The range value is + * set to the maximum possible value (approx. 512 m). */ PIXEL_NO_SIGNAL = 1U << 5U, /*! 0x40: Pixel is not in sync with respect to the dual frequency algorithm. - * I.e. the pixel may have a correct value but is estimated into the - * wrong unambiguous window. */ + * I.e. the pixel may have a correct value but is estimated into the + * wrong unambiguous window. */ PIXEL_OUT_OF_SYNC = 1U << 6U, /*! 0x80: Pixel is stalled due to one of the following reasons: - * - #PIXEL_SAT - * - #PIXEL_AMPL_MIN - * - #PIXEL_OUT_OF_SYNC - * - Global Measurement Error - * . - * A stalled pixel does not update its measurement data and keeps the - * previous values. If the issue is resolved, the stall disappears and - * the pixel is updating again. */ + * - #PIXEL_SAT + * - #PIXEL_INVALID + * - #PIXEL_OUT_OF_SYNC + * - Global Measurement Error + * . + * A stalled pixel does not update its measurement data and keeps the + * previous values. If the issue is resolved, the stall disappears and + * the pixel is updating again. */ PIXEL_STALLED = 1U << 7U } argus_px_status_t; /*!*************************************************************************** - * @brief The evaluated measurement results per pixel. - * @details This structure contains the evaluated data for a single pixel.\n - * If the amplitude is 0, the pixel is turned off or has invalid data. + * @brief The evaluated measurement results per pixel. + * @details This structure contains the evaluated data for a single pixel.\n + * If the amplitude is 0, the pixel is turned off or has invalid data. *****************************************************************************/ -typedef struct { +typedef struct argus_pixel_t { /*! Range Values from the device in meter. It is the actual distance before * software adjustments/calibrations. */ q9_22_t Range; @@ -141,14 +149,23 @@ typedef struct { /*!*************************************************************************** * @brief Representation of a correlation vector containing sine/cosine components. *****************************************************************************/ -typedef struct { - /*! The sine component. */ - q15_16_t S; +typedef struct argus_vector_t { + union { + /*! The sine [0] and cosine [1] components. */ + q15_16_t SC[2]; - /*! The cosine component. */ - q15_16_t C; + struct { + /*! The sine component. */ + q15_16_t S; + /*! The cosine component. */ + q15_16_t C; + }; + }; } argus_vector_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_PX_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h index f59d817634..7cb81bfcf1 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the generic measurement results data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the generic measurement results data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,35 +36,39 @@ #ifndef ARGUS_RES_H #define ARGUS_RES_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusres Measurement Data - * @ingroup argusapi + * @defgroup argus_res Measurement Data + * @ingroup argus_api * - * @brief Measurement results data structures. + * @brief Measurement results data structures. * - * @details The interface defines all data structures that correspond to - * the AFBR-S50 measurement results, e.g. - * - 1D distance and amplitude values, - * - 3D distance and amplitude values (i.e. per pixel), - * - Auxiliary channel measurement results (VDD, IAPD, temperature, ...) - * - Device and result status - * - ... - * . + * @details The interface defines all data structures that correspond to + * the AFBR-S50 measurement results, e.g. + * - 1D distance and amplitude values, + * - 3D distance and amplitude values (i.e. per pixel), + * - Auxiliary channel measurement results (VDD, IAPD, temperature, ...) + * - Device and result status + * - ... + * . * - * @addtogroup argusres + * @addtogroup argus_res * @{ *****************************************************************************/ -#include "argus_def.h" #include "argus_px.h" +#include "argus_def.h" #include "argus_meas.h" +#include "argus_xtalk.h" /*!*************************************************************************** - * @brief The 1d measurement results data structure. + * @brief The 1d measurement results data structure. * @details The 1d measurement results obtained by the Pixel Binning Algorithm. *****************************************************************************/ -typedef struct { +typedef struct argus_results_bin_t { /*! Raw 1D range value in meter (Q9.22 format). The distance obtained by * the Pixel Binning Algorithm from the current measurement frame. */ q9_22_t Range; @@ -83,11 +87,11 @@ typedef struct { } argus_results_bin_t; /*!*************************************************************************** - * @brief The auxiliary measurement results data structure. - * @details The auxiliary measurement results obtained by the auxiliary task.\n - * Special values, i.e. 0xFFFFU, indicate no readout value available. + * @brief The auxiliary measurement results data structure. + * @details The auxiliary measurement results obtained by the auxiliary task.\n + * Special values, i.e. 0xFFFFU, indicate no readout value available. *****************************************************************************/ -typedef struct { +typedef struct argus_results_aux_t { /*! VDD ADC channel readout value.\n * Special Value if no value has been measured:\n * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ @@ -129,32 +133,66 @@ typedef struct { } argus_results_aux_t; /*!*************************************************************************** - * @brief The measurement results data structure. + * @brief The debug data of measurement results data structure. + * @details This data structure will be filled with API internal data for + * debugging purposes. + *****************************************************************************/ +typedef struct argus_results_debug_t { + /*! The amplitude that is evaluated and used in the DCA module. */ + uq12_4_t DCAAmplitude; + + /*! Raw x-y-sorted ADC results from the device.\n + * Data is arranged as 32-bit values in following order: + * index > phase; where index is pixel number n and auxiliary ADC channel.\n + * Note that disabled pixels are skipped.\n + * e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */ + uint32_t Data[ARGUS_RAW_DATA_VALUES]; + + /*! The current crosstalk correction values as determined by the + * crosstalk predictor algorithm. This is basically the temperature + * dependent portion of the crosstalk correction.\n + * Note that there are two values for the upper and lower two rows + * respectively. */ + xtalk_t XtalkPredictor[ARGUS_PIXELS_Y / 2U]; + + /*! The current crosstalk correction values as determined by the + * crosstalk monitor algorithm. This is a dynamic portion of the + * crosstalk correction that is determined by monitoring passive + * pixels.\n + * Note that the values are valid row-wise. */ + xtalk_t XtalkMonitor[ARGUS_PIXELS_Y]; + +} argus_results_debug_t; + +/*!*************************************************************************** + * @brief The measurement results data structure. * @details This structure contains all information obtained by a single - * distance measurement on the device: - * - The measurement status can be read from the #Status. - * - A timing information is given via the #TimeStamp. - * - Information about the frame state is in the #Frame structure. - * - The 1D distance results are gathered under #Bin. - * - The 3D distance results for each pixel is at #Pixels or #Pixel. - * - Auxiliary values such as temperature can be found at #Auxiliary. - * - Raw data from the device is stored in the #Data array. - * . + * distance measurement on the device: + * - The measurement status can be read from the #Status. + * - A timing information is given via the #TimeStamp. + * - Information about the frame state is in the #Frame structure. + * - The 1D distance results are gathered under #Bin. + * - The 3D distance results for each pixel is at #Pixels or #Pixel. + * - Auxiliary values such as temperature can be found at #Auxiliary. + * - Raw data and debug information from the device and API is stored + * in the optional #Debug data structure. Note that this points to + * an optional structure and can be null! + * . * - * The pixel x-y orientation is sketched in the following graph. Note that - * the laser source would be on the right side beyond the reference pixel. - * See also \link argusmap ADC Channel Mapping\endlink + * The pixel x-y orientation is sketched in the following graph. Note that + * the laser source would be on the right side beyond the reference pixel. + * See also \link argus_map ADC Channel Mapping\endlink * @code - * // Pixel Field: Pixel[x][y] - * // - * // 0 -----------> x - * // | O O O O O O O O - * // | O O O O O O O O - * // | O O O O O O O O O (ref. Px) - * // y O O O O O O O O + * // Pixel Field: Pixel[x][y] + * // + * // 0 -----------> x + * // | O O O O O O O O + * // | O O O O O O O O + * // | O O O O O O O O O (ref. Px) + * // y O O O O O O O O * @endcode *****************************************************************************/ -typedef struct { +typedef struct argus_results_t { /*! The \link #status_t status\endlink of the current measurement frame. * - 0 (i.e. #STATUS_OK) for a good measurement signal. * - > 0 for warnings and weak measurement signal. @@ -168,13 +206,6 @@ typedef struct { /*! The configuration for the current measurement frame. */ argus_meas_frame_t Frame; - /*! Raw x-y-sorted ADC results from the device.\n - * Data is arranged as 32-bit values in following order: - * index > phase; where index is pixel number n and auxiliary ADC channel.\n - * Note that disabled pixels are skipped.\n - * e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */ - uint32_t Data[ARGUS_RAW_DATA_VALUES]; - union { /*! Pixel data indexed by channel number n.\n * Contains calibrated range, amplitude and pixel status among others. @@ -183,14 +214,14 @@ typedef struct { * - 0..31: active pixels * - 32: reference pixel * - * See also \link argusmap ADC Channel Mapping\endlink */ + * See also \link argus_map ADC Channel Mapping\endlink */ argus_pixel_t Pixels[ARGUS_PIXELS + 1U]; struct { /*! Pixel data indexed by x-y-indices.\n * The pixels are ordered in a two dimensional array that represent * the x and y indices of the pixel.\n - * See also \link argusmap ADC Channel Mapping\endlink + * See also \link argus_map ADC Channel Mapping\endlink * * Contains calibrated range, amplitude and pixel status among others. */ argus_pixel_t Pixel[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; @@ -213,8 +244,17 @@ typedef struct { /*! The auxiliary ADC channel data, e.g. sensor temperature. */ argus_results_aux_t Auxiliary; + /*! Optional Debug Data. + * If the pointer is set to a #argus_results_debug_t data structure before + * passing it to the #Argus_EvaluateData function, the data structure is + * filled with internal parameters for debugging purposes. */ + argus_results_debug_t *Debug; + } argus_results_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_RES_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h index 2776308143..2b77965bb0 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the Shot Noise Monitor (SNM) setup parameters. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the Shot Noise Monitor (SNM) setup parameters. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,30 +36,33 @@ #ifndef ARGUS_SNM_H #define ARGUS_SNM_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argussnm Shot Noise Monitor - * @ingroup argusapi + * @defgroup argus_snm Shot Noise Monitor + * @ingroup argus_api * - * @brief Shot Noise Monitor (SNM) parameter definitions and API functions. + * @brief Shot Noise Monitor (SNM) parameter definitions and API functions. * - * @details The SNM is an algorithm to monitor and react on shot noise - * induced by harsh environment conditions like high ambient - * light. + * @details The SNM is an algorithm to monitor and react on shot noise + * induced by harsh environment conditions like high ambient + * light. * - * The AFBR-S50 API provides three modes: - * - Dynamic: Automatic mode, automatically adopts to current - * ambient conditions. - * - Static (Outdoor): Static mode, optimized for outdoor applications. - * - Static (Indoor): Static mode, optimized for indoor applications. - * . + * The AFBR-S50 API provides three modes: + * - Dynamic: Automatic mode, automatically adopts to current + * ambient conditions. + * - Static (Outdoor): Static mode, optimized for outdoor applications. + * - Static (Indoor): Static mode, optimized for indoor applications. + * . * - * @addtogroup argussnm + * @addtogroup argus_snm * @{ *****************************************************************************/ /*! The Shot Noise Monitor modes enumeration. */ -typedef enum { +typedef enum argus_snm_mode_t { /*! Static Shot Noise Monitoring Mode, optimized for indoor applications. * Assumes the best case scenario, i.e. no bad influence from ambient conditions. * Thus it uses a fixed setting that will result in the best performance. @@ -79,4 +82,7 @@ typedef enum { /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_SNM_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h index 244ad1beec..77cd856413 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Provides status codes for the AFBR-S50 API. + * @brief This file is part of the AFBR-S50 API. + * @details Provides status codes for the AFBR-S50 API. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,25 +36,32 @@ #ifndef ARGUS_STATUS_H #define ARGUS_STATUS_H +#ifdef __cplusplus +extern "C" { +#endif #include /*!*************************************************************************** - * @defgroup status Status Codes + * @defgroup argus_status Status Codes + * @ingroup argus + * * @brief Status and Error Code Definitions + * * @details Defines status and error codes for function return values. * Basic status number structure: * - 0 is OK or no error. * - negative values determine errors. * - positive values determine warnings or status information. * . - * @addtogroup status + * + * @addtogroup argus_status * @{ *****************************************************************************/ /*!*************************************************************************** - * @brief Type used for all status and error return values. - * @details Basic status number structure: + * @brief Type used for all status and error return values. + * @details Basic status number structure: * - 0 is OK or no error. * - negative values determine errors. * - positive values determine warnings or status information. @@ -138,8 +145,8 @@ enum Status { ********** NVM / Flash Layer Status ********************************************************* *********************************************************************************************/ - /*! -98: Flash Error: The version of the settings in the flash memory is not compatible. */ - ERROR_NVM_INVALID_FILE_VERSION = -98, + /*! -98: Flash Error: The read memory block was not written previously and contains no data. */ + ERROR_NVM_EMPTY = -98, /*! -99: Flash Error: The memory is out of range. */ ERROR_NVM_OUT_OF_RANGE = -99, @@ -183,6 +190,13 @@ enum Status { /*! -102: AFBR-S50 Error: Inconsistent configuration parameters. */ ERROR_ARGUS_INVALID_CFG = -102, + /*! -103: AFBR-S50 Error: The evaluation function has been called but no + * raw data is available yet. + * See also #Argus_EvaluateData for more information. */ + ERROR_ARGUS_BUFFER_EMPTY = -103, + + /*! -104: AFBR-S50 Error: Invalid slave identifier is passed to the module. */ + ERROR_ARGUS_INVALID_SLAVE = -104, /*! -105: AFBR-S50 Error: Invalid measurement mode configuration parameter. */ ERROR_ARGUS_INVALID_MODE = -105, @@ -191,7 +205,6 @@ enum Status { * The current measurement data set is invalid! */ ERROR_ARGUS_BIAS_VOLTAGE_REINIT = -107, - /*! -109: AFBR-S50 Error: The EEPROM readout has failed. The failure is detected * by three distinct read attempts, each resulting in invalid data. * Note: this state differs from that #STATUS_ARGUS_EEPROM_BIT_ERROR @@ -224,7 +237,6 @@ enum Status { * requested command. */ ERROR_ARGUS_BUSY = -191, - /*! -199: AFBR-S50 Error: Unknown module number. */ ERROR_ARGUS_UNKNOWN_MODULE = -199, @@ -235,24 +247,22 @@ enum Status { ERROR_ARGUS_UNKNOWN_LASER = -197, + /*! 191: AFBR-S50 Status (internal): The device is currently busy with testing the + * SPI connection to the device. */ + STATUS_ARGUS_BUSY_TEST = 191, - /*! 193: AFBR-S50 Status (internal): The device is currently busy with updating the - * configuration (i.e. with writing register values). */ - STATUS_ARGUS_BUSY_CFG_UPDATE = 193, - - /*! 194: AFBR-S50 Status (internal): The device is currently busy with updating the - * calibration data (i.e. writing to register values). */ - STATUS_ARGUS_BUSY_CAL_UPDATE = 194, + /*! 192: AFBR-S50 Status (internal): The device is currently busy with updating the + * settings parameter (i.e. with writing register values). */ + STATUS_ARGUS_BUSY_UPDATE = 192, /*! 195: AFBR-S50 Status (internal): The device is currently executing a calibration - * sequence. */ + * sequence. */ STATUS_ARGUS_BUSY_CAL_SEQ = 195, /*! 196: AFBR-S50 Status (internal): The device is currently executing a measurement * cycle. */ STATUS_ARGUS_BUSY_MEAS = 196, - /*! 100: AFBR-S50 Status (internal): The ASIC is initializing a new measurement, i.e. * a register value is written that starts an integration cycle on the ASIC. */ STATUS_ARGUS_STARTING = 100, @@ -260,9 +270,10 @@ enum Status { /*! 103: AFBR-S50 Status (internal): The ASIC is performing an integration cycle. */ STATUS_ARGUS_ACTIVE = 103, - - }; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_STATUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h index ea16342c84..a1a2d878ac 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file contains the current API version number. + * @brief This file is part of the AFBR-S50 API. + * @details This file contains the current API version number. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,16 +36,19 @@ #ifndef ARGUS_VERSION_H #define ARGUS_VERSION_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup version API Version - * @ingroup argusapi + * @defgroup argus_version API Version + * @ingroup argus_api * - * @brief API and library core version number + * @brief API and library core version number * - * @details Contains the AFBR-S50 API and Library Core Version Number. + * @details Contains the AFBR-S50 API and Library Core Version Number. * - * @addtogroup version + * @addtogroup argus_version * @{ *****************************************************************************/ @@ -53,13 +56,13 @@ #define ARGUS_API_VERSION_MAJOR 1 /*! Minor version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_MINOR 3 +#define ARGUS_API_VERSION_MINOR 4 /*! Bugfix version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_BUGFIX 5 +#define ARGUS_API_VERSION_BUGFIX 4 -/*! Build version nunber of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_BUILD "20210812171515" +/*! Build version number of the AFBR-S50 API. */ +#define ARGUS_API_VERSION_BUILD "20230327150535" /*****************************************************************************/ @@ -73,4 +76,7 @@ (ARGUS_API_VERSION_BUGFIX)) /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_VERSION_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h index 5613706267..6f3d40b49a 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 hardware API. - * @details Defines the generic device calibration API. + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic device calibration API. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,44 +36,71 @@ #ifndef ARGUS_XTALK_H #define ARGUS_XTALK_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @addtogroup arguscal + * @addtogroup argus_cal * @{ *****************************************************************************/ -#include "api/argus_def.h" +#include "argus_def.h" +#include "argus_dfm.h" /*!*************************************************************************** - * @brief Pixel Crosstalk Compensation Vector. - * @details Contains calibration data (per pixel) that belongs to the - * RX-TX-Crosstalk compensation feature. + * @brief Pixel Crosstalk Compensation Vector. + * @details Contains calibration data (per pixel) that belongs to the + * RX-TX-Crosstalk compensation feature. + * The crosstalk vector consists of a Sine and Cosine component in LSB. *****************************************************************************/ - -/*! Pixel Crosstalk Vector */ -typedef struct { +typedef struct xtalk_t { /*! Crosstalk Vector - Sine component. + * Units: LSB * Special Value: Q11_4_MIN == not available */ q11_4_t dS; /*! Crosstalk Vector - Cosine component. + * Units: LSB * Special Value: Q11_4_MIN == not available */ q11_4_t dC; } xtalk_t; /*!*************************************************************************** - * @brief Pixel-To-Pixel Crosstalk Compensation Parameters. - * @details Contains calibration data that belongs to the pixel-to-pixel - * crosstalk compensation feature. + * @brief Pixel Crosstalk Vector Table. + * @details Contains crosstalk vector values for all 32 active pixels, + * separated for A/B-Frames. *****************************************************************************/ -typedef struct { +typedef struct argus_cal_xtalk_table_t { + union { + struct { + /*! The crosstalk vector table for A-Frames. */ + xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + + /*! The crosstalk vector table for B-Frames. */ + xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + }; + + /*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/ + xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + }; + +} argus_cal_xtalk_table_t; + + +/*!*************************************************************************** + * @brief Pixel-To-Pixel Crosstalk Compensation Parameters. + * @details Contains calibration data that belongs to the pixel-to-pixel + * crosstalk compensation feature. + *****************************************************************************/ +typedef struct argus_cal_p2pxtalk_t { /*! Pixel-To-Pixel Compensation on/off. */ bool Enabled; /*! The relative threshold determines when the compensation is active for * each individual pixel. The value determines the ratio of the individual - * pixel signal is with respect to the overall average signal. If the + * pixel signal with respect to the overall average signal. If the * ratio is smaller than the value, the compensation is active. Absolute * and relative conditions are connected with AND logic. */ uq0_8_t RelativeThreshold; @@ -111,4 +138,7 @@ typedef struct { /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_XTALK_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h index dcea881d02..79cf0ede58 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file the main header of the AFBR-S50 API. + * @brief This file is part of the AFBR-S50 API. + * @details This file the main header of the AFBR-S50 API. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,7 +36,6 @@ #ifndef ARGUS_H #define ARGUS_H - #ifdef __cplusplus extern "C" { #endif @@ -44,7 +43,6 @@ extern "C" { #include "api/argus_api.h" #ifdef __cplusplus -} +} // extern "C" #endif - #endif /* ARGUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h index 3eb7d2cfd4..d55097cd8a 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for enabling/disabling interrupts. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for enabling/disabling interrupts. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,96 +36,94 @@ #ifndef ARGUS_IRQ_H #define ARGUS_IRQ_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_irq IRQ: Global Interrupt Control Layer - * @ingroup argus_platform + * @defgroup argus_irq IRQ: Global Interrupt Control Layer + * @ingroup argus_hal * - * @brief Global Interrupt Control Layer + * @brief Global Interrupt Control Layer * - * @details This module provides functionality to globally enable/disable - * interrupts in a nested way. + * @details This module provides functionality to globally enable/disable + * interrupts in a nested way. * - * Here is a simple example implementation using the CMSIS functions - * "__enable_irq()" and "__disable_irq()". An integer counter is - * used to achieve nested interrupt disabling: + * Here is a simple example implementation using the CMSIS functions + * "__enable_irq()" and "__disable_irq()". An integer counter is + * used to achieve nested interrupt disabling: * - * @code + * @code * - * // Global lock level counter value. - * static volatile int g_irq_lock_ct; + * // Global lock level counter value. + * static volatile int g_irq_lock_ct; * - * // Global unlock all interrupts using CMSIS function "__enable_irq()". - * void IRQ_UNLOCK(void) - * { - * assert(g_irq_lock_ct > 0); - * if (--g_irq_lock_ct <= 0) - * { - * g_irq_lock_ct = 0; - * __enable_irq(); - * } - * } + * // Global unlock all interrupts using CMSIS function "__enable_irq()". + * void IRQ_UNLOCK(void) + * { + * assert(g_irq_lock_ct > 0); + * if (--g_irq_lock_ct <= 0) + * { + * g_irq_lock_ct = 0; + * __enable_irq(); + * } + * } * - * // Global lock all interrupts using CMSIS function "__disable_irq()". - * void IRQ_LOCK(void) - * { - * __disable_irq(); - * g_irq_lock_ct++; - * } + * // Global lock all interrupts using CMSIS function "__disable_irq()". + * void IRQ_LOCK(void) + * { + * __disable_irq(); + * g_irq_lock_ct++; + * } * - * @endcode + * @endcode * - * @note The IRQ locking mechanism is used to create atomic sections - * (within the scope of the AFBR-S50 API) that are very few processor - * instruction only. It does NOT lock interrupts for considerable - * amounts of time. + * @note The IRQ locking mechanism is used to create atomic sections + * (within the scope of the AFBR-S50 API) that are very few processor + * instruction only. It does NOT lock interrupts for considerable + * amounts of time. * - * @note The IRQ_LOCK might get called multiple times. Therefore, the - * API expects that the IRQ_UNLOCK must be called as many times as - * the IRQ_LOCK was called before the interrupts are enabled. + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. * - * @note The interrupts utilized by the AFBR-S50 API can be interrupted - * by other, higher prioritized interrupts, e.g. some system - * critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK - * mechanism can be implemented such that only the interrupts - * required for the AFBR-S50 API are locked. The above example is - * dedicated to a ARM Corex-M0 architecture, where interrupts - * can only disabled at a global scope. Other architectures like - * ARM Cortex-M4 allow selective disabling of interrupts. + * @note The interrupts utilized by the AFBR-S50 API can be interrupted + * by other, higher prioritized interrupts, e.g. some system + * critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK + * mechanism can be implemented such that only the interrupts + * required for the AFBR-S50 API are locked. The above example is + * dedicated to a ARM Corex-M0 architecture, where interrupts + * can only disabled at a global scope. Other architectures like + * ARM Cortex-M4 allow selective disabling of interrupts. * - * @addtogroup argus_irq + * @addtogroup argus_irq * @{ *****************************************************************************/ /*!*************************************************************************** - * @brief Enable IRQ Interrupts + * @brief Enable IRQ Interrupts * - * @details Enables IRQ interrupts and enters an atomic or critical section. + * @details Enables IRQ interrupts and enters an atomic or critical section. * - * @note The IRQ_LOCK might get called multiple times. Therefore, the - * API expects that the IRQ_UNLOCK must be called as many times as - * the IRQ_LOCK was called before the interrupts are enabled. + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. *****************************************************************************/ void IRQ_UNLOCK(void); /*!*************************************************************************** - * @brief Disable IRQ Interrupts + * @brief Disable IRQ Interrupts * - * @details Disables IRQ interrupts and leaves the atomic or critical section. + * @details Disables IRQ interrupts and leaves the atomic or critical section. * - * @note The IRQ_LOCK might get called multiple times. Therefore, the - * API expects that the IRQ_UNLOCK must be called as many times as - * the IRQ_LOCK was called before the interrupts are enabled. + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. *****************************************************************************/ void IRQ_LOCK(void); -#ifdef __cplusplus -} -#endif - /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif // ARGUS_IRQ_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h index 69939b7759..b8150f0281 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the optional non-volatile memory. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the optional non-volatile memory. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,99 +36,113 @@ #ifndef ARGUS_NVM_H #define ARGUS_NVM_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_nvm NVM: Non-Volatile Memory Layer - * @ingroup argus_platform + * @defgroup argus_nvm NVM: Non-Volatile Memory Layer + * @ingroup argus_hal * - * @brief Non-Volatile Memory Layer + * @brief Non-Volatile Memory Layer * - * @details This module provides functionality to access the non-volatile - * memory (e.g. flash) on the underlying platform. + * @details This module provides functionality to access the non-volatile + * memory (e.g. flash) on the underlying platform. * - * This module is optional and only required if calibration data - * needs to be stored within the API. + * This module is optional and only required if calibration data + * needs to be stored within the API. * - * @note The implementation of this module is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. + * @note The implementation of this module is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. * - * @addtogroup argus_nvm + * @addtogroup argus_nvm * @{ *****************************************************************************/ -#include "argus.h" +#include "api/argus_def.h" + +/*! The NVM block size in the non-volatile memory. */ +#define ARGUS_NVM_BLOCK_SIZE 0x300 // 768 bytes /*!*************************************************************************** - * @brief Initializes the non-volatile memory unit and reserves a chunk of memory. + * @brief Write a block of data to the non-volatile memory. * - * @details The function is called upon API initialization sequence. If available, - * the non-volatile memory module reserves a chunk of memory with the - * provides number of bytes (size) and returns with #STATUS_OK. + * @details The function is called whenever the API wants to write data into + * non-volatile memory, e.g. flash. Later, the API reads the written + * data via the #NVM_ReadBlock function. * - * If not implemented, the function should return #ERROR_NOT_IMPLEMENTED - * in oder to inform the API to not use the NVM module. + * The data shall be written to a specified memory block that is + * uniquely dedicated to each individual device. The /p id parameter + * is passed to the function that identifies the device. The /p id + * is composed of the device ID and module type, i.e. it is unique + * among all devices. If only a single device is used anyway, the + * /p id parameter can be ignored. * - * After initialization, the API calls the #NVM_Write and #NVM_Read - * methods to write within the reserved chunk of memory. + * If no NVM module is available, the function can return with error + * #ERROR_NOT_IMPLEMENTED and the API ignores the NVM. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. + * If write fails, e.g. due to lack of memory, a negative status + * must be returned, e.g. #ERROR_NVM_OUT_OF_RANGE. * - * @param size The required size of NVM to store all parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * The block size is fixed for a single device. The actual block size + * is defined with #ARGUS_NVM_BLOCK_SIZE. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param id The 32-bit ID number to identify the corresponding memory block. + * @param block_size The number of bytes to be written. Note that this value + * is fixed, i.e. the API always writes the same data size. + * The size is defined here: #ARGUS_NVM_BLOCK_SIZE. + * @param buf The pointer to the data buffer of size /p block_size that needs + * to be written to the NVM. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t NVM_Init(uint32_t size); +status_t NVM_WriteBlock(uint32_t id, uint32_t block_size, uint8_t const *buf); /*!*************************************************************************** - * @brief Write a block of data to the non-volatile memory. + * @brief Reads a block of data from the non-volatile memory. * - * @details The function is called whenever the API wants to write data into - * the previously reserved (#NVM_Init) memory block. The data shall - * be written at a given offset and with a given size. + * @details The function is called whenever the API wants to read data from + * non-volatile memory, e.g. flash. The data will be previously + * stored using the #NVM_WriteBlock function. Otherwise, the function + * must return a corresponding error code, namely #ERROR_NVM_EMPTY. * - * If no NVM module is available, the function can return with error - * #ERROR_NOT_IMPLEMENTED. + * The data shall be read from a specified memory block that is + * uniquely dedicated to each individual device. The /p id parameter + * is passed to the function that identifies the device. The /p id + * is composed of the device ID and module type, i.e. it is unique + * among all devices. If only a single device is used anyway, the + * /p id parameter can be ignored. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. + * If no NVM module is available, the function can return with error + * #ERROR_NOT_IMPLEMENTED and the API ignores the NVM. * - * @param offset The index offset where the first byte needs to be written. - * @param size The number of bytes to be written. - * @param buf The pointer to the data buffer with the data to be written. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * If read fails, e.g. if data has not been written previously, + * a negative status must be returned, e.g. #ERROR_NVM_EMPTY if no + * data has been written yet or any other negative error else-wise. + * + * The block size is fixed for a single device. The actual block size + * is defined with #ARGUS_NVM_BLOCK_SIZE. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param id The 32-bit ID number to identify the corresponding memory block. + * @param block_size The number of bytes to be read. Note that this value + * is fixed, i.e. the API always reads the same data size. + * The size is defined here: #ARGUS_NVM_BLOCK_SIZE. + * @param buf The pointer to the data buffer of size /p block_size to copy + * the data to. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t NVM_Write(uint32_t offset, uint32_t size, uint8_t const *buf); - -/*!*************************************************************************** - * @brief Reads a block of data from the non-volatile memory. - * - * @details The function is called whenever the API wants to read data from - * the previously reserved (#NVM_Init) memory block. The data shall - * be read at a given offset and with a given size. - * - * If no NVM module is available, the function can return with error - * #ERROR_NOT_IMPLEMENTED. - * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. - * - * @param offset The index offset where the first byte needs to be read. - * @param size The number of bytes to be read. - * @param buf The pointer to the data buffer to copy the data to. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t NVM_Read(uint32_t offset, uint32_t size, uint8_t *buf); -#ifdef __cplusplus -} -#endif +status_t NVM_ReadBlock(uint32_t id, uint32_t block_size, uint8_t *buf); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif // ARGUS_NVM_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h index 0ca49f858c..9a76dbe485 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the optional debug module. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the optional debug module. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,48 +36,55 @@ #ifndef ARGUS_PRINT_H #define ARGUS_PRINT_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argus_log Debug: Logging Interface - * @ingroup argus_platform + * @defgroup argus_log Debug: Logging Interface + * @ingroup argus_hal * - * @brief Logging interface for the AFBR-S50 API. + * @brief Logging interface for the AFBR-S50 API. * - * @details This interface provides logging utility functions. - * Defines a printf-like function that is used to print error and - * log messages. + * @details This interface provides logging utility functions. + * Defines a printf-like function that is used to print error and + * log messages. * - * @addtogroup argus_log + * @addtogroup argus_log * @{ *****************************************************************************/ #include "api/argus_def.h" /*!*************************************************************************** - * @brief A printf-like function to print formatted data to an debugging interface. + * @brief A printf-like function to print formatted data to an debugging interface. * * @details Writes the C string pointed by fmt_t to an output. If format - * includes format specifiers (subsequences beginning with %), the - * additional arguments following fmt_t are formatted and inserted in - * the resulting string replacing their respective specifiers. + * includes format specifiers (subsequences beginning with %), the + * additional arguments following fmt_t are formatted and inserted in + * the resulting string replacing their respective specifiers. * - * To enable the print functionality, an implementation of the function - * must be provided that maps the output to an interface like UART or - * a debugging console, e.g. by forwarding to standard printf() method. + * To enable the print functionality, an implementation of the function + * must be provided that maps the output to an interface like UART or + * a debugging console, e.g. by forwarding to standard printf() method. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that does nothing. This will improve - * the performance but no error messages are logged. + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that does nothing. This will improve + * the performance but no error messages are logged. * - * @note The naming is different from the standard printf() on purpose to - * prevent builtin compiler optimizations. + * @note The naming is different from the standard printf() on purpose to + * prevent builtin compiler optimizations. * - * @param fmt_s The usual print() format string. - * @param ... The usual print() parameters. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param fmt_s The usual print() format string. + * @param ... The usual print() parameters. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t print(const char *fmt_s, ...); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_PRINT_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h index 4b8f5331b5..8d912afc22 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the required S2PI module. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required S2PI module. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,95 +36,94 @@ #ifndef ARGUS_S2PI_H #define ARGUS_S2PI_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_s2pi S2PI: Serial Peripheral Interface - * @ingroup argus_platform + * @defgroup argus_s2pi S2PI: Serial Peripheral Interface + * @ingroup argus_hal * - * @brief S2PI: SPI incl. GPIO Hardware Layer Module + * @brief S2PI: SPI incl. GPIO Hardware Layer Module * - * @details The S2PI module consists of a standard SPI interface plus a - * single GPIO interrupt line. Furthermore, the SPI pins are - * accessible via GPIO control to allow a software emulation of - * additional protocols using the same pins. + * @details The S2PI module consists of a standard SPI interface plus a + * single GPIO interrupt line. Furthermore, the SPI pins are + * accessible via GPIO control to allow a software emulation of + * additional protocols using the same pins. * - * **SPI interface:** + * **SPI interface:** * - * The SPI interface is based around a single functionality: + * The SPI interface is based around a single functionality: * - * #S2PI_TransferFrame. This function transfers a specified number - * of bytes via the interfaces MOSI line and simultaneously reads - * the incoming data on the MOSI line. The read can also be skipped. - * The transfer happen asynchronously, e.g. via a DMA request. After - * finishing the transfer, the provided callback is invoked with - * the status of the transfer and the provided abstract parameter. - * Furthermore, the functions receives a slave parameter that can - * be used to connect multiple slaves, each with its individual - * chip select line. + * #S2PI_TransferFrame. This function transfers a specified number + * of bytes via the interfaces MOSI line and simultaneously reads + * the incoming data on the MOSI line. The read can also be skipped. + * The transfer happen asynchronously, e.g. via a DMA request. After + * finishing the transfer, the provided callback is invoked with + * the status of the transfer and the provided abstract parameter. + * Furthermore, the functions receives a slave parameter that can + * be used to connect multiple slaves, each with its individual + * chip select line. * - * The interface also provides functionality to change the SPI - * baud rate. An additional abort method is used to cancel the - * ongoing transfer. + * The interface also provides functionality to change the SPI + * baud rate. An additional abort method is used to cancel the + * ongoing transfer. * - * **GPIO interface:** + * **GPIO interface:** * - * The GPIO part of the S2PI interface has two distinct concerns: + * The GPIO part of the S2PI interface has two distinct concerns: * - * First, the GPIO interface handles the measurement finished interrupt - * from the device. When the device invokes the interrupt, it pulls - * the interrupt line to low. Thus the interrupt must trigger when - * a transition from high to low occurs on the interrupt line. + * First, the GPIO interface handles the measurement finished interrupt + * from the device. When the device invokes the interrupt, it pulls + * the interrupt line to low. Thus the interrupt must trigger when + * a transition from high to low occurs on the interrupt line. * - * The module simply invokes a callback when this interrupt occurs. - * The #S2PI_SetIrqCallback method is used to install the callback - * for a specified slave. Each slave will have its own interrupt - * line. An additional callback parameter can be set that would be - * passed to the callback function. + * The module simply invokes a callback when this interrupt occurs. + * The #S2PI_SetIrqCallback method is used to install the callback + * for a specified slave. Each slave will have its own interrupt + * line. An additional callback parameter can be set that would be + * passed to the callback function. * - * In addition to the interrupt, all SPI pins need to be accessible - * as GPIO pins through this interface. This is required to read - * the EEPROM memory on the device hat is connected to the SPI - * pins but requires a different protocol that is not compatible - * to any standard SPI interface. Therefore, the interface provides - * the possibility to switch to GPIO control mode that allows to - * emulate the EEPROM protocol via software bit banging. + * In addition to the interrupt, all SPI pins need to be accessible + * as GPIO pins through this interface. This is required to read + * the EEPROM memory on the device hat is connected to the SPI + * pins but requires a different protocol that is not compatible + * to any standard SPI interface. Therefore, the interface provides + * the possibility to switch to GPIO control mode that allows to + * emulate the EEPROM protocol via software bit banging. * - * Two methods are provided to switch forth and back between SPI - * and GPIO control. In GPIO mode, several functions are used to - * read and write the individual GPIO pins. + * Two methods are provided to switch forth and back between SPI + * and GPIO control. In GPIO mode, several functions are used to + * read and write the individual GPIO pins. * - * Note that the GPIO mode is only required to readout the EEPROM - * upon initialization of the device, i.e. during execution of the - * #Argus_Init or #Argus_Reinit methods. The GPIO mode is not used - * during measurements. + * Note that the GPIO mode is only required to readout the EEPROM + * upon initialization of the device, i.e. during execution of the + * #Argus_Init or #Argus_Reinit methods. The GPIO mode is not used + * during measurements. * * - * @addtogroup argus_s2pi + * @addtogroup argus_s2pi * @{ *****************************************************************************/ #include "api/argus_def.h" /*!*************************************************************************** - * @brief S2PI layer callback function type for the SPI transfer completed event. + * @brief S2PI layer callback function type for the SPI transfer completed event. * - * @param status The \link #status_t status\endlink of the completed + * @param status The \link #status_t status\endlink of the completed * transfer (#STATUS_OK on success). * - * @param param The provided (optional, can be null) callback parameter. + * @param param The provided (optional, can be null) callback parameter. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ typedef status_t (*s2pi_callback_t)(status_t status, void *param); /*!*************************************************************************** - * @brief S2PI layer callback function type for the GPIO interrupt event. + * @brief S2PI layer callback function type for the GPIO interrupt event. * - * @param param The provided (optional, can be null) callback parameter. + * @param param The provided (optional, can be null) callback parameter. *****************************************************************************/ typedef void (*s2pi_irq_callback_t)(void *param); @@ -132,8 +131,8 @@ typedef void (*s2pi_irq_callback_t)(void *param); * can be used to identify the slave within the SPI module. */ typedef int32_t s2pi_slave_t; -/*! The enumeration of S2PI pins. */ -typedef enum { +/*! The enumeration of S2PI pins. */ +typedef enum s2pi_pin_t { /*! The SPI clock pin. */ S2PI_CLK, @@ -153,64 +152,141 @@ typedef enum { /*!*************************************************************************** - * @brief Returns the status of the SPI module. + * @brief Returns the status of the SPI module. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing. - * - #STATUS_BUSY: An SPI transfer is in progress. - * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing. + * - #STATUS_BUSY: An SPI transfer is in progress. + * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. *****************************************************************************/ -status_t S2PI_GetStatus(void); +status_t S2PI_GetStatus(s2pi_slave_t slave); /*!*************************************************************************** - * @brief Transfers a single SPI frame asynchronously. + * @brief Tries to grab the SPI interface mutex for the next transfer. + * + * @details This mutex prevents new asynchronous SPI requests to interfere + * with transfers already in progress for this interface. + * + * Note that this is only required if multiple device are connected to + * a single SPI interface. If only operating a single device per SPI, + * the function can simply always return #STATUS_OK. + * + * There must be a dedicated mutex object per SPI interface if + * multiple SPI interfaces are used. + * + * The mutex will be released in the #S2PI_ReleaseMutex function. + * See #S2PI_ReleaseMutex for additional information. + * + * Here is a simple example implementation for the multiple devices on + * a single SPI interface case. Note that the SpiMutexBlocked must be + * defined per SPI interface if multiple SPI interfaces are used. + * + * @code + * static volatile bool SpiMutexBlocked = false; + * status_t S2PI_TryGetMutex(s2pi_slave_t slave) + * { + * (void) slave; // not used in this implementation as all + * // SPI slaves are on the same SPI interface + * + * status_t status = STATUS_BUSY; + * IRQ_LOCK(); + * if (!SpiMutexBlocked) + * { + * SpiMutexBlocked = true; + * status = STATUS_OK; + * } + * IRQ_UNLOCK(); + * return status; + * } + * void S2PI_ReleaseMutex(s2pi_slave_t slave) + * { + * (void) slave; // not used in this implementation + * SpiMutexBlocked = false; + * } + * @endcode + * + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: the SPI interface was successfully reserved for the caller + * - #STATUS_BUSY: another transfer is ongoing, the caller must not access the bus + *****************************************************************************/ +status_t S2PI_TryGetMutex(s2pi_slave_t slave); + +/*!*************************************************************************** + * @brief Releases the SPI interface mutex. + * + * @details Once the mutex is captured, only a single thread (the one that + * captured it) will call this release function, so there is no + * need for any test or thread safe barriers. Also there is no + * side effect of calling this function when the Mutex is not + * taken so this function can be really simple and doesn't need + * to return anything. + * + * See #S2PI_TryGetMutex on more information and an example + * implementation for the single SPI interface case. + * + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + *****************************************************************************/ +void S2PI_ReleaseMutex(s2pi_slave_t slave); + +/*!*************************************************************************** + * @brief Transfers a single SPI frame asynchronously. * * @details Transfers a single SPI frame in asynchronous manner. The Tx data - * buffer is written to the device via the MOSI line. - * Optionally, the data on the MISO line is written to the provided - * Rx data buffer. If null, the read data is dismissed. Note that - * Rx and Tx buffer can be identical. I.e. the same buffer is used - * for writing and reading data. First, a byte is transmitted and - * the received byte overwrites the previously send value. + * buffer is written to the device via the MOSI line. + * Optionally, the data on the MISO line is written to the provided + * Rx data buffer. If null, the read data is dismissed. Note that + * Rx and Tx buffer can be identical. I.e. the same buffer is used + * for writing and reading data. First, a byte is transmitted and + * the received byte overwrites the previously send value. * - * The transfer of a single frame requires to not toggle the chip - * select line to high in between the data frame. The maximum - * number of bytes transfered in a single SPI transfer is given by - * the data value register of the device, which is 396 data bytes - * plus a single address byte: 397 bytes. + * The transfer of a single frame requires to not toggle the chip + * select line to high in between the data frame. The maximum + * number of bytes transferred in a single SPI transfer is given by + * the data value register of the device, which is 396 data bytes + * plus a single address byte: 397 bytes. * - * An optional callback is invoked when the asynchronous transfer - * is finished. If the \p callback parameter is a null pointer, - * no callback is provided. Note that the provided buffer must not - * change while the transfer is ongoing. + * An optional callback is invoked when the asynchronous transfer + * is finished. If the \p callback parameter is a null pointer, + * no callback is provided. Note that the provided buffer must not + * change while the transfer is ongoing. * - * Use the slave parameter to determine the corresponding slave via the - * given chip select line. + * Use the slave parameter to determine the corresponding slave via the + * given chip select line. * - * Usually, two distinct interrupts are required to handle the RX and - * TX ready events. The callback must be invoked from whichever - * interrupt comes after the SPI transfer has been finished. Note - * that new SPI transfers are invoked from within the callback function - * (i.e. from within the interrupt service routine of same priority). + * Usually, two distinct interrupts are required to handle the RX and + * TX ready events. The callback must be invoked from whichever + * interrupt comes after the SPI transfer has been finished. Note + * that new SPI transfers are invoked from within the callback function + * (i.e. from within the interrupt service routine of same priority). * - * @param slave The specified S2PI slave. - * @param txData The 8-bit values to write to the SPI bus MOSI line. - * @param rxData The 8-bit values received from the SPI bus MISO line + * @param slave The specified S2PI slave. + * @param txData The 8-bit values to write to the SPI bus MOSI line. + * @param rxData The 8-bit values received from the SPI bus MISO line * (pass a null pointer if the data don't need to be read). - * @param frameSize The number of 8-bit values to be sent/received. - * @param callback A callback function to be invoked when the transfer is - * finished. Pass a null pointer if no callback is required. - * @param callbackData A pointer to a state that will be passed to the + * @param frameSize The number of 8-bit values to be sent/received. + * @param callback A callback function to be invoked when the transfer is + * finished. Pass a null pointer if no callback is required. + * @param callbackData A pointer to a state that will be passed to the * callback. Pass a null pointer if not used. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK: Successfully invoked the transfer. - * - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed. - * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. - * - #STATUS_BUSY: An SPI transfer is already in progress. The - * transfer was not started. - * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer - * was not started. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: Successfully invoked the transfer. + * - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed. + * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. + * - #STATUS_BUSY: An SPI transfer is already in progress. The + * transfer was not started. + * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer + * was not started. *****************************************************************************/ status_t S2PI_TransferFrame(s2pi_slave_t slave, uint8_t const *txData, @@ -220,136 +296,158 @@ status_t S2PI_TransferFrame(s2pi_slave_t slave, void *callbackData); /*!*************************************************************************** - * @brief Terminates a currently ongoing asynchronous SPI transfer. + * @brief Terminates a currently ongoing asynchronous SPI transfer. * - * @details When a callback is set for the current ongoing activity, it is - * invoked with the #ERROR_ABORTED error byte. + * @details When a callback is set for the current ongoing activity, it is + * invoked with the #ERROR_ABORTED error byte. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_Abort(void); +status_t S2PI_Abort(s2pi_slave_t slave); /*!*************************************************************************** - * @brief Set a callback for the GPIO IRQ for a specified S2PI slave. + * @brief Set a callback for the GPIO IRQ for a specified S2PI slave. * - * @param slave The specified S2PI slave. - * @param callback A callback function to be invoked when the specified - * S2PI slave IRQ occurs. Pass a null pointer to disable - * the callback. - * @param callbackData A pointer to a state that will be passed to the + * @param slave The specified S2PI slave. + * @param callback A callback function to be invoked when the specified + * S2PI slave IRQ occurs. Pass a null pointer to disable + * the callback. + * @param callbackData A pointer to a state that will be passed to the * callback. Pass a null pointer if not used. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK: Successfully installation of the callback. - * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: Successfully installation of the callback. + * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. *****************************************************************************/ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData); /*!*************************************************************************** - * @brief Reads the current status of the IRQ pin. + * @brief Reads the current interrupt pending status of the IRQ pin. * * @details In order to keep a low priority for GPIO IRQs, the state of the - * IRQ pin must be read in order to reliable check for chip timeouts. + * IRQ pin must be read in order to reliable check for chip timeouts. * - * The execution of the interrupt service routine for the data-ready - * interrupt from the corresponding GPIO pin might be delayed due to - * priority issues. The delayed execution might disable the timeout - * for the eye-safety checker too late causing false error messages. - * In order to overcome the issue, the state of the IRQ GPIO input - * pin is read before raising a timeout error in order to check if - * the device has already finished but the IRQ is still pending to be - * executed! + * The execution of the interrupt service routine for the data-ready + * interrupt from the corresponding GPIO pin might be delayed due to + * priority issues. The delayed execution might disable the timeout + * for the eye-safety checker too late causing false error messages. + * In order to overcome the issue, the interrupt state of the IRQ + * GPIO input pin is read before raising a timeout error in order to + * check if the device has already finished and the IRQ is still + * pending to be executed! + * + * Note: an easy implementation is to simply return the state of the + * IRQ line, i.e. 0 if there is a low input state and 1 if there is + * a high input state on the IRQ input pin. However, this + * implementation is not fully reliable since the GPIO interrupt + * (triggered on the falling edge) might be missed and the callback + * is never invoked while the IRQ line is correctly asserted to low + * state by the device. In that case, the API is waiting forever + * until the callback is invoked which might never happen. Therefore, + * it is better if the implementation checks the state of the IRQ + * pending status register or even combines both variations. - * @param slave The specified S2PI slave. - * @return Returns 1U if the IRQ pin is high (IRQ not pending) and 0U if the - * devices pulls the pin to low state (IRQ pending). + * @param slave The specified S2PI slave. + * + * @return Returns 1U if the IRQ is NOT pending (pin is in high state) and + * 0U if the IRQ is pending (pin is pulled to low state by the device). *****************************************************************************/ uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave); /*!*************************************************************************** - * @brief Cycles the chip select line. + * @brief Cycles the chip select line. * * @details In order to cancel the integration on the ASIC, a fast toggling - * of the chip select pin of the corresponding SPI slave is required. - * Therefore, this function toggles the CS from high to low and back. - * The SPI instance for the specified S2PI slave must be idle, - * otherwise the status #STATUS_BUSY is returned. + * of the chip select pin of the corresponding SPI slave is required. + * Therefore, this function toggles the CS from high to low and back. + * The SPI instance for the specified S2PI slave must be idle, + * otherwise the status #STATUS_BUSY is returned. * - * @param slave The specified S2PI slave. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t S2PI_CycleCsPin(s2pi_slave_t slave); - - /*!***************************************************************************** - * @brief Captures the S2PI pins for GPIO usage. + * @brief Captures the S2PI pins for GPIO usage. * - * @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the - * pins are configured for GPIO operation. The GPIO control must be - * release with the #S2PI_ReleaseGpioControl function in order to - * switch back to ordinary SPI functionality. + * @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the + * pins are configured for GPIO operation. The GPIO control must be + * release with the #S2PI_ReleaseGpioControl function in order to + * switch back to ordinary SPI functionality. * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_CaptureGpioControl(void); +status_t S2PI_CaptureGpioControl(s2pi_slave_t slave); /*!***************************************************************************** - * @brief Releases the S2PI pins from GPIO usage and switches back to SPI mode. + * @brief Releases the S2PI pins from GPIO usage and switches back to SPI mode. * - * @details The GPIO pins are configured for SPI operation and the GPIO mode is - * left. Must be called if the pins are captured for GPIO operation via - * the #S2PI_CaptureGpioControl function. + * @details The GPIO pins are configured for SPI operation and the GPIO mode is + * left. Must be called if the pins are captured for GPIO operation via + * the #S2PI_CaptureGpioControl function. * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_ReleaseGpioControl(void); +status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave); /*!***************************************************************************** - * @brief Writes the output for a specified SPI pin in GPIO mode. + * @brief Writes the output for a specified SPI pin in GPIO mode. * * @details This function writes the value of an SPI pin if the SPI pins are - * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. + * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. * - * @note Since some GPIO peripherals switch the GPIO pins very fast a delay - * must be added after each GBIO access (i.e. right before returning - * from the #S2PI_WriteGpioPin method) in order to decrease the baud - * rate of the software EEPROM protocol. Increase the delay if timing - * issues occur while reading the EERPOM. For example: - * Delay = 10 µsec => Baud Rate < 100 kHz + * @note Since some GPIO peripherals switch the GPIO pins very fast a delay + * must be added after each GBIO access (i.e. right before returning + * from the #S2PI_WriteGpioPin method) in order to decrease the baud + * rate of the software EEPROM protocol. Increase the delay if timing + * issues occur while reading the EERPOM. For example: + * Delay = 10 µsec => Baud Rate < 100 kHz * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @param slave The specified S2PI slave. - * @param pin The specified S2PI pin. - * @param value The GPIO pin state to write (0 = low, 1 = high). - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. + * @param pin The specified S2PI pin. + * @param value The GPIO pin state to write (0 = low, 1 = high). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value); /*!***************************************************************************** - * @brief Reads the input from a specified SPI pin in GPIO mode. + * @brief Reads the input from a specified SPI pin in GPIO mode. * * @details This function reads the value of an SPI pin if the SPI pins are - * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. + * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @param slave The specified S2PI slave. - * @param pin The specified S2PI pin. - * @param value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level). - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. + * @param pin The specified S2PI pin. + * @param value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value); -#ifdef __cplusplus -} -#endif /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif // ARGUS_S2PI_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h index 80deb42016..b736010682 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the required timer modules. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required timer modules. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,118 +36,117 @@ #ifndef ARGUS_TIMER_H #define ARGUS_TIMER_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_timer Timer: Hardware Timer Interface - * @ingroup argus_platform + * @defgroup argus_timer Timer: Hardware Timer Interface + * @ingroup argus_hal * - * @brief Timer implementations for lifetime counting as well as periodic - * callback. + * @brief Timer implementations for lifetime counting as well as periodic + * callback. * - * @details The module provides an interface to the timing utilities that - * are required by the AFBR-S50 time-of-flight sensor API. + * @details The module provides an interface to the timing utilities that + * are required by the AFBR-S50 time-of-flight sensor API. * - * Two essential features have to be provided by the user code: - * 1. Time Measurement Capability: In order to keep track of outgoing - * signals, the API needs to measure elapsed time. In order to - * provide optimum device performance, the granularity should be - * around 10 to 100 microseconds. - * 2. Periodic Callback: The API provides an automatic starting of - * measurement cycles at a fixed frame rate via a periodic - * interrupt timer. If this feature is not used, implementation - * of the periodic interrupts can be skipped. An weak default - * implementation is provide in the API. - * . + * Two essential features have to be provided by the user code: + * 1. Time Measurement Capability: In order to keep track of outgoing + * signals, the API needs to measure elapsed time. In order to + * provide optimum device performance, the granularity should be + * around 10 to 100 microseconds. + * 2. Periodic Callback: The API provides an automatic starting of + * measurement cycles at a fixed frame rate via a periodic + * interrupt timer. If this feature is not used, implementation + * of the periodic interrupts can be skipped. An weak default + * implementation is provide in the API. + * . * - * The time measurement feature is simply implemented by the function - * #Timer_GetCounterValue. Whenever the function is called, the - * provided counter values must be written with the values obtained - * by the current time. + * The time measurement feature is simply implemented by the function + * #Timer_GetCounterValue. Whenever the function is called, the + * provided counter values must be written with the values obtained + * by the current time. * - * The periodic interrupt timer is a simple callback interface. - * After installing the callback function pointer via #Timer_SetCallback, - * the timer can be started by setting interval via #Timer_SetInterval. - * From then, the callback is invoked periodically as the corresponding - * interval may specify. The timer is stopped by setting the interval - * to 0 using the #Timer_SetInterval function. The interval can be - * updated at any time by updating the interval via the #Timer_SetInterval - * function. To any of these functions, an abstract parameter pointer - * must be passed. This parameter is passed back to the callback any - * time it is invoked. + * The periodic interrupt timer is a simple callback interface. + * After installing the callback function pointer via #Timer_SetCallback, + * the timer can be started by setting interval via #Timer_SetInterval. + * From then, the callback is invoked periodically as the corresponding + * interval may specify. The timer is stopped by setting the interval + * to 0 using the #Timer_SetInterval function. The interval can be + * updated at any time by updating the interval via the #Timer_SetInterval + * function. To any of these functions, an abstract parameter pointer + * must be passed. This parameter is passed back to the callback any + * time it is invoked. * - * In order to provide the usage of multiple devices, an mechanism is - * introduced to allow the installation of multiple callback interval - * at the same time. Therefore, the abstract parameter pointer is used - * to identify the corresponding callback interval. For example, there - * are two callbacks for two intervals, t1 and t2, required. The user - * can start two timers by calling the #Timer_SetInterval method twice, - * but with an individual parameter pointer, ptr1 and ptr2, each: - * \code - * Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1 - * Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1 - * \endcode + * In order to provide the usage of multiple devices, an mechanism is + * introduced to allow the installation of multiple callback interval + * at the same time. Therefore, the abstract parameter pointer is used + * to identify the corresponding callback interval. For example, there + * are two callbacks for two intervals, t1 and t2, required. The user + * can start two timers by calling the #Timer_SetInterval method twice, + * but with an individual parameter pointer, ptr1 and ptr2, each: + * \code + * Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1 + * Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1 + * \endcode * - * Note that the implemented timer module must therefore support - * as many different intervals as instances of the AFBR-S50 device are - * used. + * Note that the implemented timer module must therefore support + * as many different intervals as instances of the AFBR-S50 device are + * used. * - * @addtogroup argus_timer + * @addtogroup argus_timer * @{ *****************************************************************************/ -#include "api/argus_def.h" +#include "utility/status.h" /******************************************************************************* * Lifetime Counter Timer Interface ******************************************************************************/ /*!*************************************************************************** - * @brief Obtains the lifetime counter value from the timers. + * @brief Obtains the lifetime counter value from the timers. * * @details The function is required to get the current time relative to any - * point in time, e.g. the startup time. The returned values \p hct and - * \p lct are given in seconds and microseconds respectively. The current - * elapsed time since the reference time is then calculated from: + * point in time, e.g. the startup time. The returned values \p hct and + * \p lct are given in seconds and microseconds respectively. The current + * elapsed time since the reference time is then calculated from: * - * t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec + * t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec * - * Note that the accuracy/granularity of the lifetime counter does - * not need to be 1 µsec. Usually, a granularity of approximately - * 100 µsec is sufficient. However, in case of very high frame rates - * (above 1000 frames per second), it is recommended to implement - * an even lower granularity (somewhere in the 10 µsec regime). + * Note that the accuracy/granularity of the lifetime counter does + * not need to be 1 µsec. Usually, a granularity of approximately + * 100 µsec is sufficient. However, in case of very high frame rates + * (above 1000 frames per second), it is recommended to implement + * an even lower granularity (somewhere in the 10 µsec regime). * - * It must be guaranteed, that each call of the #Timer_GetCounterValue - * function must provide a value that is greater or equal, but never lower, - * than the value returned from the previous call. + * It must be guaranteed, that each call of the #Timer_GetCounterValue + * function must provide a value that is greater or equal, but never lower, + * than the value returned from the previous call. * - * A hardware based implementation of the lifetime counter functionality - * would be to chain two distinct timers such that counter 2 increases - * its value when counter 1 wraps to 0. The easiest way is to setup - * counter 1 to wrap exactly every second. Counter 1 would than count - * the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds - * (\p hct) value. A 16-bit counter is sufficient in case of counter 1 - * while counter 2 must be a 32-bit version. + * A hardware based implementation of the lifetime counter functionality + * would be to chain two distinct timers such that counter 2 increases + * its value when counter 1 wraps to 0. The easiest way is to setup + * counter 1 to wrap exactly every second. Counter 1 would than count + * the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds + * (\p hct) value. A 16-bit counter is sufficient in case of counter 1 + * while counter 2 must be a 32-bit version. * - * In case of a lack of available hardware timers, a software solution - * can be used that requires only a 16-bit timer. In a simple scenario, - * the timer is configured to wrap around every second and increase - * a software counter value in its interrupt service routine (triggered - * with the wrap around event) every time the wrap around occurs. + * In case of a lack of available hardware timers, a software solution + * can be used that requires only a 16-bit timer. In a simple scenario, + * the timer is configured to wrap around every second and increase + * a software counter value in its interrupt service routine (triggered + * with the wrap around event) every time the wrap around occurs. * * - * @note The implementation of this function is mandatory for the correct - * execution of the API. + * @note The implementation of this function is mandatory for the correct + * execution of the API. * - * @param hct A pointer to the high counter value bits representing current - * time in seconds. + * @param hct A pointer to the high counter value bits representing current + * time in seconds. * - * @param lct A pointer to the low counter value bits representing current - * time in microseconds. Range: 0, .., 999999 µsec + * @param lct A pointer to the low counter value bits representing current + * time in microseconds. Range: 0, .., 999999 µsec *****************************************************************************/ void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct); @@ -156,68 +155,70 @@ void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct); ******************************************************************************/ /*!*************************************************************************** - * @brief The callback function type for periodic interrupt timer. + * @brief The callback function type for periodic interrupt timer. * - * @details The function that is invoked every time a specified interval elapses. - * An abstract parameter is passed to the function whenever it is called. + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. * - * @param param An abstract parameter to be passed to the callback. This is - * also the identifier of the given interval. + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. *****************************************************************************/ typedef void (*timer_cb_t)(void *param); /*!*************************************************************************** - * @brief Installs an periodic timer callback function. + * @brief Installs an periodic timer callback function. * - * @details Installs an periodic timer callback function that is invoked whenever - * an interval elapses. The callback is the same for any interval, - * however, the single intervals can be identified by the passed - * parameter. - * Passing a zero-pointer removes and disables the callback. + * @details Installs an periodic timer callback function that is invoked whenever + * an interval elapses. The callback is the same for any interval, + * however, the single intervals can be identified by the passed + * parameter. + * Passing a zero-pointer removes and disables the callback. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disable the periodic timer callback - * and thus the automatic starting of measurements from the background. + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disable the periodic timer callback + * and thus the automatic starting of measurements from the background. * - * @param f The timer callback function. + * @param f The timer callback function. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Timer_SetCallback(timer_cb_t f); /*!*************************************************************************** - * @brief Sets the timer interval for a specified callback parameter. + * @brief Sets the timer interval for a specified callback parameter. * - * @details Sets the callback interval for the specified parameter and starts - * the timer with a new interval. If there is already an interval with - * the given parameter, the timer is restarted with the given interval. - * If the same time interval as already set is passed, nothing happens. - * Passing a interval of 0 disables the timer. + * @details Sets the callback interval for the specified parameter and starts + * the timer with a new interval. If there is already an interval with + * the given parameter, the timer is restarted with the given interval. + * If the same time interval as already set is passed, nothing happens. + * Passing a interval of 0 disables the timer. * - * Note that a microsecond granularity for the timer interrupt period is - * not required. Usually a microseconds granularity is sufficient. - * The required granularity depends on the targeted frame rate, e.g. in - * case of more than 1 kHz measurement rate, a granularity of less than - * a microsecond is required to achieve the given frame rate. + * When enabling the timer (or resetting by applying another interval), + * the first timer interrupt must happen after the specified interval. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disable the periodic timer callback - * and thus the automatic starting of measurements from the background. + * Note that a microsecond granularity for the timer interrupt period is + * not required. Usually a milliseconds granularity is sufficient. + * The required granularity depends on the targeted frame rate, e.g. in + * case of more than 1 kHz measurement rate, a granularity of less than + * a millisecond is required to achieve the given frame rate. * - * @param dt_microseconds The callback interval in microseconds. + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disable the periodic timer callback + * and thus the automatic starting of measurements from the background. * - * @param param An abstract parameter to be passed to the callback. This is - * also the identifier of the given interval. + * @param dt_microseconds The callback interval in microseconds. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Timer_SetInterval(uint32_t dt_microseconds, void *param); -#ifdef __cplusplus -} -#endif - /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_TIMER_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h index ae7422ada5..8d5406360b 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Provides definitions and basic macros for fixed point data types. + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,23 +36,30 @@ #ifndef FP_DEF_H #define FP_DEF_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup fixedpoint Fixed Point Math - * @ingroup argusutil - * @brief A basic math library for fixed point number in the Qx.y fomat. - * @details This module contains common fixed point type definitions as - * well as some basic math algorithms. All types are based on - * integer types. The number are defined with the Q number format. + * @defgroup argus_fp Fixed Point Math + * @ingroup argus_util * - * - For a description of the Q number format refer to: - * https://en.wikipedia.org/wiki/Q_(number_format) - * - Another resource for fixed point math in C might be found at - * http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491 - * . - * @warning This definitions are not portable and work only with - * little-endian systems! - * @addtogroup fixedpoint + * @brief A basic math library for fixed point number in the Qx.y fomat. + * + * @details This module contains common fixed point type definitions as + * well as some basic math algorithms. All types are based on + * integer types. The number are defined with the Q number format. + * + * - For a description of the Q number format refer to: + * https://en.wikipedia.org/wiki/Q_(number_format) + * - Another resource for fixed point math in C might be found at + * http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491 + * . + * + * @warning This definitions are not portable and work only with + * little-endian systems! + * + * @addtogroup argus_fp * @{ *****************************************************************************/ @@ -66,11 +73,11 @@ ***** UQ6.2 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ6.2 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 6 integer and 2 fractional bits. - * - Range: 0 .. 63.75 - * - Granularity: 0.25 + * @brief Unsigned fixed point number: UQ6.2 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 6 integer and 2 fractional bits. + * - Range: 0 .. 63.75 + * - Granularity: 0.25 *****************************************************************************/ typedef uint8_t uq6_2_t; @@ -86,11 +93,11 @@ typedef uint8_t uq6_2_t; ***** UQ4.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ4.4 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 4 integer and 4 fractional bits. - * - Range: 0 .. 15.9375 - * - Granularity: 0.0625 + * @brief Unsigned fixed point number: UQ4.4 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 4 integer and 4 fractional bits. + * - Range: 0 .. 15.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef uint8_t uq4_4_t; @@ -106,11 +113,11 @@ typedef uint8_t uq4_4_t; ***** UQ2.6 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ2.6 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 2 integer and 6 fractional bits. - * - Range: 0 .. 3.984375 - * - Granularity: 0.015625 + * @brief Unsigned fixed point number: UQ2.6 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 2 integer and 6 fractional bits. + * - Range: 0 .. 3.984375 + * - Granularity: 0.015625 *****************************************************************************/ typedef uint8_t uq2_6_t; @@ -126,11 +133,11 @@ typedef uint8_t uq2_6_t; ***** UQ1.7 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ1.7 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 1 integer and 7 fractional bits. - * - Range: 0 .. 1.9921875 - * - Granularity: 0.0078125 + * @brief Unsigned fixed point number: UQ1.7 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 1 integer and 7 fractional bits. + * - Range: 0 .. 1.9921875 + * - Granularity: 0.0078125 *****************************************************************************/ typedef uint8_t uq1_7_t; @@ -146,11 +153,11 @@ typedef uint8_t uq1_7_t; ***** UQ0.8 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ0.8 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 1 integer and 7 fractional bits. - * - Range: 0 .. 0.99609375 - * - Granularity: 0.00390625 + * @brief Unsigned fixed point number: UQ0.8 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 1 integer and 7 fractional bits. + * - Range: 0 .. 0.99609375 + * - Granularity: 0.00390625 *****************************************************************************/ typedef uint8_t uq0_8_t; @@ -167,11 +174,11 @@ typedef uint8_t uq0_8_t; ***** Q3.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q3.4 - * @details A signed fixed point number format based on the 8-bit signed - * integer type with 3 integer and 4 fractional bits. - * - Range: -8 ... 7.9375 - * - Granularity: 0.0625 + * @brief Signed fixed point number: Q3.4 + * @details A signed fixed point number format based on the 8-bit signed + * integer type with 3 integer and 4 fractional bits. + * - Range: -8 ... 7.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef int8_t q3_4_t; @@ -189,11 +196,11 @@ typedef int8_t q3_4_t; ***** Q1.6 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q1.6 - * @details A signed fixed point number format based on the 8-bit signed - * integer type with 1 integer and 6 fractional bits. - * - Range: -2 ... 1.984375 - * - Granularity: 0.015625 + * @brief Signed fixed point number: Q1.6 + * @details A signed fixed point number format based on the 8-bit signed + * integer type with 1 integer and 6 fractional bits. + * - Range: -2 ... 1.984375 + * - Granularity: 0.015625 *****************************************************************************/ typedef int8_t q1_6_t; @@ -215,11 +222,11 @@ typedef int8_t q1_6_t; ***** UQ12.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ12.4 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 12 integer and 4 fractional bits. - * - Range: 0 ... 4095.9375 - * - Granularity: 0.0625 + * @brief Unsigned fixed point number: UQ12.4 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 12 integer and 4 fractional bits. + * - Range: 0 ... 4095.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef uint16_t uq12_4_t; @@ -235,11 +242,11 @@ typedef uint16_t uq12_4_t; ***** UQ10.6 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ10.6 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 10 integer and 6 fractional bits. - * - Range: 0 ... 1023.984375 - * - Granularity: 0.015625 + * @brief Unsigned fixed point number: UQ10.6 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 10 integer and 6 fractional bits. + * - Range: 0 ... 1023.984375 + * - Granularity: 0.015625 *****************************************************************************/ typedef uint16_t uq10_6_t; @@ -255,11 +262,11 @@ typedef uint16_t uq10_6_t; ***** UQ1.15 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ1.15 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 1 integer and 15 fractional bits. - * - Range: 0 .. 1.999969 - * - Granularity: 0.000031 + * @brief Unsigned fixed point number: UQ1.15 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 1 integer and 15 fractional bits. + * - Range: 0 .. 1.999969 + * - Granularity: 0.000031 *****************************************************************************/ typedef uint16_t uq1_15_t; @@ -275,11 +282,11 @@ typedef uint16_t uq1_15_t; ***** UQ0.16 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ0.16 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 0 integer and 16 fractional bits. - * - Range: 0 .. 0.9999847412109375 - * - Granularity: 1.52587890625e-5 + * @brief Unsigned fixed point number: UQ0.16 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 0 integer and 16 fractional bits. + * - Range: 0 .. 0.9999847412109375 + * - Granularity: 1.52587890625e-5 *****************************************************************************/ typedef uint16_t uq0_16_t; @@ -296,11 +303,11 @@ typedef uint16_t uq0_16_t; ***** Q11.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q11.4 - * @details A signed fixed point number format based on the 16-bit signed - * integer type with 11 integer and 4 fractional bits. - * - Range: -2048 ... 2047.9375 - * - Granularity: 0.0625 + * @brief Signed fixed point number: Q11.4 + * @details A signed fixed point number format based on the 16-bit signed + * integer type with 11 integer and 4 fractional bits. + * - Range: -2048 ... 2047.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef int16_t q11_4_t; @@ -319,11 +326,11 @@ typedef int16_t q11_4_t; ***** Q7.8 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q7.8 - * @details A signed fixed point number format based on the 16-bit signed - * integer type with 7 integer and 8 fractional bits. - * - Range: -128 .. 127.99609375 - * - Granularity: 0.00390625 + * @brief Signed fixed point number: Q7.8 + * @details A signed fixed point number format based on the 16-bit signed + * integer type with 7 integer and 8 fractional bits. + * - Range: -128 .. 127.99609375 + * - Granularity: 0.00390625 *****************************************************************************/ typedef int16_t q7_8_t; @@ -342,11 +349,11 @@ typedef int16_t q7_8_t; ***** Q3.12 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q3.12 - * @details A signed fixed point number format based on the 16-bit integer - * type with 3 integer and 12 fractional bits. - * - Range: -8 .. 7.99975586 - * - Granularity: 0.00024414 + * @brief Signed fixed point number: Q3.12 + * @details A signed fixed point number format based on the 16-bit integer + * type with 3 integer and 12 fractional bits. + * - Range: -8 .. 7.99975586 + * - Granularity: 0.00024414 *****************************************************************************/ typedef int16_t q3_12_t; @@ -365,11 +372,11 @@ typedef int16_t q3_12_t; ***** Q0.15 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q0.15 - * @details A signed fixed point number format based on the 16-bit integer - * type with 0 integer and 15 fractional bits. - * - Range: -1 .. 0.999969482 - * - Granularity: 0.000030518 + * @brief Signed fixed point number: Q0.15 + * @details A signed fixed point number format based on the 16-bit integer + * type with 0 integer and 15 fractional bits. + * - Range: -1 .. 0.999969482 + * - Granularity: 0.000030518 *****************************************************************************/ typedef int16_t q0_15_t; @@ -389,11 +396,11 @@ typedef int16_t q0_15_t; ***** UQ28.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ28.4 - * @details An unsigned fixed point number format based on the 32-bit unsigned - * integer type with 28 integer and 4 fractional bits. - * - Range: 0 ... 268435455.9375 - * - Granularity: 0.0625 + * @brief Unsigned fixed point number: UQ28.4 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 28 integer and 4 fractional bits. + * - Range: 0 ... 268435455.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef uint32_t uq28_4_t; @@ -409,11 +416,11 @@ typedef uint32_t uq28_4_t; ***** UQ16.16 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ16.16 - * @details An unsigned fixed point number format based on the 32-bit unsigned - * integer type with 16 integer and 16 fractional bits. - * - Range: 0 ... 65535.999984741 - * - Granularity: 0.000015259 + * @brief Unsigned fixed point number: UQ16.16 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 16 integer and 16 fractional bits. + * - Range: 0 ... 65535.999984741 + * - Granularity: 0.000015259 *****************************************************************************/ typedef uint32_t uq16_16_t; @@ -432,11 +439,11 @@ typedef uint32_t uq16_16_t; ***** UQ10.22 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ10.22 - * @details An unsigned fixed point number format based on the 32-bit unsigned - * integer type with 10 integer and 22 fractional bits. - * - Range: 0 ... 1023.99999976158 - * - Granularity: 2.38418579101562E-07 + * @brief Unsigned fixed point number: UQ10.22 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 10 integer and 22 fractional bits. + * - Range: 0 ... 1023.99999976158 + * - Granularity: 2.38418579101562E-07 *****************************************************************************/ typedef uint32_t uq10_22_t; @@ -456,11 +463,11 @@ typedef uint32_t uq10_22_t; ***** Q27.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q27.4 - * @details A signed fixed point number format based on the 32-bit signed - * integer type with 27 integer and 4 fractional bits. - * - Range: -134217728 ... 134217727.9375 - * - Granularity: 0.0625 + * @brief Signed fixed point number: Q27.4 + * @details A signed fixed point number format based on the 32-bit signed + * integer type with 27 integer and 4 fractional bits. + * - Range: -134217728 ... 134217727.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef int32_t q27_4_t; @@ -475,15 +482,35 @@ typedef int32_t q27_4_t; +/******************************************************************************* + ***** Q16.15 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q16.15 + * @details A signed fixed point number format based on the 32-bit integer + * type with 16 integer and 15 fractional bits. + * - Range: -65536 .. 65536.999969482 + * - Granularity: 0.000030518 + *****************************************************************************/ +typedef int32_t q16_15_t; + +/*! Minimum value of Q16.15 number format. */ +#define Q16_15_MIN ((q16_15_t)INT32_MIN) + +/*! Maximum value of Q16.15 number format. */ +#define Q16_15_MAX ((q16_15_t)INT32_MAX) + + + /******************************************************************************* ***** Q15.16 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q15.16 - * @details A signed fixed point number format based on the 32-bit integer - * type with 15 integer and 16 fractional bits. - * - Range: -32768 .. 32767.99998 - * - Granularity: 1.52588E-05 + * @brief Signed fixed point number: Q15.16 + * @details A signed fixed point number format based on the 32-bit integer + * type with 15 integer and 16 fractional bits. + * - Range: -32768 .. 32767.99998 + * - Granularity: 1.52588E-05 *****************************************************************************/ typedef int32_t q15_16_t; @@ -502,11 +529,11 @@ typedef int32_t q15_16_t; ***** Q9.22 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q9.22 - * @details A signed fixed point number format based on the 32-bit integer - * type with 9 integer and 22 fractional bits. - * - Range: -512 ... 511.9999998 - * - Granularity: 2.38418579101562E-07 + * @brief Signed fixed point number: Q9.22 + * @details A signed fixed point number format based on the 32-bit integer + * type with 9 integer and 22 fractional bits. + * - Range: -512 ... 511.9999998 + * - Granularity: 2.38418579101562E-07 *****************************************************************************/ typedef int32_t q9_22_t; @@ -522,4 +549,7 @@ typedef int32_t q9_22_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* FP_DEF_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h new file mode 100644 index 0000000000..60b75a164a --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h @@ -0,0 +1,173 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 FP_DIV_H +#define FP_DIV_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" +#include "int_math.h" + +/*!*************************************************************************** + * Set to use hardware division (Cortex-M3/4) over software division (Cortex-M0/1). + *****************************************************************************/ +#ifndef USE_HW_DIV +#define USE_HW_DIV 0 +#endif + +/*!*************************************************************************** + * @brief 32-bit implementation of an Q15.16 division. + * + * @details Algorithm to evaluate a/b, where b is in Q15.16 format, on a 32-bit + * architecture with maximum precision. + * The result is correctly rounded and given as the input format. + * Division by 0 yields max. values determined by signa of numerator. + * Too high/low results are truncated to max/min values. + * + * Depending on the architecture, the division is implemented with a 64-bit + * division and shifting (Cortex-M3/4) or as a fast software algorithm + * (Cortex-M0/1) wich runs fast on processors without hardware division. + * + * @see https://code.google.com/archive/p/libfixmath + * + * @param a Numerator in any Qx.y format + * @param b Denominator in Q15.16 format + * @return Result = a/b in the same Qx.y format as the input parameter a. + *****************************************************************************/ +inline int32_t fp_div16(int32_t a, q15_16_t b) +{ + //assert(b); + if (b == 0) { return a < 0 ? INT32_MIN : INT32_MAX; } + +#if USE_HW_DIV + // Tested on Cortex-M4, it takes approx. 75% of the + // software algorithm below. + int64_t c = ((int64_t) a) << 30U; + + if ((uint32_t)(a ^ b) & 0x80000000U) { + c = (((-c) / b) + (1 << 13U)) >> 14U; + + if (c > 0x80000000U) { return INT32_MIN; } + + return -c; + + } else { + c = ((c / b) + (1 << 13U)) >> 14U; + + if (c > (int64_t)INT32_MAX) { return INT32_MAX; } + + return c; + } + +#else + // This uses the basic binary restoring division algorithm. + // It appears to be faster to do the whole division manually than + // trying to compose a 64-bit divide out of 32-bit divisions on + // platforms without hardware divide. + // Tested on Cortex-M0, it takes approx. 33% of the time of the + // 64-bit version above. + + uint32_t remainder = absval(a); + uint32_t divider = absval(b); + + uint32_t quotient = 0; + uint32_t bit = 0x10000U; + + /* The algorithm requires D >= R */ + while (divider < remainder) { + divider <<= 1U; + bit <<= 1U; + } + + if (!bit) { + if ((uint32_t)(a ^ b) & 0x80000000U) { // return truncated values + return INT32_MIN; + + } else { + return INT32_MAX; + } + } + + if (divider & 0x80000000U) { + // Perform one step manually to avoid overflows later. + // We know that divider's bottom bit is 0 here. + if (remainder >= divider) { + quotient |= bit; + remainder -= divider; + } + + divider >>= 1U; + bit >>= 1U; + } + + /* Main division loop */ + while (bit && remainder) { + if (remainder >= divider) { + quotient |= bit; + remainder -= divider; + } + + remainder <<= 1U; + bit >>= 1U; + } + + if (remainder >= divider) { + quotient++; + } + + uint32_t result = quotient; + + /* Figure out the sign of result */ + if ((uint32_t)(a ^ b) & 0x80000000U) { + result = -result; + } + + return (int32_t)result; +#endif +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_DIV_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h new file mode 100644 index 0000000000..f8c494a630 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h @@ -0,0 +1,204 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides averaging algorithms for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 FP_EMA_H +#define FP_EMA_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" + +#include "utility/fp_rnd.h" +#include "utility/fp_mul.h" + +/*!*************************************************************************** + * @brief Circular exponentially weighted moving average using UQ1.15 format. + * + * @details Evaluates the moving average (exponentially weighted) for circular + * data in UQ1.15 format. + * Circular data is that MAX_VALUE + 1 == MIN_VALUE. For example the + * usual phase information. + * + * Problem: Due to circularity of phase values, i.e. 0+x and 2PI+x are + * the same, the usual EMA has issues with the wrap around effect. + * Especially for vectors with phase around 0 (or 2PI), two values + * like 0 + x and PI - y are averaged to something around PI instead + * of 0 which would be more correct. + * + * Solution: Assume that phase jumps of more than PI are not allowed + * or possible. If a deviation of the new value to the smoothed signal + * occurs, it is clear that this stems from the wrap around effect and + * can be caught and correctly handled by the smoothing algorithm. + * + * Caution: If a target comes immediately into the field of view, phase + * jumps of > PI are indeed possible and volitional. However, the + * averaging break there anyway since the smoothed signal approaches + * only with delay to the correct values. The error made here is, that + * the smoothed signal approaches from the opposite direction. However, + * is approaches even faster since it always takes the shortest + * direction. + * + * @param mean The previous mean value in UQ1.15 format. + * @param x The current value to be added to the average UQ1.15 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in UQ1.15 format. + *****************************************************************************/ +inline uq1_15_t fp_ema15c(uq1_15_t mean, uq1_15_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + // Heeds the wrap around effect by casting dx to int16: + const int16_t dx = (int16_t)(x - mean); + const int32_t diff = weight * dx; + return (uq1_15_t)fp_rnds((mean << 8U) + diff, 8U); +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q11.4 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q11.4 format. + * + * @param mean The previous mean value in Q11.4 format. + * @param x The current value to be added to the average Q11.4 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q11.4 format. + *****************************************************************************/ +inline q11_4_t fp_ema4(q11_4_t mean, q11_4_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + const int32_t dx = x - mean; + const int32_t diff = weight * dx; + return (q11_4_t)fp_rnds((mean << 8U) + diff, 8U); +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q7.8 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q7.8 format. + * + * @param mean The previous mean value in Q7.8 format. + * @param x The current value to be added to the average Q7.8 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q7.8 format. + *****************************************************************************/ +inline q7_8_t fp_ema8(q7_8_t mean, q7_8_t x, uq0_8_t weight) +{ + return (q7_8_t)fp_ema4(mean, x, weight); +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q15.16 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q15.16 format. + * + * @param mean The previous mean value in Q15.16 format. + * @param x The current value to be added to the average Q15.16 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q15.16 format. + *****************************************************************************/ +inline uint32_t uint_ema32(uint32_t mean, uint32_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + if (x > mean) { + const uint32_t dx = x - mean; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean + diff; + + } else { + const uint32_t dx = mean - x; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean - diff; + } +} +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q15.16 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q15.16 format. + * + * @param mean The previous mean value in Q15.16 format. + * @param x The current value to be added to the average Q15.16 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q15.16 format. + *****************************************************************************/ +inline int32_t int_ema32(int32_t mean, int32_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + if (x > mean) { + const uint32_t dx = x - mean; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean + diff; + + } else { + const uint32_t dx = mean - x; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean - diff; + } +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q15.16 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q15.16 format. + * + * @param mean The previous mean value in Q15.16 format. + * @param x The current value to be added to the average Q15.16 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q15.16 format. + *****************************************************************************/ +inline q15_16_t fp_ema16(q15_16_t mean, q15_16_t x, uq0_8_t weight) +{ + return (q15_16_t)int_ema32(mean, x, weight); +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_EMA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h new file mode 100644 index 0000000000..f845ca6e00 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h @@ -0,0 +1,69 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an exponential function for fixed point type. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 FP_EXP_H +#define FP_EXP_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" + +/*!*************************************************************************** + * @brief Calculates the exponential of an fixed point number Q15.16 format. + * + * @details Calculates y = exp(x) in fixed point representation. + * + * Note that the result might not be 100 % accurate and might contain + * a small error! + * + * @see https://www.quinapalus.com/efunc.html + * + * @param x The input parameter in unsigned fixed point format Q15.16. + * @return Result y = exp(x) in the UQ16.16 format. + *****************************************************************************/ +uq16_16_t fp_exp16(q15_16_t x); + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_DIV_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h new file mode 100644 index 0000000000..6bc42b2ec7 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h @@ -0,0 +1,69 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an logarithm function for fixed point type. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 FP_LOG_H +#define FP_LOG_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" + +/*!*************************************************************************** + * @brief Calculates the natural logarithm (base e) of an fixed point number. + * + * @details Calculates y = ln(x) = log_e(x) in fixed point representation. + * + * Note that the result might not be 100 % accurate and might contain + * a small error! + * + * @see https://www.quinapalus.com/efunc.html + * + * @param x The input parameter in unsigned fixed point format Q15.16. + * @return Result y = ln(x) in the UQ16.16 format. + *****************************************************************************/ +q15_16_t fp_log16(uq16_16_t x); + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_DIV_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h new file mode 100644 index 0000000000..78db582644 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h @@ -0,0 +1,235 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 FP_MUL_H +#define FP_MUL_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" +#include "utility/fp_rnd.h" + +/*!*************************************************************************** + * Set to use hardware division (Cortex-M3/4) over software division (Cortex-M0/1). + *****************************************************************************/ +#ifndef USE_64BIT_MUL +#define USE_64BIT_MUL 0 +#endif + +#if !USE_64BIT_MUL +/*!*************************************************************************** + * @brief Long multiplication of two unsigned 32-bit into an 64-bit value on + * 32-bit architecture. + * + * @details w (two words) gets the product of u and v (one word each). + * w[0] is the most significant word of the result, w[1] the least. + * (The words are in big-endian order). + * It is Knuth's Algorithm M from [Knu2] section 4.3.1. + * * + * @see http://www.hackersdelight.org/hdcodetxt/muldwu.c.txt + * + * @param w The result (u * v) value given as two unsigned 32-bit numbers: + * w[0] is the most significant word of the result, w[1] the least. + * (The words are in big-endian order). + * @param u Left hand side of the multiplication. + * @param v Right hand side of the multiplication. + *****************************************************************************/ +inline void muldwu(uint32_t w[], uint32_t u, uint32_t v) +{ + const uint32_t u0 = u >> 16U; + const uint32_t u1 = u & 0xFFFFU; + const uint32_t v0 = v >> 16U; + const uint32_t v1 = v & 0xFFFFU; + + uint32_t t = u1 * v1; + const uint32_t w3 = t & 0xFFFFU; + uint32_t k = t >> 16U; + + t = u0 * v1 + k; + const uint32_t w2 = t & 0xFFFFU; + const uint32_t w1 = t >> 16U; + + t = u1 * v0 + w2; + k = t >> 16U; + + w[0] = u0 * v0 + w1 + k; + w[1] = (t << 16U) + w3; +} +#endif + +/*!*************************************************************************** + * @brief 64-bit implementation of an unsigned multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * number of 32-bit width. The multiplication is done in 64-bit and + * the result is shifted down by the passed shift parameter in order + * to return a 32-bit value. + * The shift is executed with correct rounding. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in UQx1.y1 format + * @param v The right parameter in UQx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in UQx.(y1+y2-shift) format. + *****************************************************************************/ +inline uint32_t fp_mulu(uint32_t u, uint32_t v, uint_fast8_t shift) +{ + assert(shift <= 32); +#if USE_64BIT_MUL + const uint64_t w = (uint64_t)u * (uint64_t)v; + return (w >> shift) + ((w >> (shift - 1)) & 1U); +#else + uint32_t tmp[2] = { 0 }; + muldwu(tmp, u, v); + + assert(shift ? tmp[0] <= (UINT32_MAX >> (32 - shift)) : tmp[0] == 0); + + if (32 - shift) { + return ((tmp[0] << (32 - shift)) + fp_rndu(tmp[1], shift)); + + } else { + return tmp[1] > (UINT32_MAX >> 1) ? tmp[0] + 1 : tmp[0]; + } + +#endif +} + +/*!*************************************************************************** + * @brief 64-bit implementation of a signed multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * number of 32-bit width. The multiplication is done in 64-bit and + * the result is shifted down by the passed shift parameter in order + * to return a 32-bit value. + * The shift is executed with correct rounding. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in Qx1.y1 format + * @param v The right parameter in Qx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in Qx.(y1+y2-shift) format. + *****************************************************************************/ +inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift) +{ + int_fast8_t sign = 1; + + uint32_t u2, v2; + + if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; } + + if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; } + + const uint32_t res = fp_mulu(u2, v2, shift); + + assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U); + + return sign > 0 ? res : -res; +} + + +/*!*************************************************************************** + * @brief 48-bit implementation of a unsigned multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * numbers with 32-bit unsigned and 16-bit unsigned format respectively. + * The multiplication is done in two 16x16-bit operations and the + * result is shifted down by the passed shift parameter in order to + * return a 32-bit value. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in Qx1.y1 format + * @param v The right parameter in Qx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in Qx.(y1+y2-shift) format. + *****************************************************************************/ +inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift) +{ + assert(shift <= 48); + + if (shift > 16) { + uint32_t msk = 0xFFFFU; + uint32_t a = (u >> 16U) * v; + uint32_t b = (msk & u) * v; + return fp_rndu(a, shift - 16) + fp_rndu(b, shift); + + } else { + uint32_t msk = ~(0xFFFFFFFFU << shift); + uint32_t a = (u >> shift) * v; + uint32_t b = fp_rndu((msk & u) * v, shift); + return a + b; + } +} + +/*!*************************************************************************** + * @brief 48-bit implementation of an unsigned/signed multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * numbers with 32-bit signed and 16-bit unsigned format respectively. + * The multiplication is done in two 16x16-bit operations and the + * result is shifted down by the passed shift parameter in order to + * return a 32-bit value. + * The shift is executed with correct rounding. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in Qx1.y1 format + * @param v The right parameter in Qx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in Qx.(y1+y2-shift) format. + *****************************************************************************/ +inline int32_t fp_mul_s32_u16(int32_t u, uint16_t v, uint_fast8_t shift) +{ + return u >= 0 ? fp_mul_u32_u16(u, v, shift) : - fp_mul_u32_u16(-u, v, shift); +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_MUL_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h new file mode 100644 index 0000000000..ad6f71e09c --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h @@ -0,0 +1,118 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 FP_RND_H +#define FP_RND_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" +#include + +/*!*************************************************************************** + * @brief Converting with rounding from UQx.n1 to UQx.n2. + * @details Equivalent to dividing by 2^n with correct rounding to unsigned + * integer values. + * @param Q The number in (U)Qx.n1 fixed point format to be rounded. + * @param n The number of bits to be rounded, + * e.g. UQ8.8 -> UQ12.4 => n = 8 - 4 = 4. + * @return The rounded value in (U)Qx.n2 format. + *****************************************************************************/ +inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n) +{ + if (n == 0) { return Q; } + + else if (n > 32U) { return 0; } + + // Shift by n>=32 yields undefined behavior! Thus, this extra first + // step is essential to prevent issues. + Q >>= n - 1; + return (Q >> 1) + (Q & 1U); +} + +/*!*************************************************************************** + * @brief Converting with rounding from Qx.n1 to Qx.n2. + * @details Equivalent to dividing by 2^n with correct rounding to integer + * values. + * @param Q The number in (U)Qx.n1 fixed point format to be rounded. + * @param n The number of bits to be rounded, + * e.g. Q7.8 -> Q11.4 => n = 8 - 4 = 4. + * @return The rounded value in (U)Qx.n2 format. + *****************************************************************************/ +inline int32_t fp_rnds(int32_t Q, uint_fast8_t n) +{ + return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n); +} + +/*!*************************************************************************** + * @brief Converting with truncation from UQx.n1 to UQx.n2. + * @details Equivalent to dividing by 2^n with truncating (throw away) the + * fractional part, resulting in an unsigned integer/fixed-point value. + * @param Q The number in (U)Qx.n1 fixed point format to be truncated. + * @param n The number of bits to be truncated, + * e.g. UQ8.8 -> UQ12.4 => n = 8 - 4 = 4. + * @return The truncated value in (U)Qx.n2 format. + *****************************************************************************/ +inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n) +{ + return (n < 32U) ? (Q >> n) : 0; +} + +/*!*************************************************************************** + * @brief Converting with truncation from Qx.n1 to Qx.n2. + * @details Equivalent to dividing by 2^n with truncating (throw away) the + * fractional part, resulting in a signed integer/fixed-point value. + * @param Q The number in (U)Qx.n1 fixed point format to be truncated. + * @param n The number of bits to be truncated, + * e.g. Q7.8 -> Q11.4 => n = 8 - 4 = 4. + * @return The truncated value in (U)Qx.n2 format. + *****************************************************************************/ +inline int32_t fp_truncs(int32_t Q, uint_fast8_t n) +{ + return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n); +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_RND_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h new file mode 100644 index 0000000000..27de8cc688 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h @@ -0,0 +1,281 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides algorithms applied to integer values. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 INT_MATH +#define INT_MATH +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_misc + * @{ + *****************************************************************************/ + +#include +#include + +/*! Enables the integer square root function. */ +#ifndef INT_SQRT +#define INT_SQRT 0 +#endif + +/*!*************************************************************************** + * @brief Integer Base-2 Logarithm. + * + * @details Calculates the base-2 logarithm for unsigned integer values. The + * result is the integer equivalent of floor(log2(x)). + * + * @param x Input parameter. + * @return The floor of the base-2 logarithm. + *****************************************************************************/ +inline uint32_t log2i(uint32_t x) +{ + assert(x != 0); +#if 1 + return 31 - __builtin_clz(x); +#else +#define S(k) if (x >= (1 << k)) { i += k; x >>= k; } + int i = 0; S(16); S(8); S(4); S(2); S(1); return i; +#undef S +#endif +} + +/*!*************************************************************************** + * @brief Integer Base-2 Logarithm with rounded result. + * + * @details Calculates the base-2 logarithm for unsigned integer values and + * returns the rounded result. The result is the integer equivalent + * of round(log2(x)). + * + * It is finding the nearest power-of-two value s.t. |x - 2^n| becomes + * minimum for all n. + * + * @param x Input parameter. + * @return The rounded value of the base-2 logarithm. + *****************************************************************************/ +inline uint32_t log2_round(uint32_t x) +{ + assert(x != 0); +#if 0 + const uint32_t y = x; + const uint32_t i = 0; + + while (y >>= 1) { i++; } + +#else + const uint32_t i = log2i(x); +#endif + return (i + ((x >> (i - 1U)) == 3U)); +} + +/*!*************************************************************************** + * @brief Finding the nearest power-of-two value. + * + * @details Implemented s.t. |x - 2^n| becomes minimum for all n. + * Special case 0: returns 0; + * Maximum input: 3037000499; higher number result in overflow! (returns 0) + * + * @param x Input parameter. + * @return Nearest power-of-two number, i.e. 2^n. + *****************************************************************************/ +inline uint32_t binary_round(uint32_t x) +{ + assert(x != 0); + const uint32_t shift = log2_round(x); + return (shift > 31U) ? 0 : 1U << shift; +} + +/*!*************************************************************************** + * @brief Counting bits set in a 32-bit unsigned integer. + * + * @details @see http://graphics.stanford.edu/~seander/bithacks.html + * + * @param x Input parameter. + * @return Number of bits set in input value. + *****************************************************************************/ +inline uint32_t popcount(uint32_t x) +{ + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel + x = x - ((x >> 1) & 0x55555555); + x = (x & 0x33333333) + ((x >> 2) & 0x33333333); + return (((x + (x >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; +} + +/*!*************************************************************************** + * @brief Determining if an integer is a power of 2 + * + * @details @see http://graphics.stanford.edu/~seander/bithacks.html + * + * @param x Input parameter. + * @return True if integer is power of 2. + *****************************************************************************/ +inline uint32_t ispowoftwo(uint32_t x) +{ + return x && !(x & (x - 1)); +} + +/*!*************************************************************************** + * @brief Calculates the absolute value. + * + * @param x Input parameter. + * @return The absolute value of x. + *****************************************************************************/ +inline uint32_t absval(int32_t x) +{ + // Note: special case of INT32_MIN must be handled correctly: + return x < 0 ? ((~(uint32_t)(x)) + 1) : (uint32_t)x; + + /* alternative with equal performance:*/ +// int32_t y = x >> 31; +// return (x ^ y) - y; + /* wrong implementation: + * does not correctly return abs(INT32_MIN) on 32-bit platform */ +// return x < 0 ? (uint32_t)(-x) : (uint32_t)x; +} + +/*!*************************************************************************** + * @brief Calculates the floor division by a factor of 2: floor(x / 2^n). + * + * @param x Input parameter. + * @param n The shift value, maximum is 31. + * @return The floor division by 2^n result. + *****************************************************************************/ +inline uint32_t floor2(uint32_t x, uint_fast8_t n) +{ + assert(n < 32); + return x >> n; +} + +/*!*************************************************************************** + * @brief Calculates the ceildiv division by a factor of 2: ceildiv(x / 2^n). + * + * @param x Input parameter. + * @param n The shift value, maximum is 31. + * @return The ceildiv division by 2^n result. + *****************************************************************************/ +inline uint32_t ceiling2(uint32_t x, uint_fast8_t n) +{ + assert(n < 32); + return x ? (1 + ((x - 1) >> n)) : 0; +} + +/*!*************************************************************************** + * @brief Calculates the ceildiv division: ceildiv(x / y). + * + * @param x Numerator + * @param y Denominator + * @return The result of the ceildiv division ceildiv(x / y). + *****************************************************************************/ +inline uint32_t ceildiv(uint32_t x, uint32_t y) +{ + assert(y != 0); + return x ? (1 + ((x - 1) / y)) : 0; +} + +/*!*************************************************************************** + * @brief Calculates the maximum of two values. + * + * @param a Input parameter. + * @param b Input parameter. + * @return The maximum value of the input parameters. + *****************************************************************************/ +#define MAX(a, b) ((a) > (b) ? (a) : (b)) + +/*!*************************************************************************** + * @brief Calculates the minimum of two values. + * + * @param a Input parameter. + * @param b Input parameter. + * @return The minimum value of the input parameters. + *****************************************************************************/ +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +/*!*************************************************************************** + * @brief Clamps a value between a minimum and maximum boundary. + * + * @details Clamps the values such that the condition min <= x <= max is true. + * + * @note The condition \p min <= \p max must hold!!! + * + * @param x The input parameter to be clamped. + * @param min The minimum or lower boundary. + * @param max The maximum or upper boundary. + * @return The clamped value of the input parameter within [min,max]. + *****************************************************************************/ +#define CLAMP(x, min, max) (MIN(MAX((x), (min)), (max))) + +#if INT_SQRT +/*!*************************************************************************** + * @brief Calculates the integer square root of x. + * + * @details The integer square root is defined as: + * isqrt(x) = (int)sqrt(x) + * + * @see https://en.wikipedia.org/wiki/Integer_square_root + * @see https://github.com/chmike/fpsqrt/blob/master/fpsqrt.c + * + * @param x Input parameter. + * @return isqrt(x) + *****************************************************************************/ +inline uint32_t isqrt(uint32_t v) +{ + unsigned t, q, b, r; + r = v; // r = v - x² + b = 0x40000000; // a² + q = 0; // 2ax + + while (b > 0) { + t = q + b; // t = 2ax + a² + q >>= 1; // if a' = a/2, then q' = q/2 + + if (r >= t) { // if (v - x²) >= 2ax + a² + r -= t; // r' = (v - x²) - (2ax + a²) + q += b; // if x' = (x + a) then ax' = ax + a², thus q' = q' + b + } + + b >>= 2; // if a' = a/2, then b' = b / 4 + } + + return q; +} +#endif // INT_SQRT + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* INT_MATH */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h new file mode 100644 index 0000000000..ae9a4648f5 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h @@ -0,0 +1,49 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file contains status codes for all platform specific + * functions. + * + * @copyright + * + * Copyright (c) 2023, Broadcom Inc. + * 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 of the copyright holder 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 HOLDER 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 STATUS_H +#define STATUS_H +#ifdef __cplusplus +extern "C" { +#endif + +#include "api/argus_status.h" + +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* STATUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h index 9c5f930351..1c2fe480e8 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides utility functions for timing necessities. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides utility functions for timing necessities. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,255 +36,563 @@ #ifndef TIME_H #define TIME_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup time Time Utility - * @ingroup argusutil - * @brief Timer utilities for time measurement duties. - * @details This module provides time measurement utility functions like - * delay or time measurement methods, or time math functions. - * @addtogroup time + * @defgroup argus_time Time Utility + * @ingroup argus_util + * + * @brief Timer utilities for time measurement duties. + * + * @details This module provides time measurement utility functions like + * delay or time measurement methods, or time math functions. + * + * @addtogroup argus_time * @{ *****************************************************************************/ +#include "platform/argus_timer.h" +#include #include #include /*!*************************************************************************** - * @brief A data structure to represent current time. + * @brief A data structure to represent current time. * - * @details Value is obtained from the PIT time which must be configured as - * lifetime counter. + * @details Value is obtained from the PIT time which must be configured as + * lifetime counter. + * + * Range: [0.000000, 4294967296.999999] seconds *****************************************************************************/ -typedef struct { - /*! Seconds. */ +typedef struct ltc_t { + /*! Seconds; + * Range: [0, UINT32_MAX] seconds */ uint32_t sec; - /*! Microseconds. */ + /*! Microseconds; + * Range: [0, 999999] microseconds */ uint32_t usec; } ltc_t; /*!*************************************************************************** - * @brief Obtains the elapsed time since MCU startup. - * @param t_now returned current time + * @brief Converts #ltc_t to microseconds (uint32_t). + * @details The specified time value (type #ltc_t) is converted to microseconds. + * The value is truncated to UINT32_MAX value if the result would + * exceed UINT32_MAX microseconds. + * @param t Input #ltc_t structure. + * @return Time value in microseconds. *****************************************************************************/ -void Time_GetNow(ltc_t *t_now); +inline uint32_t Time_ToUSec(ltc_t const *t) +{ + assert(t != 0); + + // max. value to convert correctly is 4294.967295 sec (UINT32_MAX/1000000) + return ((t->sec < 4294U) || (t->sec == 4294U && t->usec < 967295U)) ? + t->usec + t->sec * 1000000U : UINT32_MAX; +} /*!*************************************************************************** - * @brief Obtains the elapsed microseconds since MCU startup. - * @details Wrap around effect due to uint32_t result format!! - * @param - - * @return Elapsed microseconds since MCU startup as uint32_t. + * @brief Converts #ltc_t to milliseconds (uint32_t). + * @details The specified time value (type #ltc_t) is converted to milliseconds. + * The value is truncated to UINT32_MAX value if the result would + * exceed UINT32_MAX milliseconds. + * The returned value is correctly rounded to the nearest value. + * @param t Input #ltc_t structure. + * @return Time value in milliseconds. *****************************************************************************/ -uint32_t Time_GetNowUSec(void); +inline uint32_t Time_ToMSec(ltc_t const *t) +{ + assert(t != 0); + + // max. value to convert correctly is 4294967.295499 sec (UINT32_MAX/1000) + return ((t->sec < 4294967U) || (t->sec == 4294967U && t->usec < 295500U)) ? + (t->usec + 500U) / 1000U + t->sec * 1000U : UINT32_MAX; +} /*!*************************************************************************** - * @brief Obtains the elapsed milliseconds since MCU startup. - * @details Wrap around effect due to uint32_t result format!! - * @param - - * @return Elapsed milliseconds since MCU startup as uint32_t. + * @brief Converts #ltc_t to seconds (uint32_t). + * @details The specified time value (type #ltc_t) is converted to seconds. + * The returned value is correctly rounded to the nearest value. + * @param t Input #ltc_t structure. + * @return Time value in seconds. *****************************************************************************/ -uint32_t Time_GetNowMSec(void); +inline uint32_t Time_ToSec(ltc_t const *t) +{ + assert(t != 0); + + // max. value to convert correctly is 4294967295.499999 sec (UINT32_MAX/1000) + return (t->sec < 4294967295U || t->usec < 500000U) ? + (t->usec + 500000U) / 1000000U + t->sec : UINT32_MAX; +} /*!*************************************************************************** - * @brief Obtains the elapsed seconds since MCU startup. - * @param - - * @return Elapsed seconds since MCU startup as uint32_t. + * @brief Converts microseconds (uint32_t) to #ltc_t. + * @details The specified time value in microseconds is converted to type #ltc_t. + * @param t Output #ltc_t structure. + * @param t_usec Input time in microseconds. *****************************************************************************/ -uint32_t Time_GetNowSec(void); +inline void Time_FromUSec(ltc_t *t, uint32_t t_usec) +{ + assert(t != 0); + t->sec = t_usec / 1000000U; + t->usec = t_usec % 1000000U; +} /*!*************************************************************************** - * @brief Obtains the elapsed time since a given time point. - * @param t_elapsed Returns the elapsed time since t_start. - * @param t_start Start time point. + * @brief Converts milliseconds (uint32_t) to #ltc_t. + * @details The specified time value in milliseconds is converted to type #ltc_t. + * @param t Output #ltc_t structure. + * @param t_msec Input time in milliseconds. *****************************************************************************/ -void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start); +inline void Time_FromMSec(ltc_t *t, uint32_t t_msec) +{ + assert(t != 0); + t->sec = t_msec / 1000U; + t->usec = (t_msec % 1000U) * 1000U; +} /*!*************************************************************************** - * @brief Obtains the elapsed microseconds since a given time point. - * @details Wrap around effect due to uint32_t result format!! - * @param t_start Start time point. - * @return Elapsed microseconds since t_start as uint32_t. + * @brief Converts seconds (uint32_t) to #ltc_t. + * @details The specified time value in seconds is converted to type #ltc_t. + * @param t Output #ltc_t structure. + * @param t_sec Input time in seconds. *****************************************************************************/ -uint32_t Time_GetElapsedUSec(ltc_t const *t_start); +inline void Time_FromSec(ltc_t *t, uint32_t t_sec) +{ + assert(t != 0); + t->usec = 0; + t->sec = t_sec; +} + /*!*************************************************************************** - * @brief Obtains the elapsed milliseconds since a given time point. - * @details Wrap around effect due to uint32_t result format!! - * @param t_start Start time point. - * @return Elapsed milliseconds since t_start as uint32_t. + * @brief Checks if /p t1 is greater or equal that /p t2. + * @details Handles overflow. + * @param t1 1st operand. + * @param t2 2nd operand. + * @return Returns (t1 >= t2); *****************************************************************************/ -uint32_t Time_GetElapsedMSec(ltc_t const *t_start); +inline bool Time_GreaterEqual(ltc_t const *t1, ltc_t const *t2) +{ + assert(t1 != 0); + assert(t2 != 0); + return (t1->sec == t2->sec) ? (t1->usec >= t2->usec) : (t1->sec > t2->sec); +} + /*!*************************************************************************** - * @brief Obtains the elapsed seconds since a given time point. - * @param t_start Start time point. - * @return Elapsed seconds since t_start as uint32_t. + * @brief Obtains the elapsed time since MCU startup. + * @param t_now returned current time *****************************************************************************/ -uint32_t Time_GetElapsedSec(ltc_t const *t_start); +inline void Time_GetNow(ltc_t *t_now) +{ + assert(t_now != 0); + Timer_GetCounterValue(&(t_now->sec), &(t_now->usec)); + assert(t_now->usec < 1000000U); +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points. - * @details Result is defined as t_diff = t_end - t_start. - * Note: since no negative time differences are supported, t_end has - * to be later/larger than t_start. Otherwise, the result won't be - * a negative time span but given by max value. - * @param t_diff Returned time difference. - * @param t_start Start time point. - * @param t_end End time point. + * @brief Obtains the elapsed time since MCU startup. + * @return Returns the current time. *****************************************************************************/ -void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end); +inline ltc_t Time_Now(void) +{ + ltc_t t_now; + Time_GetNow(&t_now); + return t_now; +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points in - * microseconds. - * @details Result is defined as t_diff = t_end - t_start. - * Refers to Time_Diff() and handles overflow such that to large - * values are limited by 0xFFFFFFFF µs. - * @param t_start Start time point. - * @param t_end End time point. - * @return Time difference in microseconds. + * @brief Obtains the elapsed microseconds since MCU startup. + * @details Wrap around effect due to uint32_t result format!! + * @return Elapsed microseconds since MCU startup as uint32_t. *****************************************************************************/ -uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end); +inline uint32_t Time_GetNowUSec(void) +{ + ltc_t t_now = Time_Now(); + return Time_ToUSec(&t_now); +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points in - * milliseconds. - * @details Result is defined as t_diff = t_end - t_start. - * Refers to Time_Diff() and handles overflow. - * Wrap around effect due to uint32_t result format!! - * @param t_start Start time point. - * @param t_end End time point. - * @return Time difference in milliseconds. + * @brief Obtains the elapsed milliseconds (rounded) since MCU startup. + * @details Wrap around effect due to uint32_t result format!! + * @return Elapsed milliseconds since MCU startup as uint32_t. *****************************************************************************/ -uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end); +inline uint32_t Time_GetNowMSec(void) +{ + ltc_t t_now = Time_Now(); + return Time_ToMSec(&t_now); +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points in - * seconds. - * @details Result is defined as t_diff = t_end - t_start. - * Refers to Time_Diff() and handles overflow. - * @param t_start Start time point. - * @param t_end End time point. - * @return Time difference in seconds. + * @brief Obtains the elapsed seconds (rounded) since MCU startup. + * @return Elapsed seconds since MCU startup as uint32_t. *****************************************************************************/ -uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end); +inline uint32_t Time_GetNowSec(void) +{ + ltc_t t_now = Time_Now(); + return Time_ToSec(&t_now); +} + /*!*************************************************************************** - * @brief Time delay for a given time period. - * @param dt Delay time. + * @brief Obtains the time difference between two given time points. + * @details Result is defined as t_diff = t_end - t_start. + * Note: since no negative time differences are supported, t_end has + * to be later/larger than t_start. Otherwise, the result is undefined! + * @param t_diff Returned time difference. + * @param t_start Start time point. + * @param t_end End time point. *****************************************************************************/ -void Time_Delay(ltc_t const *dt); +inline void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end) +{ + assert(t_diff != 0); + assert(t_start != 0); + assert(t_end != 0); + assert(t_diff != t_start); + assert(t_diff != t_end); + assert(Time_GreaterEqual(t_end, t_start)); + + if (t_start->usec <= t_end->usec) { // no carry over + t_diff->sec = t_end->sec - t_start->sec; + t_diff->usec = t_end->usec - t_start->usec; + + } else { // with carry over + t_diff->sec = t_end->sec - 1 - t_start->sec; + t_diff->usec = (1000000U - t_start->usec) + t_end->usec; + } +} /*!*************************************************************************** - * @brief Time delay for a given time period in microseconds. - * @param dt_usec Delay time in microseconds. + * @brief Obtains the time difference between two given time points in + * microseconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow such that to large + * values are limited by 0xFFFFFFFF µs. + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in microseconds. *****************************************************************************/ -void Time_DelayUSec(uint32_t dt_usec); +inline uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end) +{ + ltc_t t_diff; + Time_Diff(&t_diff, t_start, t_end); + return Time_ToUSec(&t_diff); +} /*!*************************************************************************** - * @brief Time delay for a given time period in milliseconds. - * @param dt_msec Delay time in milliseconds. + * @brief Obtains the time difference between two given time points in + * milliseconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow. + * Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in milliseconds. *****************************************************************************/ -void Time_DelayMSec(uint32_t dt_msec); +inline uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end) +{ + ltc_t t_diff; + Time_Diff(&t_diff, t_start, t_end); + return Time_ToMSec(&t_diff); +} /*!*************************************************************************** - * @brief Time delay for a given time period in seconds. - * @param dt_sec Delay time in seconds. + * @brief Obtains the time difference between two given time points in + * seconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow. + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in seconds. *****************************************************************************/ -void Time_DelaySec(uint32_t dt_sec); +inline uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end) +{ + ltc_t t_diff; + Time_Diff(&t_diff, t_start, t_end); + return Time_ToSec(&t_diff); +} + /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout Timeout period. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed time since a given time point. + * @details Calculates the currently elapsed time since a specified start time + * (/p t_start). + * + * Note that /p t_start must be in the past! Otherwise, the behavior is + * undefined! + * + * @param t_elapsed Returns the elapsed time since /p t_start. + * @param t_start Start time point. *****************************************************************************/ -bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout); +inline void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start) +{ + assert(t_elapsed != 0); + assert(t_start != 0); + assert(t_elapsed != t_start); + ltc_t t_now = Time_Now(); + Time_Diff(t_elapsed, t_start, &t_now); +} /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout_usec Timeout period in microseconds. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed microseconds since a given time point. + * @details Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @return Elapsed microseconds since t_start as uint32_t. *****************************************************************************/ -bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec); +inline uint32_t Time_GetElapsedUSec(ltc_t const *t_start) +{ + assert(t_start != 0); + ltc_t t_now = Time_Now(); + return Time_DiffUSec(t_start, &t_now); +} /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout_msec Timeout period in milliseconds. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed milliseconds since a given time point. + * @details Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @return Elapsed milliseconds since t_start as uint32_t. *****************************************************************************/ -bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec); +inline uint32_t Time_GetElapsedMSec(ltc_t const *t_start) +{ + assert(t_start != 0); + ltc_t t_now = Time_Now(); + return Time_DiffMSec(t_start, &t_now); +} /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout_sec Timeout period in seconds. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed seconds since a given time point. + * @param t_start Start time point. + * @return Elapsed seconds since t_start as uint32_t. *****************************************************************************/ -bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec); +inline uint32_t Time_GetElapsedSec(ltc_t const *t_start) +{ + assert(t_start != 0); + ltc_t t_now = Time_Now(); + return Time_DiffSec(t_start, &t_now); +} + /*!*************************************************************************** - * @brief Adds two ltc_t values. - * @details Result is defined as t = t1 + t2. Results are wrapped around at - * maximum values just like integers. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2 2nd operand. + * @brief Adds two #ltc_t values. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t, t1 and t2 may point to the same instance(s). + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2 2nd operand. *****************************************************************************/ -void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2); +inline void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2) +{ + assert(t != 0); + assert(t1 != 0); + assert(t2 != 0); + + t->sec = t1->sec + t2->sec; + t->usec = t1->usec + t2->usec; + + if (t->usec > 999999U) { + t->sec += 1U; + t->usec -= 1000000U; + } +} /*!*************************************************************************** - * @brief Adds a given time in microseconds to an ltc_t value. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2_usec 2nd operand in microseconds. + * @brief Adds a given time in microseconds to an #ltc_t value. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t and t1 may point to the same instance. + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_usec 2nd operand in microseconds. *****************************************************************************/ -void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec); +inline void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec) +{ + assert(t != 0); + assert(t1 != 0); + ltc_t t2; + Time_FromUSec(&t2, t2_usec); + Time_Add(t, t1, &t2); +} /*!*************************************************************************** - * @brief Adds a given time in milliseconds to an ltc_t value. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2_msec 2nd operand in milliseconds. + * @brief Adds a given time in milliseconds to an #ltc_t value. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t and t1 may point to the same instance. + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_msec 2nd operand in milliseconds. *****************************************************************************/ -void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec); +inline void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec) +{ + assert(t != 0); + assert(t1 != 0); + ltc_t t2; + Time_FromMSec(&t2, t2_msec); + Time_Add(t, t1, &t2); +} /*!*************************************************************************** - * @brief Adds a given time in seconds to an ltc_t value. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2_sec 2nd operand in seconds. + * @brief Adds a given time in seconds to an #ltc_t value. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t and t1 may point to the same instance. + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_sec 2nd operand in seconds. *****************************************************************************/ -void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec); +inline void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec) +{ + assert(t != 0); + assert(t1 != 0); + ltc_t t2; + Time_FromSec(&t2, t2_sec); + Time_Add(t, t1, &t2); +} + /*!*************************************************************************** - * @brief Converts ltc_t to microseconds (uint32_t). - * @param t Input ltc_t struct. - * @return Time value in microseconds. + * @brief Checks if /p t is within the time interval /p t_start and /p t_end. + * @details The interval is from /p t_start to /p t_end. + * The function returns true if /p t >= /p t_start AND /p t < /p t_end. + * If /p t_end is before /p t_start, /p t_end is consider to be wrapped + * around and the condition inverts (i.e. the function returns true if + * /p < /p t_end OR /p t >= t_start. + * @param t_start The start of the time interval. + * @param t_end The end of the time interval. + * @param t The time to be checked if it is with the interval. + * @return True if t is within t_start and t_stop. *****************************************************************************/ -uint32_t Time_ToUSec(ltc_t const *t); +inline bool Time_CheckWithin(ltc_t const *t_start, ltc_t const *t_end, ltc_t const *t) +{ + if (Time_GreaterEqual(t_end, t_start)) { + return Time_GreaterEqual(t, t_start) && !Time_GreaterEqual(t, t_end); + + } else { + return Time_GreaterEqual(t, t_start) || !Time_GreaterEqual(t, t_end); + } +} + /*!*************************************************************************** - * @brief Converts ltc_t to milliseconds (uint32_t). - * @param t Input ltc_t struct. - * @return Time value in milliseconds. + * @brief Checks if timeout is reached from a given starting time. + * @details Checks if a specified time (/p t_timeout) has elapsed since a + * specified start time (/p t_start). + * Handles overflow/wraparound of time values at the maximum value. + * @param t_start Start time. + * @param t_timeout Timeout period. + * @return Timeout elapsed? True/False (boolean value) *****************************************************************************/ -uint32_t Time_ToMSec(ltc_t const *t); +inline bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout) +{ + assert(t_start != 0); + assert(t_timeout != 0); + + ltc_t t_end; + ltc_t t_now = Time_Now(); + Time_Add(&t_end, t_start, t_timeout); + return !Time_CheckWithin(t_start, &t_end, &t_now); +} /*!*************************************************************************** - * @brief Converts ltc_t to seconds (uint32_t). - * @param t Input ltc_t struct. - * @return Time value in seconds. + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_usec Timeout period in microseconds. + * @return Timeout elapsed? True/False (boolean value) *****************************************************************************/ -uint32_t Time_ToSec(ltc_t const *t); +inline bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec) +{ + ltc_t t_timeout; + Time_FromUSec(&t_timeout, t_timeout_usec); + return Time_CheckTimeout(t_start, &t_timeout); +} + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_msec Timeout period in milliseconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +inline bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec) +{ + ltc_t t_timeout; + Time_FromMSec(&t_timeout, t_timeout_msec); + return Time_CheckTimeout(t_start, &t_timeout); +} + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_sec Timeout period in seconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +inline bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec) +{ + ltc_t t_timeout; + Time_FromSec(&t_timeout, t_timeout_sec); + return Time_CheckTimeout(t_start, &t_timeout); +} + + +/*!*************************************************************************** + * @brief Time delay for a given time period. + * @param dt Delay time. + *****************************************************************************/ +inline void Time_Delay(ltc_t const *dt) +{ + assert(dt != 0); + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeout(&t_start, dt)); +} + +/*!*************************************************************************** + * @brief Time delay for a given time period in microseconds. + * @param dt_usec Delay time in microseconds. + *****************************************************************************/ +inline void Time_DelayUSec(uint32_t dt_usec) +{ + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeoutUSec(&t_start, dt_usec)); +} + +/*!*************************************************************************** + * @brief Time delay for a given time period in milliseconds. + * @param dt_msec Delay time in milliseconds. + *****************************************************************************/ +inline void Time_DelayMSec(uint32_t dt_msec) +{ + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeoutMSec(&t_start, dt_msec)); +} + +/*!*************************************************************************** + * @brief Time delay for a given time period in seconds. + * @param dt_sec Delay time in seconds. + *****************************************************************************/ +inline void Time_DelaySec(uint32_t dt_sec) +{ + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeoutSec(&t_start, dt_sec)); +} + /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* TIME_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a index c646ab3116..e4e6dc0c52 100644 Binary files a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a differ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a index 905b2fa66d..23bcd6fbea 100644 Binary files a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a differ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c index 5f8fd4b1f1..2e0e8dff16 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c @@ -1,10 +1,10 @@ /*************************************************************************//** - * @file argus_hal_test.c - * @brief Tests for the AFBR-S50 API hardware abstraction layer. + * @file + * @brief Tests for the AFBR-S50 API hardware abstraction layer. * * @copyright * - * Copyright (c) 2021, Broadcom, Inc. + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,6 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ +/*!*************************************************************************** + * @addtogroup argus_test + * @{ + *****************************************************************************/ /******************************************************************************* * Include Files @@ -46,44 +50,44 @@ #include "platform/argus_nvm.h" #include "platform/argus_irq.h" -#include -#include -#include - /******************************************************************************* * Definitions ******************************************************************************/ -/*! An error log message via print(); */ +/*! An error log message via #print function. */ #define error_log(fmt, ...) print("ERROR: " fmt "\n", ##__VA_ARGS__) /******************************************************************************* * Prototypes ******************************************************************************/ +static status_t VerifyHALImplementation(s2pi_slave_t spi_slave); + static status_t TimerPlausibilityTest(void); static status_t TimerWraparoundTest(void); static status_t SpiConnectionTest(s2pi_slave_t slave); -static status_t SpiInterruptTest(s2pi_slave_t slave); +static status_t SpiMaxLengthTest(s2pi_slave_t slave); +//static status_t SpiInterruptTest(s2pi_slave_t slave); +static status_t GpioInterruptTest(s2pi_slave_t slave); static status_t GpioModeTest(s2pi_slave_t slave); static status_t TimerTest(s2pi_slave_t slave); static status_t PITTest(void); +static status_t SpiTransferFromInterruptTest(s2pi_slave_t slave); static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct); -static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size); +static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, size_t size); static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim); -static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples); -static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms); +static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples, s2pi_callback_t callback, void *callbackData); static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom); static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *RcoTrim); static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples); static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n); static void PIT_Callback(void *param); -static void DataReadyCallback(void *param); +static void GPIO_Callback(void *param); /// @cond EXTERN extern uint32_t EEPROM_ReadChipId(uint8_t const *eeprom); -extern argus_module_version_t EEPROM_ReadModule(uint8_t const *eeprom); +extern uint8_t EEPROM_ReadModule(uint8_t const *eeprom); extern status_t EEPROM_Read(s2pi_slave_t slave, uint8_t address, uint8_t *data); extern uint8_t hamming_decode(uint8_t const *code, uint8_t *data); /// @endcond @@ -98,99 +102,129 @@ extern uint8_t hamming_decode(uint8_t const *code, uint8_t *data); status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave) { - status_t status = STATUS_OK; + print("########################################################\n"); + print("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); + print("########################################################\n"); + print("- SPI Slave: %d \n\n", spi_slave); - PX4_INFO_RAW("########################################################\n"); - PX4_INFO_RAW("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); - PX4_INFO_RAW("########################################################\n\n"); + const status_t status = VerifyHALImplementation(spi_slave); - PX4_INFO_RAW("1 > Timer Plausibility Test\n"); - status = TimerPlausibilityTest(); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("1 > PASS\n\n"); - - PX4_INFO_RAW("2 > Timer Wraparound Test\n"); - status = TimerWraparoundTest(); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("2 > PASS\n\n"); - - PX4_INFO_RAW("3 > SPI Connection Test\n"); - status = SpiConnectionTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("3 > PASS\n\n"); - - PX4_INFO_RAW("4 > SPI Interrupt Test\n"); - status = SpiInterruptTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("4 > PASS\n\n"); - - PX4_INFO_RAW("5 > GPIO Mode Test\n"); - status = GpioModeTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("5 > PASS\n\n"); - - PX4_INFO_RAW("6 > Lifetime Counter Timer (LTC) Test\n"); - status = TimerTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("6 > PASS\n\n"); - - PX4_INFO_RAW("7 > Periodic Interrupt Timer (PIT) Test\n"); - status = PITTest(); - - if (status == ERROR_NOT_IMPLEMENTED) { - PX4_INFO_RAW("7 > SKIPPED (PIT is not implemented)\n\n"); - - } else { - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("7 > PASS\n\n"); - } - - -summary: - PX4_INFO_RAW("########################################################\n"); + print("########################################################\n"); if (status != STATUS_OK) { - PX4_INFO_RAW("# FAIL: HAL Verification Test finished with error %d!\n", (int)status); + print("# FAIL: HAL Verification Test finished with error %d!\n", status); } else { - PX4_INFO_RAW("# PASS: HAL Verification Test finished successfully!\n"); + print("# PASS: HAL Verification Test finished successfully!\n"); } - PX4_INFO_RAW("########################################################\n\n"); + print("########################################################\n\n"); + return status; } /*!*************************************************************************** - * @brief Checks the validity of timer counter values. + * @brief Executes a series of tests in order to verify the HAL implementation. * - * @details This verifies that the counter values returned from the - * #Timer_GetCounterValue function are valid. This means, the low - * counter value \p lct is within 0 and 999999 µs. + * @details See #Argus_VerifyHALImplementation for details. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL on failure (check the error log for more information). + * @param spi_slave The SPI hardware slave. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +static status_t VerifyHALImplementation(s2pi_slave_t spi_slave) +{ + status_t status = STATUS_OK; + + print("1 > Timer Plausibility Test\n"); + status = TimerPlausibilityTest(); + + if (status != STATUS_OK) { return status; } + + print("1 > PASS\n\n"); + + print("2 > Timer Wraparound Test\n"); + status = TimerWraparoundTest(); + + if (status != STATUS_OK) { return status; } + + print("2 > PASS\n\n"); + + print("3 > SPI Connection Test\n"); + status = SpiConnectionTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("3 > PASS\n\n"); + + print("4 > SPI Maximum Data Length Test\n"); + status = SpiMaxLengthTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("4 > PASS\n\n"); + + print("5 > GPIO Interrupt Test\n"); + status = GpioInterruptTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("5 > PASS\n\n"); + + print("6 > GPIO Mode Test\n"); + status = GpioModeTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("6 > PASS\n\n"); + + print("7 > Lifetime Counter Timer (LTC) Test\n"); + status = TimerTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("7 > PASS\n\n"); + + print("8 > Periodic Interrupt Timer (PIT) Test\n"); + status = PITTest(); + + if (status == ERROR_NOT_IMPLEMENTED) { + print("8 > SKIPPED (PIT is not implemented)\n\n"); + + } else { + if (status != STATUS_OK) { return status; } + + print("8 > PASS\n\n"); + } + + print("9 > SPI Interrupt Test\n"); + status = SpiTransferFromInterruptTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("9 > PASS\n\n"); + + return status; +} + +/*!*************************************************************************** + * @brief Checks the validity of timer counter values. + * + * @details This verifies that the counter values returned from the + * #Timer_GetCounterValue function are valid. This means, the low + * counter value \p lct is within 0 and 999999 µs. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). *****************************************************************************/ static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct) { if (lct > 999999) { - PX4_INFO_RAW("Timer plausibility check:\n" - "The parameter \"lct\" of Timer_GetCounterValue() must always " - "be within 0 and 999999.\n" - "Current Values: hct = %d, lct = %d", (uint)hct, (uint)lct); + error_log("Timer plausibility check:\n" + "The parameter \"lct\" of Timer_GetCounterValue() must always " + "be within 0 and 999999.\n" + "Current Values: hct = %d, lct = %d", hct, lct); return ERROR_FAIL; } @@ -198,24 +232,24 @@ static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct) } /*!*************************************************************************** - * @brief Plausibility Test for Timer HAL Implementation. + * @brief Plausibility Test for Timer HAL Implementation. * - * @details Rudimentary tests the lifetime counter (LTC) implementation. - * This verifies that the LTC is running by checking if the returned - * values of two consecutive calls to the #Timer_GetCounterValue - * function are ascending. An artificial delay using the NOP operation - * is induced such that the timer is not read to fast. + * @details Rudimentary tests the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. * * @warning If using an ultra-fast processor with a rather low timer granularity, - * the test may fail! In this case, it could help to increase the delay - * by increasing the for-loop exit criteria. + * the test may fail! In this case, it could help to increase the delay + * by increasing the for-loop exit criteria. * - * @warning This test does not test yet verify if the timing is correct at all! - * This it done in later test... + * @warning This test does not test yet verify if the timing is correct at all! + * This it done in later test... * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL on failure (check the error log for more information). + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). *****************************************************************************/ static status_t TimerPlausibilityTest(void) { @@ -230,7 +264,7 @@ static status_t TimerPlausibilityTest(void) /* Check max value is not exceeded for LCT timer (us) */ status_t status = CheckTimerCounterValues(hct0, lct0); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Adding a delay. Depending on MCU speed, this takes any time. * However, the Timer should be able to solve this on any MCU. */ @@ -242,18 +276,18 @@ static status_t TimerPlausibilityTest(void) /* Check max value is not exceeded for LCT timer (us) */ status = CheckTimerCounterValues(hct1, lct1); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Either the hct value must have been increased or the lct value if the hct * value is still the same. */ if (!((hct1 > hct0) || ((hct1 == hct0) && (lct1 > lct0)))) { - PX4_INFO_RAW("Timer plausibility check: the elapsed time could not be " - "measured with the Timer_GetCounterValue() function; no time " - "has elapsed!\n" - "The delay was induced by the following code:\n" - "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n" - "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", - (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); + error_log("Timer plausibility check: the elapsed time could not be " + "measured with the Timer_GetCounterValue() function; no time " + "has elapsed!\n" + "The delay was induced by the following code:\n" + "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n", + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + hct0, lct0, hct1, lct1); return ERROR_FAIL; } @@ -261,29 +295,29 @@ static status_t TimerPlausibilityTest(void) } /*!*************************************************************************** - * @brief Wraparound Test for the Timer HAL Implementation. + * @brief Wraparound Test for the Timer HAL Implementation. * * @details The LTC values must wrap from 999999 µs to 0 µs and increase the * seconds counter accordingly. This test verifies the correct wrapping * by consecutively calling the #Timer_GetCounterValue function until * at least 2 wraparound events have been occurred. * - * @note This test requires the timer to basically run and return ascending - * values. Also, if the timer is too slow, this may take very long! - * Usually, the test takes 2 seconds, since 2 wraparound events are - * verified. + * @note This test requires the timer to basically run and return ascending + * values. Also, if the timer is too slow, this may take very long! + * Usually, the test takes 2 seconds, since 2 wraparound events are + * verified. * - * @warning This test does not test yet verify if the timing is correct at all! - * This it done in later test... + * @warning This test does not test yet verify if the timing is correct at all! + * This it done in later test... * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL on failure (check the error log for more information). + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). *****************************************************************************/ static status_t TimerWraparoundTest(void) { /* Test parameter configuration: *****************************************/ - const int8_t n = 2; // The number of wraparounds to test. + const uint8_t n = 2; // The number of wraparounds to test. /*************************************************************************/ uint32_t hct0 = 0; @@ -297,14 +331,12 @@ static status_t TimerWraparoundTest(void) /* Check max value is not exceeded for LCT timer (us) */ status_t status = CheckTimerCounterValues(hct0, lct0); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Set end after 2 seconds, i.e. 2 wrap around events. */ uint32_t hct2 = hct0 + n; uint32_t lct2 = lct0; - px4_usleep(20000); - /* Periodically read timer values. From previous tests we * already know the timer value is increasing. */ while (hct0 < hct2 || lct0 < lct2) { @@ -315,69 +347,92 @@ static status_t TimerWraparoundTest(void) /* Check max value is not exceeded for LCT timer (us) */ status = CheckTimerCounterValues(hct0, lct0); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Testing if calls to Timer_GetCounterValue are equal or increasing. * Also testing if wraparound is correctly handled. * Assumption here is that two sequential calls to the get functions are - * only a few µs appart! I.e. if hct wraps, the new lct must be smaller + * only a few µs apart! I.e. if hct wraps, the new lct must be smaller * than previous one. */ if (!(((hct1 == hct0 + 1) && (lct1 < lct0)) || ((hct1 == hct0) && (lct1 >= lct0)))) { - PX4_INFO_RAW("Timer plausibility check: the wraparound of \"lct\" or " - "\"hct\" parameters of the Timer_GetCounterValue() " - "function was not handled correctly!\n" - "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", - (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); + error_log("Timer plausibility check: the wraparound of \"lct\" or " + "\"hct\" parameters of the Timer_GetCounterValue() " + "function was not handled correctly!\n" + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + hct0, lct0, hct1, lct1); return ERROR_FAIL; } hct0 = hct1; lct0 = lct1; - - px4_usleep(20000); } return STATUS_OK; } /*!*************************************************************************** - * @brief Helper function for transfer data to SPI in blocking mode. + * @brief SPI interrupt callback function for the SPI transfer interrupt test. + * + * @details The interrupt callback is invoked from the S2PI module upon + * finishing the SPI transfer. The callback is used by the + * #SPITransferSync helper function to retrieve the status of the + * SPI transfer. + * + * @param status The S2PI module status passed to the callback. + * @param param The abstract interrupt callback parameter. + * + * @return Returns #STATUS_OK. + *****************************************************************************/ +static status_t SpiTransferInterruptCallback(status_t status, void *param) +{ + *((status_t *)param) = status; + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Helper function for transfer data to SPI in blocking mode. * * @details Calls the #S2PI_TransferFrame function and waits until the transfer - * has been finished by checking the #S2PI_GetStatus return code to - * become #STATUS_IDLE (or #STATUS_OK). + * has been finished by checking the #S2PI_GetStatus return code to + * become #STATUS_IDLE (or #STATUS_OK). * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param data The data array to be transfered. - * @param size The size of the data array to be transfered. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param data The data array to be transferred. + * @param size The size of the data array to be transferred. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the operation did not finished within a specified - * time (check also timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ -static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) +static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, size_t size) { /* Test parameter configuration: *****************************************/ - const uint32_t timeout_ms = 100; // The transfer timeout in ms. + const uint32_t timeout_ms = 100; // The transfer timeout in ms. /*************************************************************************/ - status_t status = S2PI_TransferFrame(slave, data, data, size, 0, 0); + /* The status will be changed in the SPI callback. */ + volatile status_t callbackStatus = STATUS_BUSY; - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI transfer failed! The call to S2PI_TransferFrame " - "yielded error code: %d", (int)status); + status_t status = S2PI_TransferFrame(slave, data, data, size, + SpiTransferInterruptCallback, + (void *)&callbackStatus); + + if (status != STATUS_OK) { + error_log("SPI transfer failed! The call to S2PI_TransferFrame " + "yielded error code: %d", + status); return status; } @@ -388,53 +443,67 @@ static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) Time_GetNow(&start); do { - status = S2PI_GetStatus(); + status = S2PI_GetStatus(slave); if (status < STATUS_OK) { - PX4_INFO_RAW("SPI transfer failed! The call to S2PI_GetStatus " - "yielded error code: %d", (int)status); - S2PI_Abort(); + error_log("SPI transfer failed! The call to S2PI_GetStatus " + "yielded error code: %d", status); + S2PI_Abort(slave); return status; } if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - PX4_INFO_RAW("SPI transfer failed! The operation did not finished " - "within %u ms. This may also be caused by an invalid " - "timer implementation!", (uint)timeout_ms); + error_log("SPI transfer failed! The operation did not finished " + "within %d ms. This may also be caused by an invalid " + "timer implementation!", timeout_ms); return ERROR_TIMEOUT; } } while (status == STATUS_BUSY); + if (callbackStatus != STATUS_OK) { + error_log("Invocation of the SPI callback failed! The SPI transfer " + "callback yielded error code: %d", callbackStatus); + return callbackStatus; + } + return status; } /*!*************************************************************************** - * @brief SPI Connection Test for S2PI HAL Implementation. + * @brief SPI Connection Test for S2PI HAL Implementation. * * @details This test verifies the basic functionality of the SPI interface. - * The test utilizes the devices laser pattern register, which can - * be freely programmed by any 128-bit pattern. Thus, it writes a byte - * sequence and reads back the written values on the consecutive SPI - * access. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * Note: The test verifies the SPI interface transfer functionality + * in blocking mode and also verifies the interrupt callback. + * In order to wait for the transfer to finish, it reads the S2PI + * status in a loop. If the status does not change to #STATUS_IDLE, + * the test will fail with an #ERROR_TIMEOUT. Finally, the test will + * verify the SPI transfer callback status. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the operation did not finished within a specified - * time (check also timer HAL implementation). - * - #ERROR_FAIL if the device access failed and the read data did not - * match the expected values. - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or the SPI callback yield any negative status. *****************************************************************************/ static status_t SpiConnectionTest(s2pi_slave_t slave) { @@ -448,8 +517,8 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) status = SPITransferSync(slave, data, 17U); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI connection test failed!"); + if (status != STATUS_OK) { + error_log("SPI connection test failed!"); return status; } @@ -460,18 +529,18 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) status = SPITransferSync(slave, data, 17U); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI connection test failed!"); + if (status != STATUS_OK) { + error_log("SPI connection test failed!"); return status; } /* Verify the read pattern. */ for (uint8_t i = 1; i < 17U; ++i) { if (data[i] != i) { - PX4_INFO_RAW("SPI connection test failed!\n" - "Verification of read data is invalid!\n" - "read_data[%d] = %d, but expected was %d", - i, data[i], i); + error_log("SPI connection test failed!\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data[i], i); return ERROR_FAIL; } } @@ -479,142 +548,236 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) return STATUS_OK; } - /*!*************************************************************************** - * @brief The data ready callback invoked by the API. + * @brief Maximum SPI Data Size Test for S2PI HAL Implementation. * - * @details The callback is invoked by the API when the device GPIO IRQ is - * pending after a measurement has been executed and data is ready to - * be read from the device. + * @details This test verifies the maximum data transfer length of the SPI + * interface. The test sends and receives up to 396 data bytes plus + * a single address byte over the SPI interface and verifies that no + * data get lost. * - * @param param The abstract pointer to the boolean value that determines if - * the callback is invoked. + * The test utilizes the channel select register which is 3 bytes plus + * address. This register can be repeatedly written with any pattern + * using the DMA mode. The register is written 100 times in a row + * to verify that long data frames with up to 400 bytes can be + * transmitted. + * + * Note that this test was motivated by an invalid implementation that + * used uint8_t type for the frame length in the #S2PI_TransferFrame + * function instead of an uint16_t value. This resulted in a maximum + * data length of 141 bytes (367 & 0xFF = 141) when reading the + * data value register. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ -static void DataReadyCallback(void *param) +static status_t SpiMaxLengthTest(s2pi_slave_t slave) { - IRQ_LOCK(); - *((bool *) param) = true; - IRQ_UNLOCK(); + status_t status = STATUS_OK; + uint8_t data[400U] = { 0 }; + + /* Setup device (enable DMA mode). */ + data[0] = 0x10; data[1] = 0x12; + status = SPITransferSync(slave, data, 2); + + if (status != STATUS_OK) { + error_log("Device configuration failed!"); + return status; + } + + data[0] = 0x12; data[1] = 0x00; data[2] = 0x2B; + status = SPITransferSync(slave, data, 3); + + if (status != STATUS_OK) { + error_log("Device configuration failed!"); + return status; + } + + /* Transfer a pattern to the register */ + for (uint32_t i = 0; i < sizeof(data); i += 4) { + data[i + 0] = 0x1E; // Address + data[i + 1] = (uint8_t)i; // Random Data Byte 0 + data[i + 2] = (uint8_t)(i + 1); // Random Data Byte 1 + data[i + 3] = (uint8_t)(i * 2); // Random Data Byte 2 + } + + status = SPITransferSync(slave, data, sizeof(data)); + + if (status != STATUS_OK) { + error_log("SPI maximum data length test failed!"); + return status; + } + + /* Repeat ... */ + for (uint32_t i = 0; i < sizeof(data); i += 4) { + data[i + 0] = 0x1E; // Address + data[i + 1] = (uint8_t)i; // Random Data Byte 0 + data[i + 2] = (uint8_t)(i + 1); // Random Data Byte 1 + data[i + 3] = (uint8_t)(i * 2); // Random Data Byte 2 + } + + status = SPITransferSync(slave, data, sizeof(data)); + + if (status != STATUS_OK) { + error_log("SPI maximum data length test failed!"); + return status; + } + + /* Verify the read pattern; skip all address bytes. */ + for (uint32_t i = 0; i < sizeof(data); i += 4) { + uint32_t j = (i + 4) % sizeof(data); + + if (data[j + 1] != (uint8_t)i + || data[j + 2] != (uint8_t)(i + 1) + || data[j + 3] != (uint8_t)(i * 2)) { + error_log("SPI maximum data length test failed!\n" + "Verification of read data is invalid at byte %d!\n" + " - expected: 0x%02X%02X%02X\n" + " - actual: 0x%02X%02X%02X", + i, (uint8_t)i, (uint8_t)(i + 1), (uint8_t)(i * 2), + data[j + 1], data[j + 2], data[j + 3]); + return ERROR_FAIL; + } + } + + return STATUS_OK; } /*!*************************************************************************** - * @brief Configures the device with a bare minimum setup to run the tests. + * @brief Configures the device with a bare minimum setup to run the tests. * - * @details This function applies a number of configuration values to the - * device, such that a pseudo measurement w/o laser output can be - * performed. + * @details This function applies a number of configuration values to the + * device, such that a pseudo measurement w/o laser output can be + * performed. * - * A \p rcoTrim parameter can be passed to adjust the actual clock - * setup. + * A \p rcoTrim parameter can be passed to adjust the actual clock + * setup. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param rcoTrim The RCO Trimming value added to the nominal RCO register - * value. Pass 0 if no fine tuning is required. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param rcoTrim The RCO Trimming value added to the nominal RCO register + * value. Pass 0 if no fine tuning is required. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the SPI operation did not finished within a - * specified time (check also timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the SPI operation did not finished within a + * specified time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) { /* Setup Device and Trigger Measurement. */ - uint16_t v = 0x0010U | (((34 + rcoTrim) & 0x3F) << 6); - uint8_t d1[] = { 0x14, v >> 8, v & 0xFF, 0x21 }; + assert(rcoTrim >= -34 && rcoTrim < 0x3F - 34); + const uint16_t v = (uint16_t)(0x0010U | (((uint16_t)(34 + rcoTrim) & 0x3F) << 6U)); + uint8_t d1[] = { 0x14U, (uint8_t)(v >> 8U), v & 0xFFU, 0x21U }; status_t status = SPITransferSync(slave, d1, sizeof(d1)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d2[] = { 0x16, 0x7F, 0xFF, 0x7F, 0xE9 }; status = SPITransferSync(slave, d2, sizeof(d2)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d3[] = { 0x18, 0x00, 0x00, 0x03 }; status = SPITransferSync(slave, d3, sizeof(d3)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d4[] = { 0x10, 0x12 }; status = SPITransferSync(slave, d4, sizeof(d4)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d5[] = { 0x12, 0x00, 0x2B }; status = SPITransferSync(slave, d5, sizeof(d5)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d6[] = { 0x08, 0x04, 0x84, 0x10 }; status = SPITransferSync(slave, d6, sizeof(d6)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d7[] = { 0x0A, 0xFE, 0x51, 0x0F, 0x05 }; status = SPITransferSync(slave, d7, sizeof(d7)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d8[] = { 0x0C, 0x00, 0x00, 0x00 }; status = SPITransferSync(slave, d8, sizeof(d8)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d9[] = { 0x1E, 0x00, 0x00, 0x00 }; status = SPITransferSync(slave, d9, sizeof(d9)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d10[] = { 0x20, 0x01, 0xFF, 0xFF }; status = SPITransferSync(slave, d10, sizeof(d10)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d11[] = { 0x22, 0xFF, 0xFF, 0x04 }; status = SPITransferSync(slave, d11, sizeof(d11)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } @@ -622,40 +785,55 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) } /*!*************************************************************************** - * @brief Triggers a measurement on the device with specified sample count. + * @brief Triggers a measurement on the device with specified sample count. * * @details The function triggers a measurement cycle on the device. A - * \p sample count can be specified to setup individual number of - * digital averaging. + * \p sample count can be specified to setup individual number of + * digital averaging. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * The measurement in triggered asynchronously without waiting + * for any event to finish. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param samples The specified number of averaging samples for the measurement. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the operation did not finished within a specified - * time (check also timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param samples The specified number of averaging samples for the measurement. + * @param callback An optional SPI callback. + * @param callbackData The optional callback data parameter. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ -static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples) +static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples, + s2pi_callback_t callback, void *callbackData) { // samples is zero based, i.e. writing 0 yields 1 sample samples = samples > 0 ? samples - 1 : samples; - uint16_t v = 0x8000U | ((samples & 0x03FFU) << 5U); - uint8_t d[] = { 0x1C, v >> 8, v & 0xFFU }; - status_t status = SPITransferSync(slave, d, sizeof(d)); + const uint16_t v = (uint16_t)(0x8000U | ((samples & 0x03FFU) << 5U)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Trigger measurement failed!"); + // data is static as the transfer is asynchronous and the buffer must persist. + static uint8_t data[] = { 0x1CU, 0x00U, 0x00U }; + data[0] = 0x1CU; + data[1] = (uint8_t)(v >> 8U); + data[2] = v & 0xFFU; + + status_t status = S2PI_TransferFrame(slave, data, data, sizeof(data), + callback, callbackData); + + if (status != STATUS_OK) { + error_log("SPI transfer failed to trigger measurements! " + "The call to S2PI_TransferFrame yielded error code: %d", + status); return status; } @@ -663,162 +841,263 @@ static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples) } /*!*************************************************************************** - * @brief Waits for the data ready interrupt to be pending. + * @brief Data structure for the GPIO interrupt test. * - * @details The function polls the current interrupt pending state of the data - * ready interrupt from the device, i.e. reads the IRQ GPIO pin until - * it is pulled to low by the device. - * - * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. - * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param timeout_ms The timeout to cancel waiting for the IRQ. - * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus - * or #S2PI_SetIrqCallback return any negative status. + * @details Contains data that is required by the GPIO interrupt test. *****************************************************************************/ -static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms) -{ - ltc_t start; - Time_GetNow(&start); +typedef struct gpio_data_t { + /* The S2PI slave parameter passed to the S2PI HAL functions. */ + s2pi_slave_t Slave; - while (S2PI_ReadIrqPin(slave)) { - if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did not " - "determine an pending interrupt within %u ms.", (uint)timeout_ms); - return ERROR_TIMEOUT; - } + /* The callback status. */ + volatile status_t Status; + + /* The GPIO timeout in milliseconds. */ + uint32_t Timeout_ms; + + /* A counter to determine how often the callback is invoked. */ + volatile uint32_t CallbackInvoked; + + /* The return value of the #S2PI_ReadIrqPin function. */ + volatile uint32_t ReadIrqPinValue; + +} gpio_data_t; + +/*!*************************************************************************** + * @brief The IRQ callback dedicated to the #GpioInterruptTest. + * + * @details The callback is invoked by the API when the device GPIO IRQ is + * pending after a measurement has been executed and data is ready to + * be read from the device. + * + * @param param The abstract pointer to the boolean value that determines if + * the callback is invoked. + *****************************************************************************/ +static void GPIO_Callback(void *param) +{ + if (param == NULL) { + error_log("GPIO interrupt test failed: callback parameter \"param\" was NULL!"); + return; } + gpio_data_t *data = (gpio_data_t *)param; + data->CallbackInvoked = 1; +} + +/*!*************************************************************************** + * @brief The SPI transfer callback dedicated to the #GpioInterruptTest. + * + * @details The callback is invoked by the S2PI layer when the SPI transfer + * finished IRQ is invoked. The callback is used to simulate a + * deferred GPIO interrupt by locking the interrupts until the + * #S2PI_ReadIrqPin detects an GPIO interrupt pending state and + * returns 0. + * + * @param status The status of the SPI transfer. + * @param param The abstract pointer to the boolean value that determines if + * the callback is invoked. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the \p param parameter is NULL. + * - #ERROR_TIMEOUT if the #S2PI_ReadIrqPin does not return 0 after + * a specified time (check also timer HAL implementation). + * - The S2PI layer error code that may be received from the S2PI + * module via the \p status parameter. + *****************************************************************************/ +static status_t GPIO_SPI_Callback(status_t status, void *param) +{ + IRQ_LOCK(); // prevents GPIO interrupt to preempt if set to higher priority. + + if (param == NULL) { + IRQ_UNLOCK(); + error_log("GPIO interrupt test failed: callback parameter \"param\" was NULL!"); + return ERROR_FAIL; + } + + gpio_data_t *data = (gpio_data_t *)param; + + if (status != STATUS_OK) { + IRQ_UNLOCK(); + error_log("GPIO interrupt test failed: callback parameter \"status\" was %d!", + status); + data->Status = status; + return status; + } + + /* The S2PI_ReadIrqPin must correctly return the GPIO IRQ state if the GPIO + * interrupt is pending but deferred due to any higher priority or critical + * sections. Therefore, the SPI callback with the #IRQ_LOCK/#IRQ_UNLOCK is + * used to delay the GPIO callback and test the #S2PI_ReadIrqPin function. + * + * The purpose is to simulate a delayed GPIO interrupt that can in the + * production code happen due to any higher priority interrupts (such as + * the SPI interrupt in this test). In those cases, the API relies on the + * #S2PI_ReadIrqPin method to obtain if the device has finished in time and + * the interrupt is already pending. Otherwise, it would fail with an + * timeout due to the deferred GPIO interrupt callback event. */ + + ltc_t start; + Time_GetNow(&start); + data->ReadIrqPinValue = S2PI_ReadIrqPin(data->Slave); + + while (data->ReadIrqPinValue) { + if (Time_CheckTimeoutMSec(&start, data->Timeout_ms)) { + IRQ_UNLOCK(); + error_log("GPIO interrupt test failed! The IRQ pin did not assert " + "to low state when reading from the IRQ callback. " + "Elapsed %d ms.", data->Timeout_ms); + data->Status = ERROR_TIMEOUT; + return ERROR_TIMEOUT; + } + + data->ReadIrqPinValue = S2PI_ReadIrqPin(data->Slave); + } + + IRQ_UNLOCK(); + data->Status = STATUS_OK; return STATUS_OK; } /*!*************************************************************************** - * @brief SPI Interrupt Test for S2PI HAL Implementation. + * @brief SPI Interrupt Test for S2PI HAL Implementation. * - * @details This test verifies the correct implementation of the device - * integration finished interrupt callback. Therefore it configures - * the device with a minimal setup to run a pseudo measurement that - * does not emit any laser light. + * @details This test verifies the correct implementation of the device + * integration finished interrupt callback, a.k.a. the GPIO interrupt. + * Therefore it configures the device with a minimal setup to run a + * pseudo measurement that does not emit any laser light but triggers + * an GPIO interrupt once finished. * - * Note that this test does verify the GPIO interrupt that occurs - * whenever the device has finished the integration/measurement and - * new data is waiting to be read from the device. This does not test - * the interrupt that is triggered when the SPI transfer has finished. + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs and + * the #S2PI_ReadIrqPin function to obtain the pending interrupt state. * - * The data ready interrupt implies two S2PI layer functions that - * are tested in this test: The #S2PI_SetIrqCallback function installs - * a callback function that is invoked whenever the IRQ occurs. - * The IRQ can be delayed due to higher priority task, e.g. from the - * user code. It is essential for the laser safety timeout algorithm - * to determine the device ready signal as fast as possible, another - * method is implemented to read if the IRQ is pending but the - * callback has not been reset yet. This is what the #S2PI_ReadIrqPin - * function is for. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible. Thus a + * method is required to obtain if the IRQ is currently pending but + * the callback has not been invoked yet. This is what the + * #S2PI_ReadIrqPin function is for. Note that the #S2PI_ReadIrqPin + * must return 0 if not interrupt is pending and 1 else. Just like + * the IRQ pin is active low. * + * The test simulate a delayed GPIO interrupt by locking the interrupts + * until the #S2PI_ReadIrqPin detects an GPIO interrupt pending state + * and returns 0. This is done by the #GPIO_SPI_Callback function. + * + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. * * @warning The test assumes the device is in a fresh power on state and no - * additional reset is required. If the test fail, one may want to - * power cycle the device and try again. + * additional reset is required. If the test fail, one may want to + * power cycle the device and try again. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test locks the interrupts for a quite long period of time in + * order to simulate a delayed GPIO interrupt. This is not a good + * practice in production code. However, it is required to test the + * #S2PI_ReadIrqPin function. Please be aware of that when you run + * this test. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - #ERROR_FAIL if the IRQ pin readout failed and the no or invalid - * interrupt was detected. - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus - * or #S2PI_SetIrqCallback return any negative status. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - #ERROR_FAIL if the IRQ pin readout failed and the no or invalid + * interrupt was detected. + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. *****************************************************************************/ -static status_t SpiInterruptTest(s2pi_slave_t slave) +static status_t GpioInterruptTest(s2pi_slave_t slave) { /* Test parameter configuration: *****************************************/ const uint32_t timeout_ms = 300; // timeout for measurement, might be increased.. /*************************************************************************/ - /* Install IRQ callback. */ - volatile bool isDataReady = false; - status_t status = S2PI_SetIrqCallback(slave, DataReadyCallback, (void *)&isDataReady); + gpio_data_t data = { .Slave = slave, + .Status = ERROR_FAIL, + .Timeout_ms = timeout_ms, + .ReadIrqPinValue = 12345, + .CallbackInvoked = 0 + }; - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " - "yielded error code: %d", (int)status); + /* Install IRQ callback. */ + status_t status = S2PI_SetIrqCallback(slave, GPIO_Callback, &data); + + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed! The call to S2PI_SetIrqCallback " + "yielded error code: %d", status); + return status; + } + + /* Setup Device. */ + status = ConfigureDevice(slave, 0); + + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed!"); return status; } /* Check if IRQ is not yet pending. */ if (S2PI_ReadIrqPin(slave) == 0) { - PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did " - "return 0 but no interrupt is pending since no " - "measurements are executed yet!"); + error_log("GPIO interrupt test failed! The S2PI_ReadIrqPin did " + "return 0 but no interrupt is pending since no " + "measurements are executed yet!"); return ERROR_FAIL; }; - /* Setup Device. */ - status = ConfigureDevice(slave, 0); - - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed!"); - return status; - } - /* Trigger Measurement. */ - status = TriggerMeasurement(slave, 0); + status = TriggerMeasurement(slave, 0, GPIO_SPI_Callback, &data); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed!"); - return status; - } - - ltc_t start; - Time_GetNow(&start); - - /* Wait for Interrupt using the S2PI_ReadIrqPin method. */ - status = AwaitDataReady(slave, timeout_ms); - - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed!"); + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed!"); return status; } /* Wait for Interrupt using the callback method. */ - while (!isDataReady) { + ltc_t start; + Time_GetNow(&start); + + while (!data.CallbackInvoked) { if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - PX4_INFO_RAW("SPI interrupt test failed! The IRQ callback was not " - "invoked within %u ms.", (uint)timeout_ms); + error_log("GPIO interrupt test failed! The IRQ callback was not " + "invoked within %d ms.", timeout_ms); return ERROR_TIMEOUT; } } + /* Verify ... */ + if (data.Status != STATUS_OK) { + error_log("GPIO interrupt test failed! The SPI IRQ callback yielded " + "an error status: %d (expected 0)", data.Status); + return ERROR_FAIL; + } + + if (data.ReadIrqPinValue != 0) { + error_log("GPIO interrupt test failed! The IRQ pin returned " + "the wrong value: %d (expected 0)", data.ReadIrqPinValue); + return ERROR_FAIL; + } + /* Remove callback. */ status = S2PI_SetIrqCallback(slave, 0, 0); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " - "with null pointers yielded error code: %d", (int)status); + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed! The call to S2PI_SetIrqCallback " + "with null pointers yielded error code: %d", status); return status; } @@ -826,28 +1105,28 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) } /*!*************************************************************************** - * @brief Reads the EEPROM bytewise and applies Hamming weight. - * @details The EEPROM bytes are consecutevly read from the device via GPIO mode. - * The #EEPROM_Read function is an internal API function that enables - * the GPIO mode from the S2PI module and reads the data via a software - * bit-banging protocol. Finally it disables the GPIO mode and returns - * to SPI mode. + * @brief Reads the EEPROM byte-wise and applies Hamming weight. + * @details The EEPROM bytes are consecutively read from the device via GPIO mode. + * The EEPROM_Read function is an internal API function that enables + * the GPIO mode from the S2PI module and reads the data via a software + * bit-banging protocol. Finally it disables the GPIO mode and returns + * to SPI mode. * - * The calls to S2PI HAL module is as follows: - * 1. S2PI_CaptureGpioControl - * 2. multiple calls to S2PI_WriteGpioPin and S2PI_ReadGpioPin - * 3. S2PI_ReleaseGpioControl + * The calls to S2PI HAL module is as follows: + * 1. S2PI_CaptureGpioControl + * 2. multiple calls to S2PI_WriteGpioPin and S2PI_ReadGpioPin + * 3. S2PI_ReleaseGpioControl * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param eeprom The 16 byte array to be filled with EEPROM data. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param eeprom The 16 byte array to be filled with EEPROM data. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. - * interrupt was detected. - * - The S2PI layer error code if #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or - * #S2PI_ReadGpioPin return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * interrupt was detected. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. *****************************************************************************/ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) { @@ -855,9 +1134,9 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t d1[] = { 0x12, 0x00, 0x4B }; status_t status = SPITransferSync(slave, d1, sizeof(d1)); - if (status < STATUS_OK) { - PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " - "error code: %d", (int)status); + if (status != STATUS_OK) { + error_log("EEPROM readout failed (enable EEPROM), " + "error code: %d", status); return status; } @@ -868,8 +1147,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) status = EEPROM_Read(slave, address, &data[address]); if (status != STATUS_OK) { - PX4_INFO_RAW("EEPROM readout failed @ address 0x%02x, " - "error code: %d!", address, (int)status); + error_log("EEPROM readout failed @ address 0x%02x, " + "error code: %d!", address, status); return status; } } @@ -878,9 +1157,9 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t d2[] = { 0x12, 0x00, 0x2B }; status = SPITransferSync(slave, d2, sizeof(d2)); - if (status < STATUS_OK) { - PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " - "error code: %d", (int)status); + if (status != STATUS_OK) { + error_log("EEPROM readout failed (enable EEPROM), " + "error code: %d", status); return status; } @@ -888,8 +1167,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t err = hamming_decode(data, eeprom); if (err != 0) { - PX4_INFO_RAW("EEPROM readout failed! Failed to decoding " - "Hamming weight (error: %d)!", err); + error_log("EEPROM readout failed! Failed to decoding " + "Hamming weight (Hamming parity error: %d)!", err); return STATUS_ARGUS_EEPROM_BIT_ERROR; } @@ -900,30 +1179,30 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) } /*!*************************************************************************** - * @brief GPIO Mode Test for S2PI HAL Implementation. + * @brief GPIO Mode Test for S2PI HAL Implementation. * * @details This test verifies the GPIO mode of the S2PI HAL module. This is - * done by leveraging the EEPROM readout sequence that accesses the - * devices EEPROM via a software protocol that depends on the GPIO - * mode. + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. * - * This the requires several steps, most of them are already verified - * in previous tests: - * - Basic device configuration and enable EEPROM. - * - Read EERPOM via GPIO mode and apply Hamming weight - * - Repeat several times (to eliminate random readout issues). - * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c) - * - Check if Module Number and Chip ID is not 0 + * This the requires several steps, most of them are already verified + * in previous tests: + * - Basic device configuration and enable EEPROM. + * - Read EEPROM via GPIO mode and apply Hamming weight + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c) + * - Check if Module Number and Chip ID is not 0 * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL if the GPIO test fails. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. - * - The S2PI layer error code if #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or - * #S2PI_ReadGpioPin return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the GPIO test fails. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. *****************************************************************************/ static status_t GpioModeTest(s2pi_slave_t slave) { @@ -934,66 +1213,66 @@ static status_t GpioModeTest(s2pi_slave_t slave) status_t status = ReadEEPROM(slave, eeprom1); - if (status < STATUS_OK) { - PX4_INFO_RAW("GPIO mode test failed (1st attempt)!"); + if (status != STATUS_OK) { + error_log("GPIO mode test failed (1st attempt)!"); return status; } status = ReadEEPROM(slave, eeprom2); - if (status < STATUS_OK) { - PX4_INFO_RAW("GPIO mode test failed (2nd attempt)!"); + if (status != STATUS_OK) { + error_log("GPIO mode test failed (2nd attempt)!"); return status; } status = ReadEEPROM(slave, eeprom3); - if (status < STATUS_OK) { - PX4_INFO_RAW("GPIO mode test failed (3rd attempt)!"); + if (status != STATUS_OK) { + error_log("GPIO mode test failed (3rd attempt)!"); return status; } /* Verify EEPROM data. */ if ((memcmp(eeprom1, eeprom2, 16) != 0) || (memcmp(eeprom1, eeprom3, 16) != 0)) { - PX4_INFO_RAW("GPIO Mode test failed (data comparison)!\n" - "The data from 3 distinct EEPROM readout does not match!"); + error_log("GPIO Mode test failed (data comparison)!\n" + "The data from 3 distinct EEPROM readout does not match!"); return ERROR_FAIL; } /* Check EEPROM data for reasonable chip and module number (i.e. not 0) */ uint32_t chipID = EEPROM_ReadChipId(eeprom1); - argus_module_version_t module = EEPROM_ReadModule(eeprom1); + uint8_t module = EEPROM_ReadModule(eeprom1); if (chipID == 0 || module == 0) { - PX4_INFO_RAW("GPIO Mode test failed (data verification)!\n" - "Invalid EEPROM data: Module = %d; Chip ID = %u!", module, (uint)chipID); + error_log("GPIO Mode test failed (data verification)!\n" + "Invalid EEPROM data: Module = %d; Chip ID = %d!", module, chipID); return ERROR_FAIL; } - PX4_INFO_RAW("EEPROM Readout succeeded!\n"); - PX4_INFO_RAW("- Module: %d\n", module); - PX4_INFO_RAW("- Device ID: %u\n", (uint)chipID); + print("EEPROM Readout succeeded!\n"); + print("- Module: %d\n", module); + print("- Device ID: %d\n", chipID); return STATUS_OK; } /*!*************************************************************************** - * @brief Reads the RCO_TRIM value from the devices EEPROM. + * @brief Reads the RCO_TRIM value from the devices EEPROM. * * @details The function reads the devices EEPROM via GPIO mode and extracts - * the RCO_TRIM value from the EEPROM map. + * the RCO_TRIM value from the EEPROM map. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param rcotrim The read RCO_TRIM value will be returned via this pointer. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param rcotrim The read RCO_TRIM value will be returned via this pointer. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. - * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. - * - The S2PI layer error code if #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or - * #S2PI_ReadGpioPin return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. *****************************************************************************/ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) { @@ -1003,25 +1282,15 @@ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) if (status != STATUS_OK) { return status; } - argus_module_version_t module = EEPROM_ReadModule(eeprom); - - switch (module) { - case AFBR_S50MV85G_V1: - case AFBR_S50MV85G_V2: - case AFBR_S50MV85G_V3: - case AFBR_S50LV85D_V1: - case AFBR_S50MV68B_V1: - case AFBR_S50MV85I_V1: - case AFBR_S50SV85K_V1: + uint8_t module = EEPROM_ReadModule(eeprom); + if (module > 0 && module < 8) { /* Read RCO Trim Value from EEPROM Map 1/2/3: */ *rcotrim = ((int8_t) eeprom[0]) >> 3; - break; - case MODULE_NONE: /* Uncalibrated module; use all 0 data. */ - default: - - PX4_INFO_RAW("EEPROM Readout failed! Unknown module number: %d", module); + } else { + /* Uncalibrated module; use all 0 data. */ + error_log("EEPROM Readout failed! Unknown module number: %d", module); return ERROR_ARGUS_UNKNOWN_MODULE; } @@ -1029,112 +1298,151 @@ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) } /*!*************************************************************************** - * @brief Triggers a measurement on the device and waits for the data ready - * interrupt. + * @brief Callback function for the data ready interrupt. + * + * @details The function is called by the S2PI layer when the data ready + * interrupt is pending. The function sets the \p param to + * #STATUS_IDLE. + * + * @param param The parameter passed to the #S2PI_SetIrqCallback function as + * an abstract pointer to an #status_t type. + *****************************************************************************/ +static void MeasurementCallback(void *param) +{ + *(status_t *) param = STATUS_IDLE; +} + +/*!*************************************************************************** + * @brief Triggers a measurement on the device and waits for the data ready + * interrupt. * * @details The function triggers a measurement cycle on the device and waits - * until the measurement has been finished. A \p sample count can be - * specified to setup individual number of digital averaging. + * until the measurement has been finished. A \p sample count can be + * specified to setup individual number of digital averaging. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param samples The specified number of averaging samples for the measurement. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param samples The specified number of averaging samples for the measurement. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus - * or #S2PI_SetIrqCallback return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. *****************************************************************************/ static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples) { - status_t status = TriggerMeasurement(slave, samples); + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 300; // The transfer timeout in ms. + /*************************************************************************/ - if (status < STATUS_OK) { - PX4_INFO_RAW("Speed test failed!\n" - "Call to TransferFrame returned code: %d", - (int)status); + volatile status_t callbackStatus = STATUS_BUSY; + + status_t status = S2PI_SetIrqCallback(slave, MeasurementCallback, (void *)&callbackStatus); + + if (status != STATUS_OK) { + error_log("Failed to run a measurement!\n" + "Call to SetIrqCallback returned code: %d", status); + return status; + } + + status = TriggerMeasurement(slave, samples, 0, 0); + + if (status != STATUS_OK) { + error_log("Failed to run a measurement!\n" + "Call to TransferFrame returned code: %d", status); return status; } /* Wait until the transfer is finished using a timeout. */ - status = AwaitDataReady(slave, 300); - if (status < STATUS_OK) { - PX4_INFO_RAW("Speed test failed!\n" - "SPI Read IRQ pin didn't raised, timeout activated at 200ms, error code: %d", - (int)status); - return status; + ltc_t start; + Time_GetNow(&start); + + while (callbackStatus == STATUS_BUSY) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + error_log("Failed to run a measurement!\n" + "Timeout occurred while waiting for the SPI interrupt (%d ms).", + timeout_ms); + return ERROR_TIMEOUT; + } } - return status; + if (callbackStatus != STATUS_OK) { + error_log("Failed to run a measurement!\n" + "The SPI callback yielded returned code: %d", + callbackStatus); + return callbackStatus; + } + + return STATUS_OK; } /*!*************************************************************************** - * @brief Test for Timer HAL Implementation by comparing timings to the device. + * @brief Test for Timer HAL Implementation by comparing timings to the device. * - * @details The test verifies the timer HAL implementation by comparing the - * timings to the AFBR-S50 device as a reference. - * Therefore several measurement are executed on the device, each with - * a different averaging sample count. The elapsed time increases - * linearly with the number of averaging samples. In order to remove - * the time for software/setup, a linear regression fit is applied to - * the measurement results and only the slope is considered for the - * result. A delta of 102.4 microseconds per sample is expected. - * If the measured delta per sample is within an specified error range, - * the timer implementation is considered correct. + * @details The test verifies the timer HAL implementation by comparing the + * timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL if the timer test fails. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the EEPROM Hamming weight fails. - * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus, - * #S2PI_SetIrqCallback, #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or #S2PI_ReadGpioPin - * return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the timer test fails. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the EEPROM Hamming weight fails. + * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus, + * #S2PI_SetIrqCallback, #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or #S2PI_ReadGpioPin + * return any negative status. *****************************************************************************/ static status_t TimerTest(s2pi_slave_t slave) { /* Test parameter configuration: *****************************************/ - const int8_t n = 10; // The number of measurements. - const uint32_t ds = 100; // The step size in averaging samples. - const float exp_slope = 102.4; // Expected slope is 102.4 µs / phase / sample - const float rel_slope_error = 3e-2; // Relative slope tolerance is 3%. + const int8_t n = 10; // The number of measurements. + const uint32_t ds = 100; // The step size in averaging samples. + const float exp_slope = 102.4f; // Expected slope is 102.4 µs / phase / sample + const float rel_slope_error = 3e-2f; // Relative slope tolerance is 3%. /*************************************************************************/ /* Read RCOTrim value from EEPROM*/ int8_t RcoTrim = 0; status_t status = ReadRcoTrim(slave, &RcoTrim); - if (status < STATUS_OK) { - PX4_INFO_RAW("Timer test failed!\n" - "EEPROM Read test returned code: %d", (int)status); + if (status != STATUS_OK) { + error_log("Timer test failed!\n" + "EEPROM Read test returned code: %d", status); return status; } - PX4_INFO_RAW("RCOTrim = %d\n", RcoTrim); + print("RCOTrim = %d\n", RcoTrim); /* Configure the device with calibrated RCO to 24MHz. */ status = ConfigureDevice(slave, RcoTrim); - if (status < STATUS_OK) { - PX4_INFO_RAW("Timer test failed!\n" - "Configuration test returned code: %d", (int)status); + if (status != STATUS_OK) { + error_log("Timer test failed!\n" + "Configuration test returned code: %d", status); return status; } @@ -1146,21 +1454,23 @@ static status_t TimerTest(s2pi_slave_t slave) float x2sum = 0; float xysum = 0; - PX4_INFO_RAW("+-------+---------+------------+\n"); - PX4_INFO_RAW("| count | samples | elapsed us |\n"); - PX4_INFO_RAW("+-------+---------+------------+\n"); + print("+-------+---------+------------+\n"); + print("| count | samples | elapsed us |\n"); + print("+-------+---------+------------+\n"); for (uint8_t i = 1; i <= n; ++i) { ltc_t start; Time_GetNow(&start); - int samples = ds * i; - status = RunMeasurement(slave, samples); + uint32_t samples = ds * i; + assert(samples < UINT16_MAX); - if (status < STATUS_OK) { - PX4_INFO_RAW("Timer test failed!\n" - "Run measurement returned code: %d", - (int)status); + status = RunMeasurement(slave, (uint16_t)samples); + + if (status != STATUS_OK) { + error_log("Timer test failed!\n" + "Run measurement returned code: %d", + status); return status; } @@ -1168,30 +1478,30 @@ static status_t TimerTest(s2pi_slave_t slave) xsum += (float) samples; ysum += (float) elapsed_usec; - x2sum += (float) samples * samples; - xysum += (float) samples * elapsed_usec; + x2sum += (float) samples * (float) samples; + xysum += (float) samples * (float) elapsed_usec; - PX4_INFO_RAW("| %5d | %7d | %10d |\n", i, samples, (uint)elapsed_usec); + print("| %5d | %7d | %10d |\n", i, samples, elapsed_usec); } - PX4_INFO_RAW("+-------+---------+------------+\n"); + print("+-------+---------+------------+\n"); const float slope = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum); const float intercept = (ysum * x2sum - xsum * xysum) / (n * x2sum - xsum * xsum); - PX4_INFO_RAW("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", - (int)(10 * slope), (int)(10 * intercept)); + print("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", + (int)(10 * slope), (int)(10 * intercept)); /* Check the error of the slope. */ const float max_slope = exp_slope * (1.f + rel_slope_error); const float min_slope = exp_slope * (1.f - rel_slope_error); if (slope > max_slope || slope < min_slope) { - PX4_INFO_RAW("Time test failed!\n" - "The measured time slope does not match the expected value! " - "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", - (int)(10 * slope), (int)(10 * exp_slope), - (int)(10 * min_slope), (int)(10 * max_slope)); + error_log("Time test failed!\n" + "The measured time slope does not match the expected value! " + "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", + (int)(10 * slope), (int)(10 * exp_slope), + (int)(10 * min_slope), (int)(10 * max_slope)); return ERROR_FAIL; } @@ -1200,11 +1510,11 @@ static status_t TimerTest(s2pi_slave_t slave) /*!*************************************************************************** - * @brief Data structure for the PIT test. + * @brief Data structure for the PIT test. * - * @details Contains data that is required by the PIT timer test. + * @details Contains data that is required by the PIT timer test. *****************************************************************************/ -typedef struct { +typedef struct pit_data_t { /*! The number of PIT callback events. */ volatile uint32_t n; @@ -1216,90 +1526,113 @@ typedef struct { } pit_data_t; - - /*!*************************************************************************** - * @brief Callback function invoked by the PIT. + * @brief Callback function invoked by the PIT. * - * @details The function that is invoked every time a specified interval elapses. - * An abstract parameter is passed to the function whenever it is called. + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. * - * This implementation collects callback time stamps and counts the - * number of callback events using the abstract parameter. + * This implementation collects callback time stamps and counts the + * number of callback events using the abstract parameter. * - * @param param An abstract parameter to be passed to the callback. This is - * also the identifier of the given interval. + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. *****************************************************************************/ static void PIT_Callback(void *param) { - pit_data_t *data = (pit_data_t *) param; - - if (data->n == 0) { - Time_GetNow(&data->t_first); - data->t_last = data->t_first; + if (param == NULL) { + error_log("PIT interrupt test failed: callback parameter \"param\" was NULL!"); } else { - Time_GetNow(&data->t_last); - } + pit_data_t *data = (pit_data_t *)param; - data->n++; + if (data->n == 0) { + Time_GetNow(&data->t_first); + data->t_last = data->t_first; + + } else { + Time_GetNow(&data->t_last); + } + + data->n++; + } } /*!*************************************************************************** - * @brief Executes a PIT measurement and verifies the callback interval. + * @brief Executes a PIT measurement and verifies the callback interval. * * @details The function configures the PIT with a given interval and waits - * several callback events to happen. In each callback event, the - * elapsed time is measured and the number of calls are counted. - * Finally, the average interrupt period is compared with the - * lifetime timer that has been already verified in a previous test - * (see #Timer_Test). + * several callback events to happen. In each callback event, the + * elapsed time is measured and the number of calls are counted. + * Finally, the average interrupt period is compared with the + * lifetime timer that has been already verified in a previous test + * (see #TimerTest). The time until the first interrupt event is also + * verified. * - * @param exp_dt_us The expected timer interval in microseconds. - * @param n The number of PIT events to await. + * @param exp_dt_us The expected timer interval in microseconds. + * @param n The number of PIT events to await. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL if the measured interval does not match the - * expectations or the PIT was not disabled properly. - * - #ERROR_TIMEOUT if either the PIT events do not occur within the - * expected time. - * - The PIT layer error code if #Timer_SetInterval return any - * negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval return any + * negative status. *****************************************************************************/ static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n) { /* Test parameter configuration: *****************************************/ - const float rel_dt_error = 1e-3; // Relative timer interval tolerance is 0.1%. - const float abs_dt_error = 1.0; // Absolute timer interval tolerance is 1us. + const float rel_dt_error = 5e-3f; // Relative timer interval tolerance: 0.5 %. + const float abs_dt_error = 5.0f; // Absolute timer interval tolerance: 5.0 us. /*************************************************************************/ - float dt = exp_dt_us * rel_dt_error; + float dt = (float) exp_dt_us * rel_dt_error; if (dt < abs_dt_error) { dt = abs_dt_error; } - const float max_dt = exp_dt_us + dt; - const float min_dt = exp_dt_us - dt; + const float max_dt = (float) exp_dt_us + dt; + const float min_dt = (float) exp_dt_us - dt; + + if (dt < abs_dt_error * 3) { dt = abs_dt_error * 3; } + + const float t_first_max = (float) exp_dt_us + dt * 5; // use 5x tolerance for + const float t_first_min = (float) exp_dt_us - dt * 5; // the first interval /*************************************************************************/ + print("Run PIT Test (w/ %d us interval):\n" + " - expected event count: %d\n" + " - expected interval: %d us, min: %d us, max: %d us\n" + " - expected first event: %d us, min: %d us, max: %d us\n", + exp_dt_us, n, exp_dt_us, (int)min_dt, (int)max_dt, + exp_dt_us, (int)t_first_min, (int)t_first_max); + /* Setup the PIT callback with specified interval. */ pit_data_t data = { 0 }; status_t status = Timer_SetInterval(exp_dt_us, &data); - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetInterval returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", status); return status; } /* Wait until n PIT callback have been happened. */ - uint32_t timeout_us = (n + 1) * exp_dt_us; + const uint32_t timeout_us = (n + 1) * exp_dt_us; + ltc_t start; Time_GetNow(&start); while (data.n < n) { if (Time_CheckTimeoutUSec(&start, timeout_us)) { - PX4_INFO_RAW("PIT test failed!\n" - "Waiting for the PIT interrupt events yielded a timeout."); + const uint32_t elapsed_us = Time_GetElapsedUSec(&start); + const uint32_t t_first_us = Time_DiffUSec(&start, &data.t_first); + const uint32_t t_last_us = Time_DiffUSec(&start, &data.t_last); + error_log("PIT test failed!\n" + "Waiting for the PIT interrupt events yielded a timeout.\n" + "Timeout: %d us; Elapsed: %d us (%d of %d events).\n" + "First event @ %d us, last event @ %d us", + timeout_us, elapsed_us, data.n, n, t_first_us, t_last_us); status = ERROR_TIMEOUT; break; } @@ -1309,59 +1642,71 @@ static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n) /* Disable the PIT timer callback. */ status = Timer_SetInterval(0, &data); - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetInterval returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", status); } } if (status == STATUS_OK) { /* Check if PIT callback is not invoked any more. */ - timeout_us = 2 * exp_dt_us; - Time_GetNow(&start); - - while (!Time_CheckTimeoutUSec(&start, timeout_us)) { __asm("nop"); } + Time_DelayUSec(3 * exp_dt_us); if (data.n > n) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetInterval has been called after it was disabled."); + const uint32_t elapsed_us = Time_GetElapsedUSec(&start); + error_log("PIT test failed!\n" + "Timer_SetInterval has been called again after it was disabled\n" + "(within %d us; %d of %d events in total).", + elapsed_us, data.n, n); status = ERROR_FAIL; } } /* Verify the measured average timer interval. */ - const float act_dt_us = Time_DiffUSec(&data.t_first, &data.t_last) / (n - 1); + const float act_dt_us = Time_DiffUSec(&data.t_first, &data.t_last) / (float)(n - 1); + const uint32_t t_first_us = Time_DiffUSec(&start, &data.t_first); + const uint32_t t_last_us = Time_DiffUSec(&start, &data.t_last); - if (status == STATUS_OK && (act_dt_us > max_dt || act_dt_us < min_dt)) { - PX4_INFO_RAW("PIT test failed!\n" - "The measured timer interval does not match the expected value!\n"); + print(" - actual event count: %d\n" + " - actual interval: %d us\n" + " - actual first event: %d us\n" + " - actual last event: %d us\n\n", + data.n, (int)act_dt_us, t_first_us, t_last_us); + + if (status == STATUS_OK && (t_first_us > t_first_max || t_first_us < t_first_min)) { + error_log("PIT test failed!\n" + "The first timer event did not occur after the expected interval!"); status = ERROR_FAIL; } - PX4_INFO_RAW("PIT Test Results:\n" - " - event count: %u\n" - " - actual interval: %d us\n" - " - expected interval: %d us, min: %d us, max: %d us\n", - (uint)data.n, (int)act_dt_us, (uint)exp_dt_us, (int)min_dt, (int)max_dt); + if (status == STATUS_OK && (act_dt_us > max_dt || act_dt_us < min_dt)) { + error_log("PIT test failed!\n" + "The measured timer interval does not match the expected value!"); + status = ERROR_FAIL; + } + + print(" - test status: %d\n\n", status); return status; } /*!*************************************************************************** - * @brief Test for PIT HAL Implementation by comparing timings to the device. + * @brief Test for PIT HAL Implementation by comparing timings to the device. * - * @details The test verifies the timer HAL implementation by comparing the + * @details The test verifies the timer HAL implementation by comparing the + * period between the interrupts with the lifetime timer values + * that has been already verified in a previous test (see #TimerTest). * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not - * implemented and the test is skipped. - * - #ERROR_FAIL if the measured interval does not match the - * expectations or the PIT was not disabled properly. - * - #ERROR_TIMEOUT if either the PIT events do not occur within the - * expected time. - * - The PIT layer error code if #Timer_SetInterval or - * #Timer_SetCallback return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not + * implemented and the test is skipped. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval or + * #Timer_SetCallback return any negative status. *****************************************************************************/ static status_t PITTest(void) { @@ -1369,31 +1714,595 @@ static status_t PITTest(void) if (status == ERROR_NOT_IMPLEMENTED) { return status; } - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetCallback returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetCallback returned status code: %d", status); return status; } + status = RunPITTest(200000, 5); + + if (status != STATUS_OK) { return status; } + status = RunPITTest(10000, 10); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } - status = RunPITTest(333, 1000); + /* High Speed Test down to 1000 microseconds. If this fails, just print + * a message that very high frame rates might have issues. */ + status = RunPITTest(1000, 500); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { + print("WARNING: PIT test failed for 1000 us interval!\n" + " This is only critical if high frame rates (up to 1000 fps)\n" + " need to be achieved. Otherwise, the error can be safely ignored.\n"); + status = STATUS_IGNORE; // ignore + } - status = RunPITTest(100000, 5); + if (status == STATUS_OK) { // only run if previous test succeeded! + /* High Speed Test down to 333 microseconds. If this fails, just print + * a message that very high frame rates might have issues. */ + status = RunPITTest(333, 500); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { + print("WARNING: PIT test failed for 333 us interval!\n" + " This is only critical if very high frame rates (up to 3000 fps)\n" + " need to be achieved. Otherwise, the error can be safely ignored.\n"); + status = STATUS_IGNORE; // ignore + } + } status = Timer_SetCallback(0); - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetCallback to 0 returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetCallback to 0 returned status code: %d", status); return status; } return STATUS_OK; } + +/*!*************************************************************************** + * @brief Data structure for the S2PI transfer from interrupt tests. + * + * @details Contains data that is required by the S2PI transfer from interrupt + * test. The data structure is passed to the corresponding interrupt + * callback functions. + *****************************************************************************/ +typedef struct spi_irq_data_t { + /*! The status of the interrupt callback function. */ + volatile status_t Status; + + /*! The S2PI slave parameter passed to the S2PI HAL functions. */ + s2pi_slave_t Slave; + + /*! The data buffer to be transferred from/to the device for testing purposes. */ + uint8_t Data[17U]; + + /*! Set to true when all SPI transfers are finished. */ + volatile bool Finished; + + /*! Set to true when the second SPI transfers is started. + The second transfer is used to read-back the previously set values. */ + volatile bool ReadBack; + +} spi_irq_data_t; + + +/*!*************************************************************************** + * @brief SPI interrupt callback function for the SPI transfer from IRQ test. + * + * @details The interrupt callback is invoked from the S2PI module upon + * finishing the SPI transfer. The callback is used by the + * #SpiTransferFromSpiInterrupt test to trigger the second SPI transfer + * from the interrupt callback context. + * + * @note The callback also utilizes the #print functionality. This requires + * a correct implementation of the corresponding function such that it + * can be invoked from the given interrupt context. This usually + * requires the underlying send (e.g. UART or USB send functions) to + * have higher priority that this interrupt in order to finished the + * print statement asynchronously. + * + * @param status The S2PI module status passed to the callback. + * @param param The abstract interrupt callback parameter. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_INVALID_ARGUMENT if the \p param is NULL. + * - The S2PI layer error code if any is passed to the callback function. + * - The S2PI layer error code if #S2PI_TransferFrame return any. + *****************************************************************************/ +static status_t SpiTransferFromSpiInterruptCallback(status_t status, void *param) +{ + if (param == NULL) { + error_log("SPI transfer from SPI interrupt test failed\n" + "callback parameter \"param\" was NULL!"); + return ERROR_INVALID_ARGUMENT; + } + + spi_irq_data_t *data = (spi_irq_data_t *) param; + + if (status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "callback received error! Error code: %d", status); + data->Status = status; + return status; + } + + if (!data->ReadBack) { + print("Invoking SPI transfer from SPI interrupt callback...\n"); + + /* Clear the laser pattern and read back previous values. */ + data->Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data->Data[i] = 0; } + + status = S2PI_TransferFrame(data->Slave, data->Data, data->Data, 17U, + SpiTransferFromSpiInterruptCallback, param); + + if (status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Calling S2PI_TransferFrame from SPI interrupt " + "returned error code: %d", status); + data->Status = status; + return status; + } + + data->ReadBack = true; + + } else { + data->Finished = true; + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief SPI transfer from SPI interrupt callback test. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the SPI + * interrupt service routine context. + * + * The test basically repeats the #SpiConnectionTest but this time it + * invokes the second SPI transfer from the SPI callback function. + * A very common error is that the callback is invoked while the SPI + * module is still busy which does not allow to invoke another SPI + * transfer from the callback. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the test did not finish within a specified time. + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or the SPI + * callback yield in any non-OK status. + *****************************************************************************/ +static status_t SpiTransferFromSpiInterrupt(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_us = 100000; // timeout for SPI transfers to finish + /*************************************************************************/ + + status_t status = STATUS_OK; + spi_irq_data_t data = { .Slave = slave }; + + print("Invoking SPI transfer from task level...\n"); + + /* Transfer a pattern to the register */ + data.Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data.Data[i] = i; } + + status = S2PI_TransferFrame(slave, data.Data, data.Data, 17U, + SpiTransferFromSpiInterruptCallback, &data); + + if (status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Failed to transfer a data frame! Error code: %d", status); + return status; + } + + /* Wait until transfers has finished. */ + ltc_t start; + Time_GetNow(&start); + + while (!data.Finished && (data.Status == STATUS_OK)) { + if (Time_CheckTimeoutUSec(&start, timeout_us)) { + const uint32_t elapsed_us = Time_GetElapsedUSec(&start); + error_log("SPI transfer from SPI interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a timeout.\n" + "Timeout: %d us; Elapsed: %d us (%d of %d events).", + timeout_us, elapsed_us); + status = ERROR_TIMEOUT; + break; + } + } + + if (data.Status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a error code: %d", + data.Status); + return data.Status; + } + + print("Verify read data...\n"); + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data.Data[i] != i) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data.Data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief GPIO interrupt callback function for the SPI transfer from IRQ test. + * + * @details The interrupt callback is invoked from the S2PI module upon + * receiving an GPIO interrupt from the devices IRQ pin. The callback + * is used by the #SpiTransferFromGpioInterrupt test to trigger the + * first SPI transfer from the interrupt callback context. + * + * @note The callback also utilizes the #print functionality. This requires + * a correct implementation of the corresponding function such that it + * can be invoked from the given interrupt context. This usually + * requires the underlying send (e.g. UART or USB send functions) to + * have higher priority that this interrupt in order to finished the + * print statement asynchronously. + * + * @param param The abstract interrupt callback parameter. + *****************************************************************************/ +static void SpiTransferFromGpioInterruptCallback(void *param) +{ + if (param == NULL) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "callback parameter \"param\" was NULL!"); + return; + } + + print("Invoking SPI transfer from GPIO interrupt callback...\n"); + + /* Clear the laser pattern and read back previous values. */ + spi_irq_data_t *data = (spi_irq_data_t *) param; + data->Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data->Data[i] = i; } + + status_t status = S2PI_TransferFrame(data->Slave, data->Data, data->Data, 17U, + SpiTransferFromSpiInterruptCallback, param); + + if (status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "Calling S2PI_TransferFrame from GPIO interrupt " + "returned error code: %d", status); + data->Status = status; + return; + } +} + +/*!*************************************************************************** + * @brief SPI transfer from GPIO interrupt callback test. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the + * GPIO interrupt service routine context. + * + * The test basically repeats the #SpiTransferFromSpiInterrupt but + * this time it invokes the first SPI transfer from the GPIO callback + * function. In order to trigger a GPIO interrupt, the device is + * configured and a measurement is started (see #GpioInterruptTest). + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the test did not finish within a specified time. + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or the GPIO or + * SPI callback yield in any non-OK status. + *****************************************************************************/ +static status_t SpiTransferFromGpioInterrupt(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 300; // timeout for measurement, might be increased.. + /*************************************************************************/ + + spi_irq_data_t data = { .Slave = slave }; + + /* Install IRQ callback. */ + status_t status = S2PI_SetIrqCallback(slave, SpiTransferFromGpioInterruptCallback, &data); + + if (status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "The call to S2PI_SetIrqCallback returned error code: %d", status); + return status; + } + + /* Setup Device for invoking GPIO interrupt. */ + status = ConfigureDevice(slave, 0); + + if (status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed."); + return status; + } + + /* Trigger Measurement and invoke GPIO interrupt. */ + status = TriggerMeasurement(slave, 0, 0, 0); + + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed!"); + return status; + } + + ltc_t start; + Time_GetNow(&start); + + /* Wait for Interrupt using the callback method. */ + while (!data.Finished) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "The IRQ callback was not invoked within %d ms.", + timeout_ms); + return ERROR_TIMEOUT; + } + } + + if (data.Status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a error code: %d", + data.Status); + return data.Status; + } + + print("Verify read data...\n"); + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data.Data[i] != i) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data.Data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief PIT interrupt callback function for the SPI transfer from IRQ test. + * + * @details The interrupt callback is invoked from the PIT module upon periodic + * timeout event. The callback is used by the + * #SpiTransferFromPitInterrupt test to trigger the first SPI transfer + * from the interrupt callback context. + * + * @note The callback also utilizes the #print functionality. This requires + * a correct implementation of the corresponding function such that it + * can be invoked from the given interrupt context. This usually + * requires the underlying send (e.g. UART or USB send functions) to + * have higher priority that this interrupt in order to finished the + * print statement asynchronously. + * + * @param param The abstract interrupt callback parameter. + *****************************************************************************/ +static void SpiTransferFromPitInterruptCallback(void *param) +{ + status_t status = Timer_SetInterval(0, param); // disable timer + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetCallback to 0 returned status code: %d", + status); + + if (param != NULL) { ((spi_irq_data_t *)param)->Status = status; } + + return; + } + + + if (param == NULL) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "callback parameter \"param\" was NULL!"); + return; + } + + print("Invoking SPI transfer from PIT interrupt callback...\n"); + + /* Clear the laser pattern and read back previous values. */ + spi_irq_data_t *data = (spi_irq_data_t *) param; + data->Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data->Data[i] = i; } + + status = S2PI_TransferFrame(data->Slave, data->Data, data->Data, 17U, + SpiTransferFromSpiInterruptCallback, param); + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Calling S2PI_TransferFrame from GPIO interrupt " + "returned error code: %d", status); + data->Status = status; + return; + } +} + +/*!*************************************************************************** + * @brief SPI transfer from PIT interrupt callback test. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the + * PIT interrupt service routine context. + * + * The test basically repeats the #SpiTransferFromSpiInterrupt but + * this time it invokes the first SPI transfer from the PIT callback + * function. In order to trigger a PIT interrupt, the timer is + * configured with a small interval and immediately disabled upon the + * first event. + * + * Note that this test is only executed if the PIT module is actually + * implemented. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not + * implemented and the test is skipped. + * - #ERROR_TIMEOUT if the test did not finish within a specified time. + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or the SPI + * callback yield in any non-OK status. + * - The PIT layer error code if #Timer_SetCallback or the PIT + * callback yield in any non-OK status. + *****************************************************************************/ +static status_t SpiTransferFromPitInterrupt(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 100; // timeout for test. + const uint32_t interval_us = 1000; // PIT interval for the first event. + /*************************************************************************/ + + spi_irq_data_t data = { .Slave = slave }; + + status_t status = Timer_SetCallback(SpiTransferFromPitInterruptCallback); + + if (status == ERROR_NOT_IMPLEMENTED) { return status; } + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetCallback returned status code: %d", status); + return status; + } + + /* Setup the PIT callback with specified interval. */ + status = Timer_SetInterval(interval_us, &data); + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetInterval returned status code: %d", status); + return status; + } + + ltc_t start; + Time_GetNow(&start); + + /* Wait for test to be finished. */ + while (!data.Finished) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "The IRQ callback was not invoked within %d ms.", + timeout_ms); + return ERROR_TIMEOUT; + } + } + + if (data.Status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a error code: %d", + data.Status); + return data.Status; + } + + status = Timer_SetCallback(0); + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetCallback to 0 returned status code: %d", status); + return status; + } + + print("Verify read data...\n"); + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data.Data[i] != i) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data.Data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief SPI Transfer from Interrupt Test for S2PI HAL Implementation. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the + * interrupt service routine context. I.e. the #S2PI_TransferFrame + * function is called from the following interrupts: + * - SPI interrupt + * - GPIO interrupt + * - PIT interrupt (optional, if PIT is implemented) + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. + *****************************************************************************/ +static status_t SpiTransferFromInterruptTest(s2pi_slave_t slave) +{ + status_t status = STATUS_OK; + + print(" .1 >> SPI Transfer from SPI Interrupt Test\n"); + status = SpiTransferFromSpiInterrupt(slave); + + if (status != STATUS_OK) { return status; } + + print(" .1 >> PASS\n\n"); + + print(" .2 >> SPI Transfer from GPIO Interrupt Test\n"); + status = SpiTransferFromGpioInterrupt(slave); + + if (status != STATUS_OK) { return status; } + + print(" .2 >> PASS\n\n"); + + print(" .3 >> SPI Transfer from PIT Interrupt Test\n"); + status = SpiTransferFromPitInterrupt(slave); + + if (status == ERROR_NOT_IMPLEMENTED) { + print(" .3 >> SKIPPED (PIT is not implemented)\n\n"); + + } else { + if (status != STATUS_OK) { return status; } + + print(" .3 >> PASS\n\n"); + } + + return STATUS_OK; +} + +/*! @} */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h index 5af6660dfa..be9c0071e4 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h @@ -1,10 +1,10 @@ /*************************************************************************//** - * @file argus_hal_test.c - * @brief Tests for the AFBR-S50 API hardware abstraction layer. + * @file + * @brief Tests for the AFBR-S50 API hardware abstraction layer. * * @copyright * - * Copyright (c) 2021, Broadcom, Inc. + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -41,147 +41,192 @@ extern "C" { #endif - /*!*************************************************************************** - * @defgroup argustest HAL Self Test + * @defgroup argus_test HAL Self Test + * @ingroup argus * - * @brief A test module to verify implementation of the HAL. + * @brief A test module to verify implementation of the HAL. * - * @details A series of automated tests that can be executed on the target - * platform in order to verify the implementation of the HAL that - * are required by the API. + * @details A series of automated tests that can be executed on the target + * platform in order to verify the implementation of the HAL that + * are required by the API. * - * @addtogroup argustest + * See #Argus_VerifyHALImplementation for a detailed documentation. + * + * @addtogroup argus_test * @{ *****************************************************************************/ #include "argus.h" /*!*************************************************************************** - * @brief Version number of the HAL Self Test. + * @brief Version number of the HAL Self Test. * * @details Changes: - * * v1.0: - * - Initial release. - * * v1.1: - * - Added additional print output. - * - Increased tolerance for timer test to 3%. - * - Fixed callback issue by disabling it after IRQ test. - * * v1.1: - * - Added PIT test cases. + * * v1.0: + * - Initial release. + * * v1.1: + * - Added additional print output. + * - Increased tolerance for timer test to 3%. + * - Fixed callback issue by disabling it after IRQ test. + * * v1.2: + * - Added PIT test cases. + * * v1.3: + * - Added test case for SPI maximum data transfer size. + * - Added tests for SPI transfers invoked from all IRQ callbacks. + * - Added verification of first PIT event occurrence. + * - Relaxed PIT pass conditions (0.1% -> 0.5%) + * * v1.4: + * - Adopted to new multi-device HAL interface of API v1.4.4 release. + * - Added verification of SPI callback invocation. + * - Updated GPIO interrupt test to verify if delayed interrupt + * pending states can be detected via #S2PI_ReadIrqPin. + * *****************************************************************************/ -#define HAL_TEST_VERSION "v1.2" +#define HAL_TEST_VERSION "v1.4" /*!*************************************************************************** - * @brief Executes a series of tests in order to verify the HAL implementation. + * @brief Executes a series of tests in order to verify the HAL implementation. * * @details A series of automated tests are executed on the target platform in - * order to verify the implementation of the HAL that are required by - * the API. + * order to verify the implementation of the HAL that are required by + * the API. * - * Each test will write an error description via the print (i.e. UART) - * function that shows what went wrong. Also an corresponding status is - * returned in case no print functionality is available. + * Each test will write an error description via the print (i.e. UART) + * function that shows what went wrong. Also an corresponding status is + * returned in case no print functionality is available. * - * The following tests are executed: + * The following tests are executed: * - * **1) Timer Plausibility Test:** + * **1) Timer Plausibility Test:** * - * Rudimentary tests of the lifetime counter (LTC) implementation. - * This verifies that the LTC is running by checking if the returned - * values of two consecutive calls to the #Timer_GetCounterValue - * function are ascending. An artificial delay using the NOP operation - * is induced such that the timer is not read to fast. + * Rudimentary tests of the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. * - * **2) Timer Wraparound Test:** + * **2) Timer Wraparound Test:** * - * The LTC values must wrap from 999999 µs to 0 µs and increase the - * seconds counter accordingly. This test verifies the correct wrapping - * by consecutively calling the #Timer_GetCounterValue function until - * at least 2 wraparound events have been occurred. + * The LTC values must wrap from 999999 µs to 0 µs and increase the + * seconds counter accordingly. This test verifies the correct wrapping + * by consecutively calling the #Timer_GetCounterValue function until + * at least 2 wraparound events have been occurred. * - * **3) SPI Connection Test:** + * **3) SPI Connection Test:** * - * This test verifies the basic functionality of the SPI interface. - * The test utilizes the devices laser pattern register, which can - * be freely programmed by any 128-bit pattern. Thus, it writes a byte - * sequence and reads back the written values on the consecutive SPI - * access. + * This test verifies the basic functionality of the SPI interface. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. * - * **4) SPI Interrupt Test:** + * **4) SPI Maximum Data Length Test**: * - * This test verifies the correct implementation of the device - * integration finished interrupt callback. Therefore it configures - * the device with a minimal setup to run a pseudo measurement that - * does not emit any laser light. + * This test verifies the maximum data transfer length of the SPI + * interface. The test sends and receives up to 396 data bytes plus + * a single address byte over the SPI interface and verifies that no + * data get lost. * - * Note that this test does verify the GPIO interrupt that occurs - * whenever the device has finished the integration/measurement and - * new data is waiting to be read from the device. This does not test - * the interrupt that is triggered when the SPI transfer has finished. + * **5) SPI Interrupt Test:** * - * The data ready interrupt implies two S2PI layer functions that - * are tested in this test: The #S2PI_SetIrqCallback function installs - * a callback function that is invoked whenever the IRQ occurs. - * The IRQ can be delayed due to higher priority task, e.g. from the - * user code. It is essential for the laser safety timeout algorithm - * to determine the device ready signal as fast as possible, another - * method is implemented to read if the IRQ is pending but the - * callback has not been reset yet. This is what the #S2PI_ReadIrqPin - * function is for. + * This test verifies the correct implementation of the device + * integration finished interrupt callback. Therefore it configures + * the device with a minimal setup to run a pseudo measurement that + * does not emit any laser light. * - * **5) GPIO Mode Test:** + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. * - * This test verifies the GPIO mode of the S2PI HAL module. This is - * done by leveraging the EEPROM readout sequence that accesses the - * devices EEPROM via a software protocol that depends on the GPIO - * mode. + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible, another + * method is implemented to read if the IRQ is pending but the + * callback has not been reset yet. This is what the #S2PI_ReadIrqPin + * function is for. * - * This the requires several steps, most of them are already verified - * in previous tests: + * **6) GPIO Mode Test:** * - * - Basic device configuration and enable EEPROM. - * - Read EERPOM via GPIO mode and apply Hamming weight. - * - Repeat several times (to eliminate random readout issues). - * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c). - * - Check if Module Number and Chip ID is not 0. + * This test verifies the GPIO mode of the S2PI HAL module. This is + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. * - * **6) Timer Test for Lifetime Counter:** + * This the requires several steps, most of them are already verified + * in previous tests: * - * The test verifies the lifetime counter timer HAL implementation by - * comparing the timings to the AFBR-S50 device as a reference. - * Therefore several measurement are executed on the device, each with - * a different averaging sample count. The elapsed time increases - * linearly with the number of averaging samples. In order to remove - * the time for software/setup, a linear regression fit is applied to - * the measurement results and only the slope is considered for the - * result. A delta of 102.4 microseconds per sample is expected. - * If the measured delta per sample is within an specified error range, - * the timer implementation is considered correct. + * - Basic device configuration and enable EEPROM. + * - Read EERPOM via GPIO mode and apply Hamming weight. + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c). + * - Check if Module Number and Chip ID is not 0. * - * **7) Timer Test for Periodic Interrupt Timer:** + * **7) Timer Test for Lifetime Counter:** * - * The test verifies the correct implementation of the periodic - * interrupt timer (PIT). It sets different intervals and waits for - * a certain number of interrupts to happen. Each interrupt event - * is counted and the time between the first and the last interrupt - * is measured. Finally, the measured interval is compared to the - * expectations. + * The test verifies the lifetime counter timer HAL implementation by + * comparing the timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. * + * **8) Timer Test for Periodic Interrupt Timer (optional):** * - * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ - * lines. This is actually just a number that is passed - * to the SPI interface to distinct for multiple SPI slave - * devices. Note that the slave must be not equal to 0, - * since is reserved for error handling. + * The test verifies the correct implementation of the periodic + * interrupt timer (PIT). It sets different intervals and waits for + * a certain number of interrupts to happen. Each interrupt event + * is counted and the time between the first and the last interrupt + * is measured. Finally, the measured interval is compared to the + * expectations. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * Note that this test is only executed if the PIT is actually + * implemented. Otherwise, the test is skipped. + * + * **9) SPI Transfer from Interrupt Callback Test:** + * + * The test verifies that the #S2PI_TransferFrame method of the + * S2PI layer can be invoked from a interrupt callback function too. + * Thus, it repeats the S2PI Connection Test but this time from + * different interrupt callback functions: + * + * - SPI Callback: The first transfer is invoked from thread level, + * the second transfer is invoke from the SPI interrupt callback + * function. + * + * - GPIO Callback: The device is setup to trigger an GPIO interrupt + * (see also the SPI Interrupt Test). The corresponding GPIO + * interrupt callback function will trigger the first transfer while + * the second one is triggered from the SPI callback function. + * + * - PIT Callback (optional): This test is only executed optional if + * the PIT interface is implemented. The test sequence is the same + * as for the GPIO callback, but the first transfer is triggered + * from the PIT callback function. + * + * @note See #HAL_TEST_VERSION for a version history and change log of + * the HAL self tests. + * + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave); -#ifdef __cplusplus -} -#endif - /*! @} */ -#endif /* ARGUS_CAL_API_H */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* ARGUS_HAL_TEST_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c b/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c new file mode 100644 index 0000000000..28ef2a17c1 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +/** + * AFBR Rangefinder Mode + * + * This parameter defines the mode of the AFBR Rangefinder. + * + * @reboot_required true + * @min 0 + * @max 3 + * @group Sensors + * + * @value 0 Short Range Mode + * @value 1 Long Range Mode + * @value 2 High Speed Short Range Mode + * @value 3 High Speed Long Range Mode + */ +PARAM_DEFINE_INT32(SENS_AFBR_MODE, 1); + +/** + * AFBR Rangefinder Short Range Rate + * + * This parameter defines measurement rate of the AFBR Rangefinder in short range mode. + * + * @min 1 + * @max 100 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_S_RATE, 50); + +/** + * AFBR Rangefinder Long Range Rate + * + * This parameter defines measurement rate of the AFBR Rangefinder in long range mode. + * + * @min 1 + * @max 100 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_L_RATE, 25); + +/** + * AFBR Rangefinder Short/Long Range Threshold + * + * This parameter defines the threshold for switching between short and long range mode. + * The mode will switch from short to long range when the distance is greater than the threshold plus the hysteresis. + * The mode will switch from long to short range when the distance is less than the threshold minus the hysteresis. + * + * @unit m + * @min 1 + * @max 50 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 5); + + +/** + * AFBR Rangefinder Short/Long Range Threshold Hysteresis + * + * This parameter defines the hysteresis for switching between short and long range mode. + * + * @unit m + * @min 1 + * @max 10 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_HYSTER, 1); diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp index f27636657f..648d5f649f 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp +++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp @@ -219,6 +219,11 @@ int LightwareLaserSerial::collect() } else { for (int i = 0; i < ret; i++) { + // Check for overflow + if (_linebuf_index >= sizeof(_linebuf)) { + _parse_state = LW_PARSE_STATE0_UNSYNC; + } + if (OK == lightware_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) { valid = true; } diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 17563002c4..3e295abfc9 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -294,17 +294,17 @@ namespace time_literals // User-defined integer literals for different time units. // The base unit is hrt_abstime in microseconds -constexpr hrt_abstime operator "" _s(unsigned long long seconds) +constexpr hrt_abstime operator ""_s(unsigned long long seconds) { return hrt_abstime(seconds * 1000000ULL); } -constexpr hrt_abstime operator "" _ms(unsigned long long milliseconds) +constexpr hrt_abstime operator ""_ms(unsigned long long milliseconds) { return hrt_abstime(milliseconds * 1000ULL); } -constexpr hrt_abstime operator "" _us(unsigned long long microseconds) +constexpr hrt_abstime operator ""_us(unsigned long long microseconds) { return hrt_abstime(microseconds); } diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index fa64e1d7d3..549b7313c1 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,6 +82,7 @@ #define DRV_IMU_DEVTYPE_IIM42652 0x2B #define DRV_IMU_DEVTYPE_IAM20680HP 0x2C #define DRV_IMU_DEVTYPE_ICM42686P 0x2D +#define DRV_IMU_DEVTYPE_IIM42653 0x2E #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 @@ -114,6 +115,7 @@ #define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4B #define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4C + #define DRV_BARO_DEVTYPE_TCBP001TA 0x4D #define DRV_BARO_DEVTYPE_MS5837 0x4E #define DRV_BARO_DEVTYPE_SPL06 0x4F @@ -233,6 +235,7 @@ #define DRV_INS_DEVTYPE_VN100 0xE1 #define DRV_INS_DEVTYPE_VN200 0xE2 #define DRV_INS_DEVTYPE_VN300 0xE3 +#define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 1de64da42e..b443b79f57 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 1de64da42ee3564ebfd97719761434c2f4be4b64 +Subproject commit b443b79f57c5e89353430940af9e4511ea8eb0b8 diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 170bd859a4..aba6f7066e 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -231,7 +231,7 @@ void Heater::Run() _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); - if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) { + if (fabsf(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) { _temperature_target_met = true; } else { diff --git a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp index 0aa216c940..c12e8d356d 100644 --- a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp +++ b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp @@ -283,6 +283,7 @@ void ADIS16507::RunImpl() if (buffer.checksum != checksum) { //PX4_DEBUG("adis_report.checksum: %X vs calculated: %X", buffer.checksum, checksum); perf_count(_bad_transfer_perf); + perf_count(_perf_crc_bad); } if (buffer.DIAG_STAT != DIAG_STAT_BIT::Data_path_overrun) { diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index a04e74e707..67f456cdc7 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -179,11 +179,12 @@ private: int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{16}; + static constexpr uint8_t size_register_bank0_cfg{17}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_ENDIAN | INTF_CONFIG0_BIT::SENSOR_DATA_ENDIAN | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp index 6c407216f0..79971a1bdd 100644 --- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -159,6 +159,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0, +}; + +// INTF_CONFIG1 enum INTF_CONFIG1_BIT : uint8_t { RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required CLKSEL = Bit0, diff --git a/src/drivers/imu/invensense/iim42653/CMakeLists.txt b/src/drivers/imu/invensense/iim42653/CMakeLists.txt new file mode 100644 index 0000000000..4f0e7a7986 --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2022 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 drivers__imu__invensense__iim42653 + MAIN iim42653 + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + #-DDEBUG_BUILD + SRCS + iim42653_main.cpp + IIM42653.cpp + IIM42653.hpp + InvenSense_IIM42653_registers.hpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope + ) diff --git a/src/drivers/imu/invensense/iim42653/IIM42653.cpp b/src/drivers/imu/invensense/iim42653/IIM42653.cpp new file mode 100644 index 0000000000..d5f1f57c0c --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/IIM42653.cpp @@ -0,0 +1,861 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "IIM42653.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +IIM42653::IIM42653(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) +{ + if (config.drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + if (config.custom1 != 0) { + _enable_clock_input = true; + _input_clock_freq = config.custom1; + ConfigureCLKIN(); + + } else { + _enable_clock_input = false; + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +IIM42653::~IIM42653() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +int IIM42653::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool IIM42653::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void IIM42653::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void IIM42653::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled"); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +int IIM42653::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true); + } + } + } + + return PX4_ERROR; +} + +void IIM42653::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // DEVICE_CONFIG: Software reset configuration + RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00) + && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { + + // Wakeup accel and gyro and schedule remaining configuration + RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE); + _state = STATE::CONFIGURE; + ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then reset the FIFO + _state = STATE::FIFO_RESET; + ScheduleDelayed(1_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_RESET: + + _state = STATE::FIFO_READ; + FIFOReset(); + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + if (samples == 0) { + // check current FIFO count + const uint16_t fifo_count = FIFOReadCount(); + + if (fifo_count >= FIFO::SIZE) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (fifo_count == 0) { + perf_count(_fifo_empty_perf); + + } else { + // FIFO count (size in bytes) + samples = (fifo_count / sizeof(FIFO::DATA)); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + samples = 0; + } + } + } + + bool success = false; + + if (samples >= 1) { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) + && RegisterCheck(_register_bank1_cfg[_checked_register_bank1]) + && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) + ) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg; + _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +void IIM42653::ConfigureSampleRate(int sample_rate) +{ + // round down to nearest FIFO sample dt + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + +void IIM42653::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO watermark threshold in number of bytes + const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); + + for (auto &r : _register_bank0_cfg) { + if (r.reg == Register::BANK_0::FIFO_CONFIG2) { + // FIFO_WM[7:0] FIFO_CONFIG2 + r.set_bits = fifo_watermark_threshold & 0xFF; + + } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { + // FIFO_WM[11:8] FIFO_CONFIG3 + r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; + } + } +} + +void IIM42653::ConfigureCLKIN() +{ + for (auto &r0 : _register_bank0_cfg) { + if (r0.reg == Register::BANK_0::INTF_CONFIG1) { + r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE; + r0.set_bits = INTF_CONFIG1_BIT::CLKSEL; + r0.clear_bits = INTF_CONFIG1_BIT::CLKSEL_CLEAR; + } + } + + for (auto &r1 : _register_bank1_cfg) { + if (r1.reg == Register::BANK_1::INTF_CONFIG5) { + r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET; + r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR; + } + } +} + +void IIM42653::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) +{ + if (bank != _last_register_bank || force) { + // select BANK_0 + uint8_t cmd_bank_sel[2] {}; + cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); + cmd_bank_sel[1] = bank; + transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel)); + + _last_register_bank = bank; + } +} + +bool IIM42653::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_bank0_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_cfg : _register_bank1_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_cfg : _register_bank2_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_bank0_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + for (const auto ®_cfg : _register_bank1_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + for (const auto ®_cfg : _register_bank2_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // 20-bits data format used + // the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + _px4_gyro.set_range(math::radians(2000.f)); + + return success; +} + +int IIM42653::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void IIM42653::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool IIM42653::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool IIM42653::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +template +bool IIM42653::RegisterCheck(const T ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +template +uint8_t IIM42653::RegisterRead(T reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + SelectRegisterBank(reg); + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +template +void IIM42653::RegisterWrite(T reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + SelectRegisterBank(reg); + transfer(cmd, cmd, sizeof(cmd)); +} + +template +void IIM42653::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +uint16_t IIM42653::FIFOReadCount() +{ + // read FIFO count + uint8_t fifo_count_buf[3] {}; + fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); + + if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} + +bool IIM42653::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE); + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + return false; + } + + const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); + + if (fifo_count_bytes >= FIFO::SIZE) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + return false; + } + + const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); + + if (fifo_count_samples == 0) { + perf_count(_fifo_empty_perf); + return false; + } + + // check FIFO header in every sample + uint8_t valid_samples = 0; + + for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + bool valid = true; + + // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx + const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header; + + if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) { + // FIFO sample empty if HEADER_MSG set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) { + // accel bit not set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) { + // gyro bit not set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) { + // Packet does not contain a new and valid extended 20-bit data + valid = false; + + } else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) { + // Packet does not contain ODR timestamp + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) { + // accel ODR changed + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) { + // gyro ODR changed + valid = false; + } + + if (valid) { + valid_samples++; + + } else { + perf_count(_bad_transfer_perf); + break; + } + } + + if (valid_samples > 0) { + if (ProcessTemperature(buffer.f, valid_samples)) { + ProcessGyro(timestamp_sample, buffer.f, valid_samples); + ProcessAccel(timestamp_sample, buffer.f, valid_samples); + return true; + } + } + + return false; +} + +void IIM42653::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // SIGNAL_PATH_RESET: FIFO flush + RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + + // reset while FIFO is disabled + _drdy_timestamp_sample.store(0); +} + +static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c) +{ + // 0xXXXAABBC + uint32_t high = ((a << 12) & 0x000FF000); + uint32_t low = ((b << 4) & 0x00000FF0); + uint32_t lowest = (c & 0x0000000F); + + uint32_t x = high | low | lowest; + + if (a & Bit7) { + // sign extend + x |= 0xFFF00000u; + } + + return static_cast(x); +} + +void IIM42653::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + + // 18-bits of accelerometer data + bool scale_20bit = false; + + // first pass + for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + + // 20 bit hires mode + // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) + // Accel data is 18 bit () + int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0, + fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4); + int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0, + fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4); + int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0, + fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4); + + // sample invalid if -524288 + if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) { + // check if any values are going to exceed int16 limits + static constexpr int16_t max_accel = INT16_MAX; + static constexpr int16_t min_accel = INT16_MIN; + + if (accel_x >= max_accel || accel_x <= min_accel) { + scale_20bit = true; + } + + if (accel_y >= max_accel || accel_y <= min_accel) { + scale_20bit = true; + } + + if (accel_z >= max_accel || accel_z <= min_accel) { + scale_20bit = true; + } + + // shift by 2 (2 least significant bits are always 0) + accel.x[accel.samples] = accel_x / 4; + accel.y[accel.samples] = accel_y / 4; + accel.z[accel.samples] = accel_z / 4; + accel.samples++; + } + } + + if (!scale_20bit) { + // if highres enabled accel data is always 4096 LSB/g + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); + + } else { + // 20 bit data scaled to 16 bit (2^4) + for (int i = 0; i < samples; i++) { + // 20 bit hires mode + // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) + // Accel data is 18 bit () + int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0); + int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0); + int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0); + + accel.x[i] = accel_x; + accel.y[i] = accel_y; + accel.z[i] = accel_z; + } + + _px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); + } + + // correct frame for publication + for (int i = 0; i < accel.samples; i++) { + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[i] = accel.x[i]; + accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i]; + accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i]; + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + _px4_accel.updateFIFO(accel); + } +} + +void IIM42653::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = 0; + + // 20-bits of gyroscope data + bool scale_20bit = false; + + // first pass + for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + + // 20 bit hires mode + // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) + int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F); + int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F); + int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F); + + // check if any values are going to exceed int16 limits + static constexpr int16_t max_gyro = INT16_MAX; + static constexpr int16_t min_gyro = INT16_MIN; + + if (gyro_x >= max_gyro || gyro_x <= min_gyro) { + scale_20bit = true; + } + + if (gyro_y >= max_gyro || gyro_y <= min_gyro) { + scale_20bit = true; + } + + if (gyro_z >= max_gyro || gyro_z <= min_gyro) { + scale_20bit = true; + } + + gyro.x[gyro.samples] = gyro_x / 2; + gyro.y[gyro.samples] = gyro_y / 2; + gyro.z[gyro.samples] = gyro_z / 2; + gyro.samples++; + } + + if (!scale_20bit) { + // if highres enabled gyro data is always 65.5 LSB/dps + _px4_gyro.set_scale(math::radians(1.f / 65.5f)); + + } else { + // 20 bit data scaled to 16 bit (2^4) + for (int i = 0; i < samples; i++) { + gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0); + gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0); + gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0); + } + + _px4_gyro.set_scale(math::radians(4000.f / 32768.f)); + } + + // correct frame for publication + for (int i = 0; i < gyro.samples; i++) { + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro.x[i]; + gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i]; + gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i]; + } + + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (gyro.samples > 0) { + _px4_gyro.updateFIFO(gyro); + } +} + +bool IIM42653::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples) +{ + int16_t temperature[FIFO_MAX_SAMPLES]; + float temperature_sum{0}; + + int valid_samples = 0; + + for (int i = 0; i < samples; i++) { + const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0); + + // sample invalid if -32768 + if (t != -32768) { + temperature_sum += t; + temperature[valid_samples] = t; + valid_samples++; + } + } + + if (valid_samples > 0) { + const float temperature_avg = temperature_sum / valid_samples; + + for (int i = 0; i < valid_samples; i++) { + // temperature changing wildly is an indication of a transfer error + if (fabsf(temperature[i] - temperature_avg) > 1000) { + perf_count(_bad_transfer_perf); + return false; + } + } + + // use average temperature reading + const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + return true; + + } else { + perf_count(_bad_transfer_perf); + } + } + + return false; +} diff --git a/src/drivers/imu/invensense/iim42653/IIM42653.hpp b/src/drivers/imu/invensense/iim42653/IIM42653.hpp new file mode 100644 index 0000000000..ab9c551a7a --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/IIM42653.hpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 IIM42653.hpp + * + * Driver for the Invensense IIM42653 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_IIM42653_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace InvenSense_IIM42653; + +class IIM42653 : public device::SPI, public I2CSPIDriver +{ +public: + IIM42653(const I2CSPIDriverConfig &config); + ~IIM42653() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured + static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; + static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; + + static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input + + // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::BANK_0::INT_STATUS) | DIR_READ}; + uint8_t INT_STATUS{0}; + uint8_t FIFO_COUNTH{0}; + uint8_t FIFO_COUNTL{0}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_bank1_config_t { + Register::BANK_1 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_bank2_config_t { + Register::BANK_2 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + void ConfigureSampleRate(int sample_rate); + void ConfigureFIFOWatermark(uint8_t samples); + void ConfigureCLKIN(); + + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); + void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); } + void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1); } + void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2); } + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + template bool RegisterCheck(const T ®_cfg); + template uint8_t RegisterRead(T reg); + template void RegisterWrite(T reg, uint8_t value); + template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); + template void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + template void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; + perf_counter_t _drdy_missed_perf{nullptr}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + bool _enable_clock_input{false}; + float _input_clock_freq{0.f}; + + enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0}; + + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_RESET, + FIFO_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + + uint8_t _checked_register_bank0{0}; + static constexpr uint8_t size_register_bank0_cfg{16}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime + { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, + { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_4000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_32G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR }, + { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD }, + { Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW }, + { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD }, + { Register::BANK_0::TMST_CONFIG, TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN }, + { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime + { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime + { Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 }, + { Register::BANK_0::INT_CONFIG1, 0, INT_CONFIG1_BIT::INT_ASYNC_RESET }, + { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, + }; + + uint8_t _checked_register_bank1{0}; + static constexpr uint8_t size_register_bank1_cfg{5}; + register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS }, + { Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR}, + { Register::BANK_1::INTF_CONFIG5, 0, 0 }, + }; + + uint8_t _checked_register_bank2{0}; + static constexpr uint8_t size_register_bank2_cfg{8}; + register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS }, + { Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR }, + { Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR }, + { Register::BANK_2::AUX1_CONFIG1, 0, AUX1_CONFIG1_BIT::AUX1_ACCEL_LP_CLK_SEL | AUX1_CONFIG1_BIT::GYRO_AUX1_EN | AUX1_CONFIG1_BIT::ACCEL_AUX1_EN}, + { Register::BANK_2::AUX1_CONFIG2, AUX1_CONFIG2_BIT::GYRO_AUX1_HPF_DIS, 0}, + { Register::BANK_2::AUX1_SPI_REG1, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_SET, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_CLEAR }, + { Register::BANK_2::AUX1_SPI_REG2, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_SET, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_CLEAR }, + { Register::BANK_2::AUX1_SPI_REG3, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_SET, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_CLEAR }, + }; +}; diff --git a/src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp b/src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp new file mode 100644 index 0000000000..7bd183787f --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp @@ -0,0 +1,453 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 InvenSense_IIM42653_registers.hpp + * + * Invensense IIM-42653 registers. + * + */ + +#pragma once + +#include +#include + +namespace InvenSense_IIM42653 +{ +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0x56; + +static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 25.f; // C + +namespace Register +{ + +enum class BANK_0 : uint8_t { + DEVICE_CONFIG = 0x11, + + INT_CONFIG = 0x14, + + FIFO_CONFIG = 0x16, + + TEMP_DATA1 = 0x1D, + TEMP_DATA0 = 0x1E, + + INT_STATUS = 0x2D, + FIFO_COUNTH = 0x2E, + FIFO_COUNTL = 0x2F, + FIFO_DATA = 0x30, + + SIGNAL_PATH_RESET = 0x4B, + INTF_CONFIG0 = 0x4C, + INTF_CONFIG1 = 0x4D, + PWR_MGMT0 = 0x4E, + GYRO_CONFIG0 = 0x4F, + ACCEL_CONFIG0 = 0x50, + GYRO_CONFIG1 = 0x51, + GYRO_ACCEL_CONFIG0 = 0x52, + ACCEL_CONFIG1 = 0x53, + TMST_CONFIG = 0x54, + + FIFO_CONFIG1 = 0x5F, + FIFO_CONFIG2 = 0x60, + FIFO_CONFIG3 = 0x61, + + INT_CONFIG0 = 0x63, + INT_CONFIG1 = 0x64, + + INT_SOURCE0 = 0x65, + + SELF_TEST_CONFIG = 0x70, + + WHO_AM_I = 0x75, + REG_BANK_SEL = 0x76, +}; + +enum class BANK_1 : uint8_t { + GYRO_CONFIG_STATIC2 = 0x0B, + GYRO_CONFIG_STATIC3 = 0x0C, + GYRO_CONFIG_STATIC4 = 0x0D, + GYRO_CONFIG_STATIC5 = 0x0E, + + INTF_CONFIG5 = 0x7B, +}; + +enum class BANK_2 : uint8_t { + ACCEL_CONFIG_STATIC2 = 0x03, + ACCEL_CONFIG_STATIC3 = 0x04, + ACCEL_CONFIG_STATIC4 = 0x05, + AUX1_CONFIG1 = 0x44, + AUX1_CONFIG2 = 0x45, + AUX1_CONFIG3 = 0x46, + AUX1_SPI_REG1 = 0x71, + AUX1_SPI_REG2 = 0x72, + AUX1_SPI_REG3 = 0x73, +}; + +}; + +//---------------- BANK0 Register bits + +// DEVICE_CONFIG +enum DEVICE_CONFIG_BIT : uint8_t { + SOFT_RESET_CONFIG = Bit0, // +}; + +// INT_CONFIG +enum INT_CONFIG_BIT : uint8_t { + INT1_MODE = Bit2, + INT1_DRIVE_CIRCUIT = Bit1, + INT1_POLARITY = Bit0, +}; + +// FIFO_CONFIG +enum FIFO_CONFIG_BIT : uint8_t { + // 7:6 FIFO_MODE + FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode +}; + +// INT_STATUS +enum INT_STATUS_BIT : uint8_t { + RESET_DONE_INT = Bit4, + DATA_RDY_INT = Bit3, + FIFO_THS_INT = Bit2, + FIFO_FULL_INT = Bit1, +}; + +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + ABORT_AND_RESET = Bit3, + FIFO_FLUSH = Bit1, +}; + +enum INTF_CONFIG1_BIT : uint8_t { + RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required + CLKSEL = Bit0, + CLKSEL_CLEAR = Bit1, +}; + +// PWR_MGMT0 +enum PWR_MGMT0_BIT : uint8_t { + GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode + ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode +}; + +// GYRO_CONFIG0 +enum GYRO_CONFIG0_BIT : uint8_t { + // 7:5 GYRO_FS_SEL + GYRO_FS_SEL_4000_DPS = 0, // 0b000 = ±4000dps (default) + GYRO_FS_SEL_2000_DPS = Bit5, + GYRO_FS_SEL_1000_DPS = Bit6, + GYRO_FS_SEL_500_DPS = Bit6 | Bit5, + GYRO_FS_SEL_250_DPS = Bit7, + + + // 3:0 GYRO_ODR + // 0001: 32kHz + GYRO_ODR_32KHZ_SET = Bit0, + GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0010: 16kHz + GYRO_ODR_16KHZ_SET = Bit1, + GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0011: 8kHz + GYRO_ODR_8KHZ_SET = Bit1 | Bit0, + GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2, + // 0110: 1kHz (default) + GYRO_ODR_1KHZ_SET = Bit2 | Bit1, + GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0, +}; + +// ACCEL_CONFIG0 +enum ACCEL_CONFIG0_BIT : uint8_t { + // 7:5 ACCEL_FS_SEL + ACCEL_FS_SEL_32G = 0, // 000: ±32g (default) + ACCEL_FS_SEL_16G = Bit5, + ACCEL_FS_SEL_8G = Bit6, + ACCEL_FS_SEL_4G = Bit6 | Bit5, + + + // 3:0 ACCEL_ODR + // 0001: 32kHz + ACCEL_ODR_32KHZ_SET = Bit0, + ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0010: 16kHz + ACCEL_ODR_16KHZ_SET = Bit1, + ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0011: 8kHz + ACCEL_ODR_8KHZ_SET = Bit1 | Bit0, + ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2, + // 0110: 1kHz (default) + ACCEL_ODR_1KHZ_SET = Bit2 | Bit1, + ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0, +}; + +// GYRO_CONFIG1 +enum GYRO_CONFIG1_BIT : uint8_t { + GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order +}; + +// GYRO_ACCEL_CONFIG0 +enum GYRO_ACCEL_CONFIG0_BIT : uint8_t { + // 7:4 ACCEL_UI_FILT_BW + ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2 + + // 3:0 GYRO_UI_FILT_BW + GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2 +}; + +// ACCEL_CONFIG1 +enum ACCEL_CONFIG1_BIT : uint8_t { + ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order +}; + +// TMST_CONFIG +enum TMST_CONFIG_BIT : uint8_t { + TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value + TMST_RES = Bit3, // 0: 1us resolution, 1: 16us resolution + TMST_DELTA_EN = Bit2, // 1: Time Stamp delta enable + TMST_FSYNC_EN = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled + TMST_EN = Bit0, // 1: Time Stamp register enable (default) +}; + +// FIFO_CONFIG1 +enum FIFO_CONFIG1_BIT : uint8_t { + FIFO_RESUME_PARTIAL_RD = Bit6, + FIFO_WM_GT_TH = Bit5, + FIFO_HIRES_EN = Bit4, + FIFO_TMST_FSYNC_EN = Bit3, + FIFO_TEMP_EN = Bit2, + FIFO_GYRO_EN = Bit1, + FIFO_ACCEL_EN = Bit0, +}; + +// INT_CONFIG0 +enum INT_CONFIG0_BIT : uint8_t { + // 3:2 FIFO_THS_INT_CLEAR + CLEAR_ON_FIFO_READ = Bit3, +}; + +// INT_CONFIG1 +enum INT_CONFIG1_BIT : uint8_t { + INT_ASYNC_RESET = Bit4, +}; + +// INT_SOURCE0 +enum INT_SOURCE0_BIT : uint8_t { + UI_FSYNC_INT1_EN = Bit6, + PLL_RDY_INT1_EN = Bit5, + RESET_DONE_INT1_EN = Bit4, + UI_DRDY_INT1_EN = Bit3, + FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 + FIFO_FULL_INT1_EN = Bit1, + UI_AGC_RDY_INT1_EN = Bit0, +}; + +// REG_BANK_SEL +enum REG_BANK_SEL_BIT : uint8_t { + // 2:0 BANK_SEL + BANK_SEL_0 = 0, // 000: Bank 0 (default) + BANK_SEL_1 = Bit0, // 001: Bank 1 + BANK_SEL_2 = Bit1, // 010: Bank 2 + BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3 + BANK_SEL_4 = Bit2, // 100: Bank 4 +}; + + +//---------------- BANK1 Register bits + +// GYRO_CONFIG_STATIC2 +enum GYRO_CONFIG_STATIC2_BIT : uint8_t { + GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter + GYRO_NF_DIS = Bit0, // 1: Disable Notch Filter +}; + +// GYRO_CONFIG_STATIC3 +enum GYRO_CONFIG_STATIC3_BIT : uint8_t { + // 5:0 GYRO_AAF_DELT + // 585 Hz = 13 (0b00'1101) + GYRO_AAF_DELT_585HZ_SET = Bit3 | Bit2 | Bit0, + GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1, +}; + +// GYRO_CONFIG_STATIC4 +enum GYRO_CONFIG_STATIC4_BIT : uint8_t { + // 7:0 GYRO_AAF_DELTSQR + // 585 Hz = 170 (0b1010'1010) + GYRO_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// GYRO_CONFIG_STATIC5 +enum GYRO_CONFIG_STATIC5_BIT : uint8_t { + // 7:4 GYRO_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + GYRO_AAF_BITSHIFT_585HZ_SET = Bit7, + GYRO_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 GYRO_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + GYRO_AAF_DELTSQR_MSB_585HZ_SET = 0, + GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, +}; + +// INTF_CONFIG5 +enum INTF_CONFIG5_BIT : uint8_t { + // 2:1 PIN9_FUNCTION + PIN9_FUNCTION_CLKIN_SET = Bit2, // 0b10: CLKIN + PIN9_FUNCTION_CLKIN_CLEAR = Bit1, + + PIN9_FUNCTION_RESET_SET = 0, + PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1, +}; + +//---------------- BANK2 Register bits + +// ACCEL_CONFIG_STATIC2 +enum ACCEL_CONFIG_STATIC2_BIT : uint8_t { + // 6:1 ACCEL_AAF_DELT + // 585 Hz = 13 (0b00'1101) + ACCEL_AAF_DELT_585HZ_SET = Bit4 | Bit3 | Bit1, + ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2, + + // 0 ACCEL_AAF_DIS + ACCEL_AAF_DIS = Bit0, // 0: Enable accelerometer anti-aliasing filter (default) +}; + +// ACCEL_CONFIG_STATIC3 +enum ACCEL_CONFIG_STATIC3_BIT : uint8_t { + // 7:0 ACCEL_AAF_DELTSQR[7:0] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// ACCEL_CONFIG_STATIC4 +enum ACCEL_CONFIG_STATIC4_BIT : uint8_t { + // 7:4 ACCEL_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + ACCEL_AAF_BITSHIFT_585HZ_SET = Bit7, + ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 ACCEL_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_MSB_SET = 0, + ACCEL_AAF_DELTSQR_MSB_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, +}; + +// AUX1_CONFIG1 +enum AUX1_CONFIG1_BIT : uint8_t { + AUX1_ACCEL_LP_CLK_SEL = Bit5, + GYRO_AUX1_EN = Bit1, + ACCEL_AUX1_EN = Bit0, +}; + +// AUX1_CONFIG2 +enum AUX1_CONFIG2_BIT : uint8_t { + GYRO_AUX1_HPF_DIS = Bit6, +}; + +// AUX1_SPI_REG1 +enum AUX1_SPI_REG1_BIT : uint8_t { + AUX1_SPI_REG1_CLEAR = Bit1, + AUX1_SPI_REG1_SET = Bit0, +}; + +// AUX1_SPI_REG2 +enum AUX1_SPI_REG2_BIT : uint8_t { + AUX1_SPI_REG2_CLEAR = Bit1, + AUX1_SPI_REG2_SET = Bit0, +}; + +// AUX1_SPI_REG3 +enum AUX1_SPI_REG3_BIT : uint8_t { + AUX1_SPI_REG3_CLEAR = Bit1, + AUX1_SPI_REG3_SET = Bit0, +}; + +namespace FIFO +{ +static constexpr size_t SIZE = 2048; + +// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set + +// Packet 4 +struct DATA { + uint8_t FIFO_Header; + uint8_t ACCEL_DATA_X1; // Accel X [19:12] + uint8_t ACCEL_DATA_X0; // Accel X [11:4] + uint8_t ACCEL_DATA_Y1; // Accel Y [19:12] + uint8_t ACCEL_DATA_Y0; // Accel Y [11:4] + uint8_t ACCEL_DATA_Z1; // Accel Z [19:12] + uint8_t ACCEL_DATA_Z0; // Accel Z [11:4] + uint8_t GYRO_DATA_X1; // Gyro X [19:12] + uint8_t GYRO_DATA_X0; // Gyro X [11:4] + uint8_t GYRO_DATA_Y1; // Gyro Y [19:12] + uint8_t GYRO_DATA_Y0; // Gyro Y [11:4] + uint8_t GYRO_DATA_Z1; // Gyro Z [19:12] + uint8_t GYRO_DATA_Z0; // Gyro Z [11:4] + uint8_t TEMP_DATA1; // Temperature[15:8] + uint8_t TEMP_DATA0; // Temperature[7:0] + uint8_t TimeStamp_h; // TimeStamp[15:8] + uint8_t TimeStamp_l; // TimeStamp[7:0] + uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0] + uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0] + uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0] +}; + +// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx +enum FIFO_HEADER_BIT : uint8_t { + HEADER_MSG = Bit7, // 1: FIFO is empty + HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1 + HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1 + HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel + HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp + HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet + HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet +}; + +} +} // namespace InvenSense_IIM42653 diff --git a/src/drivers/imu/invensense/iim42653/Kconfig b/src/drivers/imu/invensense/iim42653/Kconfig new file mode 100644 index 0000000000..903f3d3cb9 --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_IIM42653 + bool "iim42653" + default n + ---help--- + Enable support for iim42653 diff --git a/src/drivers/imu/invensense/iim42653/iim42653_main.cpp b/src/drivers/imu/invensense/iim42653/iim42653_main.cpp new file mode 100644 index 0000000000..655fdd2e19 --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/iim42653_main.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "IIM42653.hpp" + +#include +#include + +void IIM42653::print_usage() +{ + PRINT_MODULE_USAGE_NAME("iim42653", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int iim42653_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = IIM42653; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) { + switch (ch) { + case 'C': + cli.custom1 = atoi(cli.optArg()); + break; + + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_IIM42653); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index eae3dc5486..1608b91b23 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -38,9 +38,15 @@ #include +using matrix::Vector2f; + VectorNav::VectorNav(const char *port) : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _attitude_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_attitude) : ORB_ID(vehicle_attitude)), + _local_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_local_position) : ORB_ID(vehicle_local_position)), + _global_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_global_position) : ORB_ID( + vehicle_global_position)) { // store port name strncpy(_port, port, sizeof(_port) - 1); @@ -48,6 +54,27 @@ VectorNav::VectorNav(const char *port) : // enforce null termination _port[sizeof(_port) - 1] = '\0'; + // VN_MODE 1 full INS + if (_param_vn_mode.get() == 1) { + int32_t v = 0; + + // EKF2_EN 0 (disabled) + v = 0; + param_set(param_find("EKF2_EN"), &v); + + // SYS_MC_EST_GROUP 0 (disabled) + v = 0; + param_set(param_find("SYS_MC_EST_GROUP"), &v); + + // SENS_IMU_MODE (VN handles sensor selection) + v = 0; + param_set(param_find("SENS_IMU_MODE"), &v); + + // SENS_MAG_MODE (VN handles sensor selection) + v = 0; + param_set(param_find("SENS_MAG_MODE"), &v); + } + device::Device::DeviceId device_id{}; device_id.devid_s.devtype = DRV_INS_DEVTYPE_VN300; device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; @@ -75,6 +102,16 @@ VectorNav::~VectorNav() perf_free(_sample_perf); perf_free(_comms_errors); + + perf_free(_accel_pub_interval_perf); + perf_free(_gyro_pub_interval_perf); + perf_free(_mag_pub_interval_perf); + perf_free(_gnss_pub_interval_perf); + perf_free(_baro_pub_interval_perf); + + perf_free(_attitude_pub_interval_perf); + perf_free(_local_position_pub_interval_perf); + perf_free(_global_position_pub_interval_perf); } void VectorNav::binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex) @@ -108,7 +145,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) if (VnUartPacket_isCompatible(packet, COMMONGROUP_NONE, TIMEGROUP_TIMESTARTUP, - (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO), + (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE), GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE, @@ -118,17 +155,21 @@ void VectorNav::sensorCallback(VnUartPacket *packet) uint64_t time_startup = VnUartPacket_extractUint64(packet); (void)time_startup; - // IMUGROUP_UNCOMPACCEL + // IMUGROUP_ACCEL vec3f accel = VnUartPacket_extractVec3f(packet); - // IMUGROUP_UNCOMPGYRO + // IMUGROUP_ANGULARRATE vec3f angular_rate = VnUartPacket_extractVec3f(packet); // publish sensor_accel _px4_accel.update(time_now_us, accel.c[0], accel.c[1], accel.c[2]); + perf_count(_accel_pub_interval_perf); // publish sensor_gyro _px4_gyro.update(time_now_us, angular_rate.c[0], angular_rate.c[1], angular_rate.c[2]); + perf_count(_gyro_pub_interval_perf); + + _time_last_valid_imu_us.store(hrt_absolute_time()); } // binary output 2 @@ -188,9 +229,11 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_baro.temperature = temperature; sensor_baro.timestamp = hrt_absolute_time(); _sensor_baro_pub.publish(sensor_baro); + perf_count(_baro_pub_interval_perf); // publish sensor_mag _px4_mag.update(time_now_us, mag.c[0], mag.c[1], mag.c[2]); + perf_count(_mag_pub_interval_perf); // publish attitude vehicle_attitude_s attitude{}; @@ -201,13 +244,14 @@ void VectorNav::sensorCallback(VnUartPacket *packet) attitude.q[3] = quaternion.c[2]; attitude.timestamp = hrt_absolute_time(); _attitude_pub.publish(attitude); + perf_count(_attitude_pub_interval_perf); // mode const uint16_t mode = (insStatus & 0b11); //const bool mode_initializing = (mode == 0b00); const bool mode_aligning = (mode == 0b01); const bool mode_tracking = (mode == 0b10); - const bool mode_loss_gnss = (mode == 0b11); + //const bool mode_loss_gnss = (mode == 0b11); // mode_initializing @@ -228,41 +272,88 @@ void VectorNav::sensorCallback(VnUartPacket *packet) // - attitude is good // - treat as mode 0 - - if ((mode_aligning || mode_tracking) && !mode_loss_gnss) { + if (mode_aligning || mode_tracking) { // publish local_position - // TODO: projection + const double lat = positionEstimatedLla.c[0]; + const double lon = positionEstimatedLla.c[1]; + const float alt = positionEstimatedLla.c[2]; + + if (!_pos_ref.isInitialized()) { + _pos_ref.initReference(lat, lon, time_now_us); + _gps_alt_ref = alt; + } + + const Vector2f pos_ned = _pos_ref.project(lat, lon); + vehicle_local_position_s local_position{}; local_position.timestamp_sample = time_now_us; - local_position.ax = accelerationLinearNed.c[0]; - local_position.ay = accelerationLinearNed.c[1]; - local_position.az = accelerationLinearNed.c[2]; - local_position.x = positionEstimatedLla.c[0]; // TODO - local_position.y = positionEstimatedLla.c[1]; // TODO - local_position.z = positionEstimatedLla.c[2]; // TODO + + local_position.xy_valid = true; + local_position.z_valid = true; + local_position.v_xy_valid = true; + local_position.v_z_valid = true; + + local_position.x = pos_ned(0); + local_position.y = pos_ned(1); + local_position.z = -(alt - _gps_alt_ref); + local_position.vx = velocityEstimatedNed.c[0]; local_position.vy = velocityEstimatedNed.c[1]; local_position.vz = velocityEstimatedNed.c[2]; + local_position.z_deriv = velocityEstimatedNed.c[2]; // TODO + + local_position.ax = accelerationLinearNed.c[0]; + local_position.ay = accelerationLinearNed.c[1]; + local_position.az = accelerationLinearNed.c[2]; + + matrix::Quatf q{attitude.q}; + local_position.heading = matrix::Eulerf{q}.psi(); + local_position.heading_good_for_control = mode_tracking; + + if (_pos_ref.isInitialized()) { + local_position.xy_global = true; + local_position.z_global = true; + local_position.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + local_position.ref_lat = _pos_ref.getProjectionReferenceLat(); + local_position.ref_lon = _pos_ref.getProjectionReferenceLon(); + local_position.ref_alt = _gps_alt_ref; + } + + local_position.dist_bottom_valid = false; + local_position.eph = positionUncertaintyEstimated; local_position.epv = positionUncertaintyEstimated; local_position.evh = velocityUncertaintyEstimated; local_position.evv = velocityUncertaintyEstimated; - local_position.xy_valid = true; - local_position.heading_good_for_control = mode_tracking; + + local_position.dead_reckoning = false; + + local_position.vxy_max = INFINITY; + local_position.vz_max = INFINITY; + local_position.hagl_min = INFINITY; + local_position.hagl_max = INFINITY; + + local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); _local_position_pub.publish(local_position); - + perf_count(_local_position_pub_interval_perf); // publish global_position vehicle_global_position_s global_position{}; global_position.timestamp_sample = time_now_us; - global_position.lat = positionEstimatedLla.c[0]; - global_position.lon = positionEstimatedLla.c[1]; - global_position.alt = positionEstimatedLla.c[2]; + global_position.lat = lat; + global_position.lon = lon; + global_position.alt = alt; + global_position.alt = alt; + + global_position.eph = positionUncertaintyEstimated; + global_position.epv = positionUncertaintyEstimated; + global_position.timestamp = hrt_absolute_time(); _global_position_pub.publish(global_position); + perf_count(_global_position_pub_interval_perf); } } @@ -333,13 +424,14 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.hdop = dop.hDOP; sensor_gps.vdop = dop.vDOP; - sensor_gps.eph = positionUncertaintyGpsNed.c[0]; + sensor_gps.eph = sqrtf(sq(positionUncertaintyGpsNed.c[0]) + sq(positionUncertaintyGpsNed.c[1])); sensor_gps.epv = positionUncertaintyGpsNed.c[2]; sensor_gps.s_variance_m_s = velocityUncertaintyGps; sensor_gps.timestamp = hrt_absolute_time(); _sensor_gps_pub.publish(sensor_gps); + perf_count(_gnss_pub_interval_perf); } } } @@ -493,7 +585,7 @@ bool VectorNav::configure() 1, // divider COMMONGROUP_NONE, TIMEGROUP_TIMESTARTUP, - (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO), + (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE), GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE, @@ -551,6 +643,8 @@ bool VectorNav::configure() VnSensor_registerAsyncPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this); VnSensor_registerErrorPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this); + _time_configured_us.store(hrt_absolute_time()); + return true; } @@ -582,11 +676,42 @@ void VectorNav::Run() _initialized = true; } else { - ScheduleDelayed(3_s); + ScheduleDelayed(1_s); return; } } + if (_connected && _configured && _initialized) { + + // check for timeout + const hrt_abstime time_configured_us = _time_configured_us.load(); + const hrt_abstime time_last_valid_imu_us = _time_last_valid_imu_us.load(); + + if (_param_vn_mode.get() == 1) { + if ((time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) < 3_s)) + + // update sensor_selection if configured in INS mode + if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) { + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = _px4_accel.get_device_id(); + sensor_selection.gyro_device_id = _px4_gyro.get_device_id(); + sensor_selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(sensor_selection); + } + } + + if ((time_configured_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 5_s) + && (time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 1_s) + ) { + PX4_ERR("timeout, reinitializing"); + VnSensor_unregisterAsyncPacketReceivedHandler(&_vs); + VnSensor_disconnect(&_vs); + _connected = false; + _configured = false; + _initialized = false; + } + } + ScheduleDelayed(100_ms); } diff --git a/src/drivers/ins/vectornav/VectorNav.hpp b/src/drivers/ins/vectornav/VectorNav.hpp index bc1244a009..175a8c5194 100644 --- a/src/drivers/ins/vectornav/VectorNav.hpp +++ b/src/drivers/ins/vectornav/VectorNav.hpp @@ -56,6 +56,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -99,6 +101,9 @@ private: static void binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex); + // return the square of two floating point numbers + static constexpr float sq(float var) { return var * var; } + void sensorCallback(VnUartPacket *packet); char _port[20] {}; @@ -108,7 +113,8 @@ private: bool _connected{false}; bool _configured{false}; - hrt_abstime _last_read{0}; + px4::atomic _time_configured_us{false}; + px4::atomic _time_last_valid_imu_us{false}; VnSensor _vs{}; @@ -122,16 +128,31 @@ private: PX4Gyroscope _px4_gyro{0}; PX4Magnetometer _px4_mag{0}; + MapProjection _pos_ref{}; + float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - uORB::PublicationMulti _attitude_pub{ORB_ID(external_ins_attitude)}; - uORB::PublicationMulti _local_position_pub{ORB_ID(external_ins_local_position)}; - uORB::PublicationMulti _global_position_pub{ORB_ID(external_ins_global_position)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + + uORB::PublicationMulti _attitude_pub; + uORB::PublicationMulti _local_position_pub; + uORB::PublicationMulti _global_position_pub; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _accel_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel publish interval")}; + perf_counter_t _gyro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro publish interval")}; + perf_counter_t _mag_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": mag publish interval")}; + perf_counter_t _gnss_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": GNSS publish interval")}; + perf_counter_t _baro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": baro publish interval")}; + + perf_counter_t _attitude_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": attitude publish interval")}; + perf_counter_t _local_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": local position publish interval")}; + perf_counter_t _global_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": global position publish interval")}; + // TODO: params for GNSS antenna offsets // A @@ -154,4 +175,8 @@ private: // VN_GNSS_ANTB_POS_X // Uncertainty in the X-axis position measurement. + + DEFINE_PARAMETERS( + (ParamInt) _param_vn_mode + ) }; diff --git a/src/drivers/ins/vectornav/module.yaml b/src/drivers/ins/vectornav/module.yaml index c95bfeb608..349fb9869b 100644 --- a/src/drivers/ins/vectornav/module.yaml +++ b/src/drivers/ins/vectornav/module.yaml @@ -1,7 +1,22 @@ module_name: VectorNav (VN-100, VN-200, VN-300) + serial_config: - command: vectornav start -d ${SERIAL_DEV} port_config_param: name: SENS_VN_CFG group: Sensors +parameters: + - group: Sensors + definitions: + + VN_MODE: + description: + short: VectorNav driver mode + long: INS or sensors + category: System + type: enum + values: + 0: Sensors Only (default) + 1: INS + default: 0 diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index be4ae70655..f5e5facd1e 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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 @@ -194,13 +194,13 @@ void PAA3905::RunImpl() _state = STATE::READ; if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; + _motion_interrupt_enabled = true; // backup schedule as a watchdog timeout ScheduleDelayed(1_s); } else { - _data_ready_interrupt_enabled = false; + _motion_interrupt_enabled = false; ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } @@ -222,7 +222,7 @@ void PAA3905::RunImpl() case STATE::READ: { hrt_abstime timestamp_sample = now; - if (_data_ready_interrupt_enabled) { + if (_motion_interrupt_enabled) { // scheduled from interrupt if _drdy_timestamp_sample was set as expected const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); @@ -258,12 +258,6 @@ void PAA3905::RunImpl() PX4_ERR("invalid RawData_Sum > 0x98"); } - // Number of Features = SQUAL * 4 - // RawData_Sum maximum register value is 0x98 - bool data_valid = (buffer.data.SQUAL > 0) - && (buffer.data.RawData_Sum <= 0x98) - && (_discard_reading == 0); - // Bit [5:0] check if chip is working correctly // 0x3F: chip is working correctly if ((buffer.data.Observation & 0x3F) != 0x3F) { @@ -313,7 +307,7 @@ void PAA3905::RunImpl() if (prev_mode != _mode) { // update scheduling on mode change - if (!_data_ready_interrupt_enabled) { + if (!_motion_interrupt_enabled) { ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } } @@ -348,6 +342,14 @@ void PAA3905::RunImpl() const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower; + // Number of Features = SQUAL * 4 + // RawData_Sum maximum register value is 0x98 + bool data_valid = (buffer.data.SQUAL > 0) + && (buffer.data.RawData_Sum > 0) + && (buffer.data.RawData_Sum <= 0x98) + && (shutter > 0) + && (_discard_reading == 0); + switch (_mode) { case Mode::Bright: sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0; @@ -392,12 +394,17 @@ void PAA3905::RunImpl() // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred); - if (data_valid) { - if (motion_reported) { - // only populate flow if data valid (motion and quality > 0) - const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); - const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); + const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + if (data_valid) { + + const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0); + const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1); + + bool publish = false; + + if (motion_reported) { // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; @@ -412,15 +419,53 @@ void PAA3905::RunImpl() sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + + _last_motion = timestamp_sample; + + } else if (zero_flow && (timestamp_sample > _last_motion)) { + // no motion, but burst read looks valid and we should have seen new data by now if there was any motion + const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev) + || (shutter != _shutter_prev) + || (buffer.data.RawData_Sum != _raw_data_sum_prev) + || (buffer.data.SQUAL != _quality_prev); + + if (burst_read_changed) { + + sensor_optical_flow.pixel_flow[0] = 0; + sensor_optical_flow.pixel_flow[1] = 0; + + sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + } } - // only publish when there's motion or at least every second - if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { + // only publish when there's valid data or on timeout + if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { sensor_optical_flow.timestamp = hrt_absolute_time(); _sensor_optical_flow_pub.publish(sensor_optical_flow); - _last_publish = sensor_optical_flow.timestamp; + _last_publish = sensor_optical_flow.timestamp_sample; + } + + // backup schedule if we're reliant on the motion interrupt and there's very little flow + if (_motion_interrupt_enabled && little_to_no_flow) { + switch (_mode) { + case Mode::Bright: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_0); + break; + + case Mode::LowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_1); + break; + + case Mode::SuperLowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_2); + break; + } } success = true; @@ -430,6 +475,12 @@ void PAA3905::RunImpl() } } + _delta_x_raw_prev = delta_x_raw; + _delta_y_raw_prev = delta_y_raw; + _shutter_prev = shutter; + _raw_data_sum_prev = buffer.data.RawData_Sum; + _quality_prev = buffer.data.SQUAL; + } else { perf_count(_bad_transfer_perf); } diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index e27bdf0fb6..c850b0405a 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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 @@ -114,14 +114,22 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; + hrt_abstime _last_motion{0}; + + int16_t _delta_x_raw_prev{0}; + int16_t _delta_y_raw_prev{0}; + uint32_t _shutter_prev{0}; + uint8_t _quality_prev{0}; + uint8_t _raw_data_sum_prev{0}; + int _failure_count{0}; int _discard_reading{0}; px4::atomic _drdy_timestamp_sample{0}; - bool _data_ready_interrupt_enabled{false}; + bool _motion_interrupt_enabled{false}; uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2}; - static constexpr uint32_t kBackupScheduleIntervalUs{200_ms}; + static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval Mode _mode{Mode::LowLight}; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 9b30e7efcf..05a8092a3d 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -199,13 +199,13 @@ void PAW3902::RunImpl() _state = STATE::READ; if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; + _motion_interrupt_enabled = true; // backup schedule as a watchdog timeout ScheduleDelayed(1_s); } else { - _data_ready_interrupt_enabled = false; + _motion_interrupt_enabled = false; ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } @@ -227,7 +227,7 @@ void PAW3902::RunImpl() case STATE::READ: { hrt_abstime timestamp_sample = now; - if (_data_ready_interrupt_enabled) { + if (_motion_interrupt_enabled) { // scheduled from interrupt if _drdy_timestamp_sample was set as expected const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); @@ -263,12 +263,6 @@ void PAW3902::RunImpl() PX4_ERR("invalid RawData_Sum > 0x98"); } - // Number of Features = SQUAL * 4 - // RawData_Sum maximum register value is 0x98 - bool data_valid = (buffer.data.SQUAL > 0) - && (buffer.data.RawData_Sum <= 0x98) - && (_discard_reading == 0); - // publish sensor_optical_flow sensor_optical_flow_s sensor_optical_flow{}; sensor_optical_flow.timestamp_sample = timestamp_sample; @@ -293,6 +287,14 @@ void PAW3902::RunImpl() const uint16_t shutter = (shutter_upper << 8) | shutter_lower; + // Number of Features = SQUAL * 4 + // RawData_Sum maximum register value is 0x98 + bool data_valid = (buffer.data.SQUAL > 0) + && (buffer.data.RawData_Sum > 0) + && (buffer.data.RawData_Sum <= 0x98) + && (shutter > 0) + && (_discard_reading == 0); + switch (_mode) { case Mode::Bright: sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0; @@ -395,12 +397,17 @@ void PAW3902::RunImpl() // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT); - if (data_valid) { - if (motion_reported) { - // only populate flow if data valid (motion and quality > 0) - const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); - const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); + const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + if (data_valid) { + + const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0); + const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1); + + bool publish = false; + + if (motion_reported) { // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; @@ -415,15 +422,53 @@ void PAW3902::RunImpl() sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + + _last_motion = timestamp_sample; + + } else if (zero_flow && (timestamp_sample > _last_motion)) { + // no motion, but burst read looks valid and we should have seen new data by now if there was any motion + const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev) + || (shutter != _shutter_prev) + || (buffer.data.RawData_Sum != _raw_data_sum_prev) + || (buffer.data.SQUAL != _quality_prev); + + if (burst_read_changed) { + + sensor_optical_flow.pixel_flow[0] = 0; + sensor_optical_flow.pixel_flow[1] = 0; + + sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + } } - // only publish when there's motion or at least every second - if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { + // only publish when there's valid data or on timeout + if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { sensor_optical_flow.timestamp = hrt_absolute_time(); _sensor_optical_flow_pub.publish(sensor_optical_flow); - _last_publish = sensor_optical_flow.timestamp; + _last_publish = sensor_optical_flow.timestamp_sample; + } + + // backup schedule if we're reliant on the motion interrupt and there's very little flow + if (_motion_interrupt_enabled && little_to_no_flow) { + switch (_mode) { + case Mode::Bright: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_0); + break; + + case Mode::LowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_1); + break; + + case Mode::SuperLowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_2); + break; + } } success = true; @@ -433,6 +478,12 @@ void PAW3902::RunImpl() } } + _delta_x_raw_prev = delta_x_raw; + _delta_y_raw_prev = delta_y_raw; + _shutter_prev = shutter; + _raw_data_sum_prev = buffer.data.RawData_Sum; + _quality_prev = buffer.data.SQUAL; + } else { perf_count(_bad_transfer_perf); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index f5c624e856..b2e568d619 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -114,14 +114,22 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; + hrt_abstime _last_motion{0}; + + int16_t _delta_x_raw_prev{0}; + int16_t _delta_y_raw_prev{0}; + uint16_t _shutter_prev{0}; + uint8_t _quality_prev{0}; + uint8_t _raw_data_sum_prev{0}; + int _failure_count{0}; int _discard_reading{0}; px4::atomic _drdy_timestamp_sample{0}; - bool _data_ready_interrupt_enabled{false}; + bool _motion_interrupt_enabled{false}; uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2}; - static constexpr uint32_t kBackupScheduleIntervalUs{200_ms}; + static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval Mode _mode{Mode::LowLight}; diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index 087ffce6c8..e6e7f236f7 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -350,16 +350,12 @@ void MspOsd::Run() home_position_s home_position{}; _home_position_sub.copy(&home_position); - estimator_status_s estimator_status{}; - _estimator_status_sub.copy(&estimator_status); - vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); // construct and send message const auto msg = msp_osd::construct_COMP_GPS( home_position, - estimator_status, vehicle_global_position, _heartbeat); this->Send(MSP_COMP_GPS, &msg); @@ -379,17 +375,11 @@ void MspOsd::Run() sensor_gps_s vehicle_gps_position{}; _vehicle_gps_position_sub.copy(&vehicle_gps_position); - estimator_status_s estimator_status{}; - _estimator_status_sub.copy(&estimator_status); - vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); // construct and send message - const auto msg = msp_osd::construct_ALTITUDE( - vehicle_gps_position, - estimator_status, - vehicle_local_position); + const auto msg = msp_osd::construct_ALTITUDE(vehicle_gps_position, vehicle_local_position); this->Send(MSP_ALTITUDE, &msg); } diff --git a/src/drivers/osd/msp_osd/msp_osd.hpp b/src/drivers/osd/msp_osd/msp_osd.hpp index ad233e2041..87918c7864 100644 --- a/src/drivers/osd/msp_osd/msp_osd.hpp +++ b/src/drivers/osd/msp_osd/msp_osd.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -145,7 +144,6 @@ private: // subscriptions to desired vehicle display information uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; - uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; uORB::Subscription _log_message_sub{ORB_ID(log_message)}; diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 1a78179855..e2cf9daf5e 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -365,7 +365,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, } msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, - const estimator_status_s &estimator_status, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat) { @@ -375,7 +374,8 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, // Calculate distance and direction to home if (home_position.valid_hpos && home_position.valid_lpos - && estimator_status.solution_status_flags & (1 << 4)) { + && (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) { + float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, home_position.lat, home_position.lon)); @@ -425,7 +425,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude) } msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, - const estimator_status_s &estimator_status, const vehicle_local_position_s &vehicle_local_position) { // initialize result @@ -438,7 +437,7 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, altitude.estimatedActualPosition = 0; } - if (estimator_status.solution_status_flags & (1 << 5)) { + if (vehicle_local_position.v_z_valid) { altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s } else { diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.hpp b/src/drivers/osd/msp_osd/uorb_to_msp.hpp index d841faf522..1f5cdb3b6d 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.hpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.hpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include @@ -94,7 +93,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, // construct an MSP_COMP_GPS struct msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, - const estimator_status_s &estimator_status, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat); @@ -103,7 +101,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude); // construct an MSP_ALTITUDE struct msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, - const estimator_status_s &estimator_status, const vehicle_local_position_s &vehicle_local_position); // construct an MSP_ESC_SENSOR_DATA struct diff --git a/src/drivers/pca9685_pwm_out/Kconfig b/src/drivers/pca9685_pwm_out/Kconfig index 84cc71cda7..ad7c25b8d7 100644 --- a/src/drivers/pca9685_pwm_out/Kconfig +++ b/src/drivers/pca9685_pwm_out/Kconfig @@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT bool "pca9685_pwm_out" default n ---help--- - Enable support for pca9685_pwm_out \ No newline at end of file + Enable support for pca9685_pwm_out + +if DRIVERS_PCA9685_PWM_OUT + config PCA9685_USE_EXTERNAL_CRYSTAL + bool "Use external crystal for clock reference" + default n + + config PCA9685_EXTERNAL_CRYSTAL_FREQ + int "External crystal frequency" + depends on PCA9685_USE_EXTERNAL_CRYSTAL + default 25000000 + + config PCA9685_INTERNAL_CRYSTAL_FREQ + int "Corrected frequency of internal oscillator" + depends on !PCA9685_USE_EXTERNAL_CRYSTAL + default 26075000 +endif \ No newline at end of file diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index c2a1af4b16..7eccd6c04b 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -32,23 +32,66 @@ ****************************************************************************/ #include +#include #include #include "PCA9685.h" -#include + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ +#else +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_INTERNAL_CRYSTAL_FREQ +#endif + +#define PCA9685_DEFAULT_MODE1_CFG PCA9685_MODE1_AI_MASK +#define PCA9685_DEFAULT_MODE2_CFG PCA9685_MODE2_OUTDRV_MASK using namespace drv_pca9685_pwm; PCA9685::PCA9685(int bus, int addr): - I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000) + I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000), + currentFreq(0.0) { } -int PCA9685::Stop() +int PCA9685::init() { - disableAllOutput(); - stopOscillator(); + int ret = I2C::init(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {}; + + buf[0] = PCA9685_REG_MODE1; + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK; // put into sleep mode + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK | PCA9685_MODE1_EXTCLK_MASK; + ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#endif + + buf[0] = PCA9685_REG_MODE2; + buf[1] = PCA9685_DEFAULT_MODE2_CFG; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + return PX4_OK; } @@ -63,198 +106,118 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) memcpy(out, outputs, sizeof(uint16_t) * num_outputs); for (unsigned i = 0; i < num_outputs; ++i) { - out[i] = (uint16_t)roundl((out[i] * _Freq * PCA9685_PWM_RES / (float)1e6)); // convert us to 12 bit resolution + out[i] = calcRawFromPulse(out[i]); } - setPWM(num_outputs, out); - - return 0; + return writePWM(0, out, num_outputs); } -int PCA9685::setFreq(float freq) +int PCA9685::updateFreq(float freq) { - uint16_t realResolution = floorl((float)PCA9685_CLOCK_FREQ / freq); + uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_REFERENCE / freq / PCA9685_PWM_RES) - 1; - if (realResolution < PCA9685_PWM_RES) { // unable to provide enough resolution - PX4_DEBUG("frequency too high"); - return -EINVAL; + if (divider > 0x00FF) { + PX4_ERR("frequency too low"); + return PX4_ERROR; } - uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; - - if (divider > 0x00FF) { // out of divider - PX4_DEBUG("frequency too low"); - return -EINVAL; + if (divider < 0x0003) { + PX4_ERR("frequency too high"); + return PX4_ERROR; } - float freq_err = ((float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) - - (float)(freq * PCA9685_PWM_RES)) - / (float)(freq * PCA9685_PWM_RES); // actually asked for (freq * PCA9685_PWM_RES) - - if (fabsf(freq_err) > 0.01f) { // TODO decide threshold - PX4_WARN("frequency error too large: %.4f", (double)freq_err); - // should we return an error? - } - - _Freq = (float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) / PCA9685_PWM_RES; // use actual pwm freq instead. - - setDivider(divider); - - return PX4_OK; + currentFreq = (float)PCA9685_CLOCK_REFERENCE / (float)((divider + 1) * 4096); + PX4_INFO("PCA9685 PWM frequency: target=%.2f real=%.2f", (double)freq, (double)currentFreq); + return setDivider(divider); } -int PCA9685::initReg() +int PCA9685::updateRAW(const uint16_t *outputs, unsigned int num_outputs) { - uint8_t buf[2] = {}; + return writePWM(0, outputs, num_outputs); +} - buf[0] = PCA9685_REG_MODE1; - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled +int PCA9685::setAllPWM(uint16_t output) +{ + uint16_t val = (uint16_t)roundl((output * currentFreq * PCA9685_PWM_RES / (float)1e6)); + uint8_t buf[] = { + PCA9685_REG_ALLLED_ON_L, + 0x00, 0x00, + (uint8_t)(val & (uint8_t)0xFF), + val != 0 ? (uint8_t)(val >> 8) : (uint8_t)PCA9685_LED_ON_FULL_ON_OFF_MASK + }; + return transfer(buf, sizeof(buf), nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } +int PCA9685::sleep() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK + }; + return transfer(buf, 2, nullptr, 0); +} - ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible +int PCA9685::wake() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG + }; + return transfer(buf, 2, nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - buf[0] = PCA9685_REG_MODE2; - buf[1] = DEFAULT_MODE2_CFG; - ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - return PX4_OK; +int PCA9685::doRestart() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK + }; + return transfer(buf, 2, nullptr, 0); } int PCA9685::probe() { - return I2C::probe(); + int ret = I2C::probe(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {0x00}; + return transfer(buf, 2, buf, 1); } -void PCA9685::setPWM(uint8_t channel, const uint16_t &value) -{ - if (value >= 4096) { - PX4_DEBUG("invalid pwm value"); - return; - } - - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_LED0 + channel * PCA9685_REG_LED_INCREMENT; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = (uint8_t)(value & (uint8_t)0xFF); - buf[4] = value != 0 ? ((uint8_t)(value >> (uint8_t)8)) : PCA9685_LED_ON_FULL_ON_OFF_MASK; - - int ret = transfer(buf, 5, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } -} - -void PCA9685::setPWM(uint8_t channel_count, const uint16_t *value) +int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num) { uint8_t buf[PCA9685_PWM_CHANNEL_COUNT * PCA9685_REG_LED_INCREMENT + 1] = {}; - buf[0] = PCA9685_REG_LED0; - - for (int i = 0; i < channel_count; ++i) { - if (value[i] >= 4096) { - PX4_DEBUG("invalid pwm value"); - return; - } + buf[0] = PCA9685_REG_LED0 + PCA9685_REG_LED_INCREMENT * idx; + for (int i = 0; i < num; ++i) { buf[1 + i * PCA9685_REG_LED_INCREMENT] = 0x00; - buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; - buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & (uint8_t)0xFF); - buf[4 + i * PCA9685_REG_LED_INCREMENT] = value[i] != 0 ? ((uint8_t)(value[i] >> (uint8_t)8)) : - PCA9685_LED_ON_FULL_ON_OFF_MASK; + + if (value[i] == 0) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + + } else if (value[i] == 4096) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + + } else { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & 0xFF); + buf[4 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] >> 8); + } } - int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } + return transfer(buf, num * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); } -void PCA9685::disableAllOutput() -{ - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_ALLLED_ON_L; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK; - - int ret = transfer(buf, 5, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - } -} - -void PCA9685::setDivider(uint8_t value) +int PCA9685::setDivider(uint8_t value) { uint8_t buf[2] = {}; buf[0] = PCA9685_REG_PRE_SCALE; buf[1] = value; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::stopOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // set to sleep - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::startOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("startOscillator: i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::triggerRestart() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - buf[1] |= PCA9685_MODE1_RESTART_MASK; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); - return; - } + return transfer(buf, 2, nullptr, 0); } diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index 5c7740164c..e8bd5eb4d8 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -34,10 +34,7 @@ #pragma once #include #include -#include - -namespace drv_pca9685_pwm -{ +#include #define PCA9685_REG_MODE1 0x00 // Mode register 1 #define PCA9685_REG_MODE2 0x01 // Mode register 2 @@ -82,93 +79,98 @@ namespace drv_pca9685_pwm // PRE_SCALE register #define PCA9685_PRE_SCALE_MASK 0xFF +// common sense #define PCA9685_PWM_CHANNEL_COUNT 16 -#define PCA9685_PWM_RES 4096 //Resolution 4096=12bit -/* This should be 25000000 ideally, - * but it seems most chips have its oscillator working at a higher frequency - * Reference: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/blob/6664ce936210eea53259b814062009d9569a4213/Adafruit_PWMServoDriver.h#L66 */ -#define PCA9685_CLOCK_INT 26075000.0 //25MHz internal clock -#ifndef PCA9685_CLOCK_EXT -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk -#else -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk -#endif +#define PCA9685_PWM_RES 4096 -#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" -#define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency +namespace drv_pca9685_pwm +{ //! Main class that exports features for PCA9685 chip class PCA9685 : public device::I2C { public: PCA9685(int bus, int addr); + ~PCA9685() override = default; - int Stop(); + int init() override; /* - * outputs formatted to us. + * Write new PWM value to device + * + * *output: pulse width, us */ int updatePWM(const uint16_t *outputs, unsigned num_outputs); - int setFreq(float freq); - - ~PCA9685() override = default; - - int initReg(); - - inline float getFrequency() {return _Freq;} - /* - * disable all of the output + * Set PWM frequency to new value. + * + * Only a few of precious frequency can be set, while others will be rounded to the nearest possible value. + * + * Only allowed when PCA9685 is put into sleep mode + * + * freq: Hz */ - void disableAllOutput(); + int updateFreq(float freq); /* - * turn off oscillator - */ - void stopOscillator(); - - /* - * turn on oscillator + * Write new PWM value to device, in raw counter value + * + * *output: 0~4095 */ - void startOscillator(); + int updateRAW(const uint16_t *outputs, unsigned num_outputs); /* - * turn on output + * Get the real frequency */ - void triggerRestart(); + float inline getFreq() {return currentFreq;} + + uint16_t inline calcRawFromPulse(uint16_t pulse_width) + { + return (uint16_t)roundl((pulse_width * currentFreq * PCA9685_PWM_RES / (float)1e6)); + } + + /* + * Set PWM value on all channels at once + */ + int setAllPWM(uint16_t output); + + /* + * Put PCA9685 into sleep mode + * + * This will disable the clock reference inside PCA9685 + */ + int sleep(); + + /* + * Put PCA9685 out of sleep mode. + * + * Must wait 500 us for oscillator stabilization before outputting anything + */ + int wake(); + + /* + * If PCA9685 is put into sleep without clearing all the outputs, + * then the restart command will be available, and it can bring back PWM output without the + * need of updatePWM() call. + */ + int doRestart(); protected: int probe() override; -#ifdef PCA9685_CLOCL_EXT - static const uint8_t DEFAULT_MODE1_CFG = 0x70; // Auto-Increment, Sleep, EXTCLK -#else - static const uint8_t DEFAULT_MODE1_CFG = 0x30; // Auto-Increment, Sleep -#endif - static const uint8_t DEFAULT_MODE2_CFG = 0x04; // totem pole - - float _Freq = PWM_DEFAULT_FREQUENCY; - - /** - * set PWM value for a channel[0,15]. - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel, const uint16_t &value); - - /** - * set all PWMs in a single I2C transmission. - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel_count, const uint16_t *value); - /* * set clock divider */ - void setDivider(uint8_t value); + int setDivider(uint8_t value); + + /* + * Write PWM value to PCA9685 + */ + int writePWM(uint8_t idx, const uint16_t *value, uint8_t num); private: - + float currentFreq; }; } diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 4c3ee60c54..0e9bfa63df 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -33,9 +33,10 @@ /** * @file pca9685/main.cpp - * A cross-platform driver and wrapper for pca9685. - * Designed to support all control-groups by binding to correct mixer files - * @author SalimTerryLi + * + * This file serves as the wrapper layer for PCA9685 driver, working with parameters + * and scheduling stuffs on PX4 side. + * */ #include @@ -45,11 +46,10 @@ #include #include #include +#include #include "PCA9685.h" -#include - #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -59,26 +59,25 @@ using namespace time_literals; class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface { public: - PCA9685Wrapper(int schd_rate_limit = 400); - ~PCA9685Wrapper() override ; + PCA9685Wrapper(); + ~PCA9685Wrapper() override; + PCA9685Wrapper(const PCA9685Wrapper &) = delete; + PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; int init(); - /** @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 updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override; - PCA9685Wrapper(const PCA9685Wrapper &) = delete; - PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; - int print_status() override; +protected: + void updateParams() override; + private: perf_counter_t _cycle_perf; @@ -86,40 +85,37 @@ private: INIT, WAIT_FOR_OSC, RUNNING - }; - STATE _state{STATE::INIT}; - // used to compare and cancel unecessary scheduling changes caused by parameter update - int32_t _last_fetched_Freq = -1; - // If this value is above zero, then change freq and scheduling in running state. - float _targetFreq = -1.0f; + } state{STATE::INIT}; + PCA9685 *pca9685 = nullptr; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + MixingOutput _mixing_output { + "PCA9685", + PCA9685_PWM_CHANNEL_COUNT, + *this, + MixingOutput::SchedulingPolicy::Disabled, + true + }; + + float param_pwm_freq, previous_pwm_freq; + float param_schd_rate, previous_schd_rate; + uint32_t param_duty_mode; void Run() override; - -protected: - int _schd_rate_limit = 400; - - PCA9685 *pca9685 = nullptr; // driver handle. - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - MixingOutput _mixing_output{"PCA9685", PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; }; -PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : +PCA9685Wrapper::PCA9685Wrapper() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), - _schd_rate_limit(schd_rate_limit) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { } PCA9685Wrapper::~PCA9685Wrapper() { - if (pca9685 != nullptr) { // normally this should not be called. - PX4_DEBUG("Destruction of PCA9685Wrapper without pwmDevice unloaded!"); - pca9685->Stop(); // force stop + if (pca9685 != nullptr) { + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; - pca9685 = nullptr; } perf_free(_cycle_perf); @@ -129,18 +125,7 @@ int PCA9685Wrapper::init() { int ret = pca9685->init(); - if (ret != PX4_OK) { - return ret; - } - - param_t param_h = param_find("PCA9685_RATE"); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &_targetFreq); - - } else { - PX4_DEBUG("PARAM_INVALID: PCA9685_RATE"); - } + if (ret != PX4_OK) { return ret; } this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); @@ -154,7 +139,26 @@ int PCA9685Wrapper::init() bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - return pca9685->updatePWM(outputs, num_outputs) == 0 ? true : false; + if (state != STATE::RUNNING) { return false; } + + uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] = {}; + num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs; + + for (uint8_t i = 0; i < num_outputs; ++i) { + if (param_duty_mode & (1 << i)) { + low_level_outputs[i] = outputs[i]; + + } else { + low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]); + } + } + + if (pca9685->updateRAW(low_level_outputs, num_outputs) != PX4_OK) { + PX4_ERR("Failed to write PWM to PCA9685"); + return false; + } + + return true; } void PCA9685Wrapper::Run() @@ -163,7 +167,8 @@ void PCA9685Wrapper::Run() ScheduleClear(); _mixing_output.unregister(); - pca9685->Stop(); + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; pca9685 = nullptr; @@ -171,44 +176,27 @@ void PCA9685Wrapper::Run() return; } - perf_begin(_cycle_perf); - - switch (_state) { + switch (state) { case STATE::INIT: - pca9685->initReg(); + updateParams(); + pca9685->updateFreq(param_pwm_freq); + previous_pwm_freq = param_pwm_freq; + previous_schd_rate = param_schd_rate; - if (_targetFreq > 0.0f) { - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency to %.2f, fall back to 50Hz", (double)_targetFreq); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - - } else { - // should not happen - PX4_ERR("INIT failed: invalid initial frequency settings"); - } - - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; + pca9685->wake(); + state = STATE::WAIT_FOR_OSC; ScheduleDelayed(500); break; case STATE::WAIT_FOR_OSC: { - pca9685->triggerRestart(); // start actual outputting - _state = STATE::RUNNING; - float schedule_rate = pca9685->getFrequency(); - - if (_schd_rate_limit < pca9685->getFrequency()) { - schedule_rate = _schd_rate_limit; - } - - ScheduleOnInterval(1000000 / schedule_rate, 1000000 / schedule_rate); + state = STATE::RUNNING; + ScheduleOnInterval(1000000 / param_schd_rate, 0); } break; case STATE::RUNNING: + perf_begin(_cycle_perf); + _mixing_output.update(); // check for parameter updates @@ -219,30 +207,36 @@ void PCA9685Wrapper::Run() // update parameters from storage updateParams(); + + // apply param updates + if ((float)fabs(previous_pwm_freq - param_pwm_freq) > 0.01f) { + previous_pwm_freq = param_pwm_freq; + + ScheduleClear(); + + pca9685->sleep(); + pca9685->updateFreq(param_pwm_freq); + pca9685->wake(); + + // update of PWM freq will always trigger scheduling change + previous_schd_rate = param_schd_rate; + + state = STATE::WAIT_FOR_OSC; + ScheduleDelayed(500); + + } else if ((float)fabs(previous_schd_rate - param_schd_rate) > 0.01f) { + // case when PWM freq not changed but scheduling rate does + previous_schd_rate = param_schd_rate; + ScheduleClear(); + ScheduleOnInterval(1000000 / param_schd_rate, 1000000 / param_schd_rate); + } } _mixing_output.updateSubscriptions(false); - if (_targetFreq > 0.0f) { // check if frequency should be changed - ScheduleClear(); - pca9685->disableAllOutput(); - pca9685->stopOscillator(); - - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency, fall back to 50Hz"); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; - ScheduleDelayed(500); - } - + perf_end(_cycle_perf); break; } - - perf_end(_cycle_perf); } int PCA9685Wrapper::print_usage(const char *reason) @@ -254,27 +248,24 @@ int PCA9685Wrapper::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module is responsible for generate pwm pulse with PCA9685 chip. +This is a PCA9685 PWM output driver. -It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. +It runs on I2C workqueue which is asynchronous with FC control loop, +fetching the latest mixing result and write them to PCA9685 at its scheduling ticks. -### Implementation -This module depends on ModuleBase and OutputModuleInterface. -IIC communication is based on CDev::I2C +It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width +that can be accepted by most ESCs and servos. ### Examples It is typically started with: -$ pca9685_pwm_out start -a 64 -b 1 +$ pca9685_pwm_out start -a 0x40 -b 1 -The number X can be acquired by executing -`pca9685_pwm_out status` when this driver is running. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); - PRINT_MODULE_USAGE_PARAM_INT('a',64,0,255,"device address on this bus",true); + PRINT_MODULE_USAGE_PARAM_STRING('a',"0x40","","7-bits I2C address of PCA9685",true); PRINT_MODULE_USAGE_PARAM_INT('b',1,0,255,"bus that pca9685 is connected to",true); - PRINT_MODULE_USAGE_PARAM_INT('r',400,50,400,"schedule rate limit",true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -282,39 +273,42 @@ The number X can be acquired by executing int PCA9685Wrapper::print_status() { int ret = ModuleBase::print_status(); - PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, true frequency %.5f", + PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, real frequency %.2f", pca9685->get_device_bus(), pca9685->get_device_address(), - (double)(pca9685->getFrequency())); + (double)(pca9685->getFreq())); return ret; } -int PCA9685Wrapper::custom_command(int argc, char **argv) { // only for test use +int PCA9685Wrapper::custom_command(int argc, char **argv) { return PX4_OK; } int PCA9685Wrapper::task_spawn(int argc, char **argv) { - int ch; int address=PCA9685_DEFAULT_ADDRESS; int iicbus=PCA9685_DEFAULT_IICBUS; - int schd_rate_limit=400; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "a:b:r:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': - address = atoi(myoptarg); + errno = 0; + address = strtol(myoptarg, nullptr, 16); + if (errno != 0) { + PX4_WARN("Invalid address"); + return PX4_ERROR; + } break; case 'b': - iicbus = atoi(myoptarg); - break; - - case 'r': - schd_rate_limit = atoi(myoptarg); + iicbus = strtol(myoptarg, nullptr, 10); + if (errno != 0) { + PX4_WARN("Invalid bus"); + return PX4_ERROR; + } break; case '?': @@ -326,7 +320,7 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { } } - auto *instance = new PCA9685Wrapper(schd_rate_limit); + auto *instance = new PCA9685Wrapper(); if (instance) { _object.store(instance); @@ -358,6 +352,31 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { return PX4_ERROR; } +void PCA9685Wrapper::updateParams() { + ModuleParams::updateParams(); + + param_t param = param_find("PCA9685_SCHD_HZ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_schd_rate); + } else { + PX4_ERR("param PCA9685_SCHD_HZ not found"); + } + + param = param_find("PCA9685_PWM_FREQ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_pwm_freq); + } else { + PX4_ERR("param PCA9685_PWM_FREQ not found"); + } + + param = param_find("PCA9685_DUTY_EN"); + if (param != PARAM_INVALID) { + param_get(param, (int32_t*)¶m_duty_mode); + } else { + PX4_ERR("param PCA9685_DUTY_EN not found"); + } +} + extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ return PCA9685Wrapper::main(argc, argv); } diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 78801fbdb6..198810f8cb 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -5,23 +5,75 @@ actuator_output: channel_label: 'Channel' standard_params: disarmed: { min: 800, max: 2200, default: 1000 } - min: { min: 800, max: 1400, default: 1000 } - max: { min: 1600, max: 2200, default: 2000 } + min: { min: 800, max: 1400, default: 1100 } + max: { min: 1600, max: 2200, default: 1900 } failsafe: { min: 800, max: 2200 } + custom_params: + - name: 'DUTY_EN' + label: "Duty-Cycle\n Mode" + index_offset: -1 + show_as: bitset + advanced: true num_channels: 16 + parameters: - - group: PCA9685 - definitions: - PCA9685_RATE: - description: - short: PWM frequency for PCA9685 - long: | - Update rate at which the PWM signal is sent to the ESC. - type: float - decimal: 1 - increment: 0.1 - default: 50 - min: 50 - max: 450 - unit: Hz - reboot_required: true + - group: Actuator Outputs + definitions: + PCA9685_SCHD_HZ: + description: + short: PWM update rate + long: | + Controls the update rate of PWM output. + Flight Controller will inform those numbers of update events in a second, to PCA9685. + Higher update rate will consume more I2C bandwidth, which may even lead to worse + output latency, or completely block I2C bus. + type: float + decimal: 2 + min: 50.0 + max: 400.0 + default: 50.0 + PCA9685_PWM_FREQ: + description: + short: PWM cycle frequency + long: | + Controls the PWM frequency at timing perspective. + This is independent from PWM update frequency, as PCA9685 is capable to output + without being continuously commanded by FC. + Higher frequency leads to more accurate pulse width, but some ESCs and servos may not support it. + This parameter should be set to the same value as PWM update rate in most case. + This parameter MUST NOT exceed upper limit of 400.0, if any outputs as generic 1000~2000us + pulse width is desired. Frequency higher than 400 only makes sense in duty-cycle mode. + type: float + decimal: 2 + min: 23.8 + max: 1525.87 + default: 50.0 + PCA9685_DUTY_EN: + description: + short: Put the selected channels into Duty-Cycle output mode + long: | + The driver will output standard pulse-width encoded signal without this bit set. + To make PCA9685 output in duty-cycle fashion, please enable the corresponding + channel bit here and adjusting standard params to suit your need. + The driver will have 12bits resolution for duty-cycle output. That means to achieve 0% to 100% + output range on one channel, the corresponding params MIN and MAX for the channel should be set + to 0 and 4096. Other standard params follows the same rule. + type: bitmask + bit: + 0: Put CH1 to Duty-Cycle mode + 1: Put CH2 to Duty-Cycle mode + 2: Put CH3 to Duty-Cycle mode + 3: Put CH4 to Duty-Cycle mode + 4: Put CH5 to Duty-Cycle mode + 5: Put CH6 to Duty-Cycle mode + 6: Put CH7 to Duty-Cycle mode + 7: Put CH8 to Duty-Cycle mode + 8: Put CH9 to Duty-Cycle mode + 9: Put CH10 to Duty-Cycle mode + 10: Put CH11 to Duty-Cycle mode + 11: Put CH12 to Duty-Cycle mode + 12: Put CH13 to Duty-Cycle mode + 13: Put CH14 to Duty-Cycle mode + 14: Put CH15 to Duty-Cycle mode + 15: Put CH16 to Duty-Cycle mode + default: 0 diff --git a/src/drivers/power_monitor/ina228/ina228.h b/src/drivers/power_monitor/ina228/ina228.h index d010d5e4b9..9b57e5011a 100644 --- a/src/drivers/power_monitor/ina228/ina228.h +++ b/src/drivers/power_monitor/ina228/ina228.h @@ -214,7 +214,7 @@ using namespace time_literals; #define INA228_ENERGY_SHIFTS (0) #define INA228_ENERGY_MASK (UINT64_C(0xffffffffff) << INA228_ENERGY_SHIFTS) -/* INA228 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ +/* INA228 Charge Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ #define INA228_CHARGE_SHIFTS (0) #define INA228_CHARGE_MASK (UINT64_C(0xffffffffff) << INA228_CHARGE_SHIFTS) diff --git a/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt new file mode 100644 index 0000000000..b715c5cec5 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/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 drivers__power_monitor_pm_selector_auterion + MAIN pm_selector_auterion + SRCS + PowerMonitorSelectorAuterion.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/power_monitor/pm_selector_auterion/Kconfig b/src/drivers/power_monitor/pm_selector_auterion/Kconfig new file mode 100644 index 0000000000..e9d1c3f16f --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION + bool "pm_selector_auterion" + default n + ---help--- + Enable support for pm_selector_auterion diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp new file mode 100644 index 0000000000..97c89bea61 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Automatic handling power monitors + */ + +#include "PowerMonitorSelectorAuterion.h" +#include "../ina226/ina226.h" + +#include +#include + +PowerMonitorSelectorAuterion::PowerMonitorSelectorAuterion() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +PowerMonitorSelectorAuterion::~PowerMonitorSelectorAuterion() = default; + +bool PowerMonitorSelectorAuterion::init() +{ + int32_t sens_en = 0; + param_get(param_find("SENS_EN_INA226"), &sens_en); + + if (sens_en == 1) { + + sens_en = 0; + param_set(param_find("SENS_EN_INA226"), &sens_en); + const char *stop_argv[] {"ina226", "stop", NULL}; + exec_builtin("ina226", (char **)stop_argv, NULL, 0); + } + + ScheduleNow(); + return true; +} + +void PowerMonitorSelectorAuterion::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + actuator_armed_s actuator_armed{}; + _actuator_armed_sub.copy(&actuator_armed); + + if (actuator_armed.armed) { + exit_and_cleanup(); + return; + } + + for (uint32_t i = 0U; i < SENSORS_NUMBER; ++i) { + + if (!_sensors[i].started) { + + int ret_val = ina226_probe(i); + + if (ret_val == PX4_OK) { + + float current_shunt_value = 0.0f; + param_get(param_find("INA226_SHUNT"), ¤t_shunt_value); + + if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) { + param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value)); + } + + char bus_number[4] = {0}; + itoa(_sensors[i].bus_number, bus_number, 10); + const char *start_argv[] { + _sensors[i].name, + "-X", "-b", bus_number, "-a", _sensors[i].i2c_addr, + "-t", _sensors[i].id, "-q", "start", NULL + }; + + int status = PX4_ERROR; + int pid = exec_builtin(_sensors[i].name, (char **)start_argv, NULL, 0); + + if (pid != -1) { + waitpid(pid, &status, WUNTRACED); + } + + if (status == PX4_OK) { + _sensors[i].started = true; + } + } + } + } + + ScheduleDelayed(RUN_INTERVAL); +} + +int PowerMonitorSelectorAuterion::ina226_probe(uint32_t instance) +{ + struct i2c_master_s *i2c = px4_i2cbus_initialize(_sensors[instance].bus_number); + int ret = PX4_ERROR; + + if (i2c != nullptr) { + + struct i2c_msg_s msgv[2]; + + uint8_t txdata[1] = {0}; + uint8_t rxdata[2] = {0}; + + msgv[0].frequency = I2C_SPEED_STANDARD; + msgv[0].addr = static_cast(strtol(_sensors[instance].i2c_addr, NULL, 0)); + msgv[0].flags = 0; + msgv[0].buffer = txdata; + msgv[0].length = sizeof(txdata); + + msgv[1].frequency = I2C_SPEED_STANDARD; + msgv[1].addr = static_cast(strtol(_sensors[instance].i2c_addr, NULL, 0)); + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = rxdata; + msgv[1].length = sizeof(rxdata); + + txdata[0] = {INA226_MFG_ID}; + ret = I2C_TRANSFER(i2c, msgv, 2); + uint16_t value = static_cast(rxdata[1] | rxdata[0] << 8); + + if (ret != PX4_OK || value != INA226_MFG_ID_TI) { + + ret = PX4_ERROR; + + } else { + + txdata[0] = {INA226_MFG_DIEID}; + ret = I2C_TRANSFER(i2c, msgv, 2); + value = static_cast(rxdata[1] | rxdata[0] << 8); + + if (ret != PX4_OK || value != INA226_MFG_DIE) { + ret = PX4_ERROR; + } + } + + px4_i2cbus_uninitialize(i2c); + } + + return ret; +} + +int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[]) +{ + PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion(); + + 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 PowerMonitorSelectorAuterion::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int PowerMonitorSelectorAuterion::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for starting and auto-detecting different power monitors. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pm_selector_auterion", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int pm_selector_auterion_main(int argc, char *argv[]) +{ + return PowerMonitorSelectorAuterion::main(argc, argv); +} diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h new file mode 100644 index 0000000000..2bc9bffbe3 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class PowerMonitorSelectorAuterion : public ModuleBase, public px4::ScheduledWorkItem +{ + +public: + PowerMonitorSelectorAuterion(); + virtual ~PowerMonitorSelectorAuterion(); + + /** @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); + +private: + + void Run() override; + + bool init(); + + int ina226_probe(uint32_t instance); + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; ///< system armed control topic + + struct Sensor { + const char *name; + const char *i2c_addr; + const uint8_t bus_number; + float shunt_value; + bool started; + const char *id; + }; + + static constexpr int RUN_INTERVAL{500_ms}; + static constexpr int SENSORS_NUMBER{4}; + + Sensor _sensors[SENSORS_NUMBER] = { + { + .name = "ina226", + .i2c_addr = "0x41", + .bus_number = 1, + .shunt_value = 0.0008f, + .started = false, + .id = "1" + }, + { + .name = "ina226", + .i2c_addr = "0x40", + .bus_number = 1, + .shunt_value = 0.0005f, + .started = false, + .id = "1" + }, + { + .name = "ina226", + .i2c_addr = "0x41", + .bus_number = 2, + .shunt_value = 0.0008f, + .started = false, + .id = "2" + }, + { + .name = "ina226", + .i2c_addr = "0x40", + .bus_number = 2, + .shunt_value = 0.0005f, + .started = false, + .id = "2" + } + }; +}; diff --git a/src/drivers/rc/crsf_rc/CrsfParser.cpp b/src/drivers/rc/crsf_rc/CrsfParser.cpp index ba0ddac233..aa499db9f5 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.cpp +++ b/src/drivers/rc/crsf_rc/CrsfParser.cpp @@ -293,9 +293,9 @@ bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserSta } else { // We don't know what this packet is, so we'll let the parser continue // just so that we can dequeue it in one shot - working_segment_size = packet_size + PACKET_SIZE_TYPE_SIZE; + working_segment_size = packet_size - PACKET_SIZE_TYPE_SIZE; - if (working_segment_size > CRSF_MAX_PACKET_LEN) { + if (working_index + working_segment_size + CRC_SIZE > CRSF_MAX_PACKET_LEN) { parser_statistics->invalid_unknown_packet_sizes++; parser_state = PARSER_STATE_HEADER; working_segment_size = HEADER_SIZE; diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 5b1993eaac..5caab7e973 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -124,6 +124,7 @@ RCInput::task_spawn(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; const char *device_name = nullptr; + bool silent = false; #if defined(RC_SERIAL_PORT) device_name = RC_SERIAL_PORT; #endif // RC_SERIAL_PORT @@ -133,6 +134,7 @@ RCInput::task_spawn(int argc, char *argv[]) // if RC_SERIAL_PORT == PX4IO_SERIAL_DEVICE then don't use it by default if the px4io is running if ((strcmp(RC_SERIAL_PORT, PX4IO_SERIAL_DEVICE) == 0) && (access("/dev/px4io", R_OK) == 0)) { device_name = nullptr; + silent = true; } #endif // RC_SERIAL_PORT && PX4IO_SERIAL_DEVICE @@ -141,6 +143,7 @@ RCInput::task_spawn(int argc, char *argv[]) switch (ch) { case 'd': device_name = myoptarg; + silent = false; break; case '?': @@ -173,6 +176,9 @@ RCInput::task_spawn(int argc, char *argv[]) return PX4_OK; + } else if (silent) { + return PX4_OK; + } else { if (device_name) { PX4_ERR("invalid device (-d) %s", device_name); diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 150bc21fe9..f800a00566 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -249,7 +249,7 @@ build_gps_response(uint8_t *buffer, size_t *size) if (lat < 0) { msg.latitude_ns = 1; - lat = abs(lat); + lat = fabs(lat); } int deg; diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index c5f3d1e76a..d2f064e8ba 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -4,6 +4,81 @@ menuconfig DRIVERS_UAVCAN ---help--- Enable support for uavcan +if DRIVERS_UAVCAN + config UAVCAN_ARMING_CONTROLLER + bool "Include arming status controller" + default y + + config UAVCAN_BEEP_CONTROLLER + bool "Include beep controller" + default y + + config UAVCAN_OUTPUTS_CONTROLLER + bool "Include servo & ESC controller" + default y + + config UAVCAN_HARDPOINT_CONTROLLER + bool "Include hardpoint controller" + default y + + config UAVCAN_SAFETY_STATE_CONTROLLER + bool "Include safety state controller" + default y + + config UAVCAN_RGB_CONTROLLER + bool "Include rgb controller" + default y + + config UAVCAN_SENSOR_ACCEL + bool "Subscribe to IMU: uavcan::equipment::ahrs::RawIMU" + default y + + config UAVCAN_SENSOR_AIRSPEED + bool "Subscribe to Airspeed: uavcan::equipment::air_data::IndicatedAirspeed | uavcan::equipment::air_data::TrueAirspeed | uavcan::equipment::air_data::StaticTemperature" + default y + + config UAVCAN_SENSOR_BARO + bool "Subscribe to Barometer: uavcan::equipment::air_data::StaticPressure | uavcan::equipment::air_data::StaticTemperature" + default y + + config UAVCAN_SENSOR_BATTERY + bool "Subscribe to Battery: uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux" + default y + + config UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE + bool "Subscribe to Differential Pressure: uavcan::equipment::air_data::RawAirData" + default y + + config UAVCAN_SENSOR_FLOW + bool "Subscribe to Flow: com::hex::equipment::flow::Measurement" + default y + + config UAVCAN_SENSOR_GNSS + bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" + default y + + config UAVCAN_SENSOR_HYGROMETER + bool "Subscribe to Hygrometer: dronecan::sensors::hygrometer::Hygrometer" + default y + + config UAVCAN_SENSOR_ICE_STATUS + bool "Subscribe to Internal Combustion Engine: uavcan::equipment::ice::reciprocating::Status" + default y + + config UAVCAN_SENSOR_MAG + bool "Subscribe to Magnetometer: uavcan::equipment::ahrs::MagneticFieldStrength | uavcan::equipment::ahrs::MagneticFieldStrength2" + default y + + config UAVCAN_SENSOR_RANGEFINDER + bool "Subscribe to Rangefinder: uavcan::equipment::range_sensor::Measurement" + default y + + config UAVCAN_SENSOR_SAFETY_BUTTON + bool "Subscribe to Safety Button: ardupilot::indication::Button" + default y + +endif #DRIVERS_UAVCAN + menuconfig BOARD_UAVCAN_INTERFACES depends on DRIVERS_UAVCAN || DRIVERS_UAVCANNODE diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index 099de615f0..f084cd98b3 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -79,7 +79,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructureupdate(hrt_absolute_time(), msg.range); + int8_t quality = -1; + + if (msg.reading_type == uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE) { + quality = 100; + } + + rangefinder->update(hrt_absolute_time(), msg.range, quality); } int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel) diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 023a16cf1d..3f1d424bd1 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -38,19 +38,43 @@ #include "sensor_bridge.hpp" #include +#if defined(CONFIG_UAVCAN_SENSOR_ACCEL) #include "accel.hpp" -#include "airspeed.hpp" -#include "baro.hpp" -#include "battery.hpp" -#include "differential_pressure.hpp" -#include "flow.hpp" -#include "gnss.hpp" #include "gyro.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED) +#include "airspeed.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_BARO) +#include "baro.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_BATTERY) +#include "battery.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE) +#include "differential_pressure.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_FLOW) +#include "flow.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_GNSS) +#include "gnss.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_HYGROMETER) #include "hygrometer.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_ICE_STATUS) #include "ice_status.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_MAG) #include "mag.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_RANGEFINDER) #include "rangefinder.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_SAFETY_BUTTON) #include "safety_button.hpp" +#endif /* * IUavcanSensorBridge @@ -58,6 +82,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { // airspeed +#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED) int32_t uavcan_sub_aspd = 1; param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd); @@ -65,6 +90,9 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, ListSIDFC = ((n_stdid << FDCAN_SIDFC_LSS_Pos) | ram_offset << FDCAN_SIDFC_FLSSA_Pos); + memset((void *)message_ram_.StdIdFilterSA, 0, WORD_LENGTH * n_stdid); // make sure filters are disabled ram_offset += n_stdid; - // Extended ID Filters: Allow space for 128 filters (128 words) - const uint8_t n_extid = 128; + // Extended ID Filters: Allow space for 64 filters (128 words) + const uint8_t n_extid = 64; message_ram_.ExtIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH; can_->XIDFC = ((n_extid << FDCAN_XIDFC_LSE_Pos) | ram_offset << FDCAN_XIDFC_FLESA_Pos); - ram_offset += n_extid; + memset((void *)message_ram_.ExtIdFilterSA, 0, (2 * WORD_LENGTH) * n_extid); // make sure filters are disabled + ram_offset += 2 * n_extid; // Set size of each element in the Rx/Tx buffers and FIFOs can_->RXESC = 0; // 8 byte space for every element (Rx buffer, FIFO1, FIFO0) diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index c054304d76..277b2526dd 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -80,14 +80,26 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), ModuleParams(nullptr), _node(can_driver, system_clock, _pool_allocator), +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) _arming_status_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) _beep_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _esc_controller(_node), _servo_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) _hardpoint_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) _safety_state_controller(_node), - _log_message_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) _rgbled_controller(_node), +#endif + _log_message_controller(_node), _time_sync_master(_node), _time_sync_slave(_node), _node_status_monitor(_node), @@ -103,8 +115,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys std::abort(); } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ); _mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ); +#endif } UavcanNode::~UavcanNode() @@ -395,8 +409,10 @@ UavcanNode::get_param(int remote_node_id, const char *name) void UavcanNode::update_params() { +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.updateParams(); _mixing_interface_servo.updateParams(); +#endif } int @@ -429,8 +445,11 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) } _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _instance->_mixing_interface_esc.ScheduleNow(); _instance->_mixing_interface_servo.ScheduleNow(); +#endif return OK; } @@ -482,6 +501,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) int ret; // UAVCAN_PUB_ARM +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) int32_t uavcan_pub_arm = 0; param_get(param_find("UAVCAN_PUB_ARM"), &uavcan_pub_arm); @@ -493,43 +513,60 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } } +#endif + +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) ret = _beep_controller.init(); if (ret < 0) { return ret; } +#endif + // Actuators +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) ret = _esc_controller.init(); if (ret < 0) { return ret; } +#endif + +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) ret = _hardpoint_controller.init(); if (ret < 0) { return ret; } +#endif + +#if defined(CONFIG_UAVCAN_SAFETY_CONTROLLER) ret = _safety_state_controller.init(); if (ret < 0) { return ret; } +#endif + ret = _log_message_controller.init(); if (ret < 0) { return ret; } +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) ret = _rgbled_controller.init(); if (ret < 0) { return ret; } +#endif + /* Start node info retriever to fetch node info from new nodes */ ret = _node_info_retriever.start(); @@ -552,6 +589,8 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) PX4_DEBUG("sensor bridge '%s' init ok", br->get_name()); } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) + // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE || UavcanEscController::max_output_value() > (int)UINT16_MAX) { @@ -560,6 +599,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } _mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); +#endif /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset)); @@ -921,15 +961,20 @@ UavcanNode::Run() pthread_mutex_unlock(&_node_mutex); if (_task_should_exit.load()) { + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.mixingOutput().unregister(); _mixing_interface_esc.ScheduleClear(); + _mixing_interface_servo.mixingOutput().unregister(); _mixing_interface_servo.ScheduleClear(); +#endif ScheduleClear(); _instance = nullptr; } } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { @@ -974,6 +1019,7 @@ void UavcanMixingInterfaceServo::Run() _mixing_output.updateSubscriptions(false); pthread_mutex_unlock(&_node_mutex); } +#endif void UavcanNode::print_info() @@ -1016,10 +1062,13 @@ UavcanNode::print_info() printf("\n"); +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) printf("ESC outputs:\n"); _mixing_interface_esc.mixingOutput().printStatus(); + printf("Servo outputs:\n"); _mixing_interface_servo.mixingOutput().printStatus(); +#endif printf("\n"); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 7fcc86e968..54119259ef 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -46,15 +46,36 @@ #include #include +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) #include "actuators/esc.hpp" -#include "actuators/hardpoint.hpp" #include "actuators/servo.hpp" +#endif + +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) +#include "actuators/hardpoint.hpp" +#endif + + #include "allocator.hpp" + +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) #include "arming_status.hpp" +#endif + +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) #include "beep.hpp" +#endif + #include "logmessage.hpp" + +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) #include "rgbled.hpp" +#endif + +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) #include "safety_state.hpp" +#endif + #include "sensors/sensor_bridge.hpp" #include "uavcan_driver.hpp" #include "uavcan_servers.hpp" @@ -91,6 +112,8 @@ class UavcanNode; * a fixed rate or upon bus updates). * All work items are expected to run on the same work queue. */ + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) class UavcanMixingInterfaceESC : public OutputModuleInterface { public: @@ -143,6 +166,7 @@ private: UavcanServoController &_servo_controller; MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; }; +#endif /** * A UAVCAN node. @@ -225,16 +249,30 @@ private: uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) UavcanArmingStatus _arming_status_controller; +#endif +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) UavcanBeepController _beep_controller; +#endif +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) UavcanEscController _esc_controller; - UavcanServoController _servo_controller; UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller}; + + UavcanServoController _servo_controller; UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller}; +#endif +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) UavcanHardpointController _hardpoint_controller; +#endif +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) UavcanSafetyState _safety_state_controller; - UavcanLogMessage _log_message_controller; +#endif +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) UavcanRGBController _rgbled_controller; +#endif + + UavcanLogMessage _log_message_controller; uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave; diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 810a4fd0ca..2940906c25 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -158,6 +158,7 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr _time_sync_slave(_node), _fw_update_listner(_node), _param_server(_node), + _dyn_node_id_client(_node), _reset_timer(_node) { int res = pthread_mutex_init(&_node_mutex, nullptr); @@ -168,6 +169,10 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr if (res < 0) { std::abort(); } + + // Ensure this param is marked as used + int32_t bitrate_temp = 0; + (void)param_get(param_find("CANNODE_BITRATE"), &bitrate_temp); } UavcanNode::~UavcanNode() @@ -451,6 +456,31 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline); + const int can_init_res = _can->init((uint32_t)_bitrate); + + if (can_init_res < 0) { + PX4_ERR("CAN driver init failed %i", can_init_res); + } + + int rv = _node.start(); + + if (rv < 0) { + PX4_ERR("Failed to start the node"); + } + + // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation + + if (_node.getNodeID() == 0) { + + int client_start_res = _dyn_node_id_client.start( + _node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE + _node.getNodeID()); + + if (client_start_res < 0) { + PX4_ERR("Failed to start the dynamic node ID client"); + } + } + return 1; } @@ -477,67 +507,36 @@ void UavcanNode::Run() if (!_initialized) { + /* + * Waiting for the client to obtain a node ID. + * This may take a few seconds. + */ - const int can_init_res = _can->init((uint32_t)_bitrate); + if (_dyn_node_id_client.isAllocationComplete()) { + PX4_INFO("Got node ID %d", _dyn_node_id_client.getAllocatedNodeID().get()); - if (can_init_res < 0) { - PX4_ERR("CAN driver init failed %i", can_init_res); + _node.setNodeID(_dyn_node_id_client.getAllocatedNodeID()); } - int rv = _node.start(); + if (_node.getNodeID() != 0) { + up_time = hrt_absolute_time(); + get_node().setRestartRequestHandler(&restart_request_handler); + _param_server.start(&_param_manager); - if (rv < 0) { - PX4_ERR("Failed to start the node"); - } + // Set up the time synchronization + const int slave_init_res = _time_sync_slave.start(); - // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation - - if (_node.getNodeID() == 0) { - - uavcan::DynamicNodeIDClient client(_node); - - int client_start_res = client.start(_node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE - _node.getNodeID()); - - if (client_start_res < 0) { - PX4_ERR("Failed to start the dynamic node ID client"); + if (slave_init_res < 0) { + PX4_ERR("Failed to start time_sync_slave"); + _task_should_exit.store(true); } - watchdog_pet(); // If allocation takes too long reboot + _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - /* - * Waiting for the client to obtain a node ID. - * This may take a few seconds. - */ + _node.setModeOperational(); - while (!client.isAllocationComplete()) { - const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter - - if (res < 0) { - PX4_ERR("Transient failure: %d", res); - } - } - - _node.setNodeID(client.getAllocatedNodeID()); + _initialized = true; } - - up_time = hrt_absolute_time(); - get_node().setRestartRequestHandler(&restart_request_handler); - _param_server.start(&_param_manager); - - // Set up the time synchronization - const int slave_init_res = _time_sync_slave.start(); - - if (slave_init_res < 0) { - PX4_ERR("Failed to start time_sync_slave"); - _task_should_exit.store(true); - } - - _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - - _node.setModeOperational(); - - _initialized = true; } perf_begin(_cycle_perf); @@ -636,6 +635,19 @@ void UavcanNode::PrintInfo() { pthread_mutex_lock(&_node_mutex); + // Firmware version + printf("Hardware and software status:\n"); + printf("\tNode ID: %d\n", int(_node.getNodeID().get())); + printf("\tHardware version: %d.%d\n", + int(_node.getHardwareVersion().major), + int(_node.getHardwareVersion().minor)); + printf("\tSoftware version: %d.%d.%08x\n", + int(_node.getSoftwareVersion().major), + int(_node.getSoftwareVersion().minor), + int(_node.getSoftwareVersion().vcs_commit)); + + printf("\n"); + // Memory status printf("Pool allocator status:\n"); printf("\tCapacity hard/soft: %u/%u blocks\n", @@ -752,7 +764,6 @@ extern "C" int uavcannode_start(int argc, char *argv[]) } else #endif { - (void)param_get(param_find("CANNODE_NODE_ID"), &node_id); (void)param_get(param_find("CANNODE_BITRATE"), &bitrate); } } @@ -761,7 +772,7 @@ extern "C" int uavcannode_start(int argc, char *argv[]) #if defined(SUPPORT_ALT_CAN_BOOTLOADER) board_booted_by_px4() && #endif - (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast())) { + (node_id < 0 || node_id > uavcan::NodeID::Max)) { PX4_ERR("Invalid Node ID %" PRId32, node_id); return 1; } diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index f7a3a25374..bbe7158ec1 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -181,6 +181,8 @@ private: UavcanNodeParamManager _param_manager; uavcan::ParamServer _param_server; + uavcan::DynamicNodeIDClient _dyn_node_id_client; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 8a9e1ba0ee..5b6a27e44e 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -31,17 +31,6 @@ * ****************************************************************************/ -/** - * UAVCAN Node ID. - * - * Read the specs at http://uavcan.org to learn more about Node ID. - * - * @min 1 - * @max 125 - * @group UAVCAN - */ -PARAM_DEFINE_INT32(CANNODE_NODE_ID, 120); - /** * UAVCAN CAN bus bitrate. * diff --git a/src/drivers/uwb/uwb_sr150/module.yaml b/src/drivers/uwb/uwb_sr150/module.yaml index 721972ba05..7a62d0cc75 100644 --- a/src/drivers/uwb/uwb_sr150/module.yaml +++ b/src/drivers/uwb/uwb_sr150/module.yaml @@ -4,7 +4,6 @@ serial_config: port_config_param: name: UWB_PORT_CFG group: UWB - default: "TEL2" parameters: - group: UWB diff --git a/src/include/containers/BlockingQueue.hpp b/src/include/containers/BlockingQueue.hpp index 3b1ec77a86..00b64d1053 100644 --- a/src/include/containers/BlockingQueue.hpp +++ b/src/include/containers/BlockingQueue.hpp @@ -46,6 +46,8 @@ public: { px4_sem_init(&_sem_head, 0, N); px4_sem_init(&_sem_tail, 0, 0); + px4_sem_setprotocol(&_sem_head, SEM_PRIO_NONE); + px4_sem_setprotocol(&_sem_tail, SEM_PRIO_NONE); } ~BlockingQueue() @@ -56,7 +58,7 @@ public: void push(T newItem) { - px4_sem_wait(&_sem_head); + do {} while (px4_sem_wait(&_sem_head) != 0); _data[_tail] = newItem; _tail = (_tail + 1) % N; @@ -66,7 +68,7 @@ public: T pop() { - px4_sem_wait(&_sem_tail); + do {} while (px4_sem_wait(&_sem_tail) != 0); T ret = _data[_head]; _head = (_head + 1) % N; diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 479b0fa141..1094ff33c1 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2023 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,11 +33,13 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) +add_subdirectory(atmosphere EXCLUDE_FROM_ALL) add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) add_subdirectory(cdev EXCLUDE_FROM_ALL) +add_subdirectory(cdrstream EXCLUDE_FROM_ALL) add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL) add_subdirectory(collision_prevention EXCLUDE_FROM_ALL) add_subdirectory(component_information EXCLUDE_FROM_ALL) @@ -62,6 +64,7 @@ add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) +add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) @@ -71,6 +74,7 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL) add_subdirectory(timesync EXCLUDE_FROM_ALL) add_subdirectory(tinybson EXCLUDE_FROM_ALL) add_subdirectory(tunes EXCLUDE_FROM_ALL) +add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(version EXCLUDE_FROM_ALL) add_subdirectory(weather_vane EXCLUDE_FROM_ALL) add_subdirectory(wind_estimator EXCLUDE_FROM_ALL) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 24f7f01850..4d29be4a00 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -44,6 +44,9 @@ #include #include +#include + +using atmosphere::getDensityFromPressureAndTemp; float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) @@ -53,7 +56,7 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ } // air density in kg/m3 - const float rho_air = get_air_density(pressure_ambient, temperature_celsius); + const float rho_air = getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius); const float dp = fabsf(differential_pressure); float dp_tot = dp; @@ -153,18 +156,6 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ break; } - // if (!PX4_ISFINITE(dp_tube)) { - // dp_tube = 0.0f; - // } - - // if (!PX4_ISFINITE(dp_pitot)) { - // dp_pitot = 0.0f; - // } - - // if (!PX4_ISFINITE(dv)) { - // dv = 0.0f; - // } - // computed airspeed without correction for inflow-speed at tip of pitot-tube const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); @@ -192,7 +183,7 @@ float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float te temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius } - return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius)); } @@ -203,7 +194,7 @@ float calc_CAS_from_IAS(float speed_indicated, float scale) float calc_TAS(float total_pressure, float static_pressure, float temperature_celsius) { - float density = get_air_density(static_pressure, temperature_celsius); + float density = getDensityFromPressureAndTemp(static_pressure, temperature_celsius); if (density < 0.0001f || !PX4_ISFINITE(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; @@ -219,15 +210,6 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce } } -float get_air_density(float static_pressure, float temperature_celsius) -{ - if (!PX4_ISFINITE(temperature_celsius)) { - temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius - } - - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} - float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index dba000a4e5..6b5161c5dd 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -127,7 +127,6 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe * @param static_pressure ambient pressure in millibar * @param temperature_celcius air / ambient temperature in Celsius */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); /** * @brief Calculates calibrated airspeed from true airspeed and air density diff --git a/src/lib/atmosphere/CMakeLists.txt b/src/lib/atmosphere/CMakeLists.txt new file mode 100644 index 0000000000..eb25c8df4b --- /dev/null +++ b/src/lib/atmosphere/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2023 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(atmosphere + atmosphere.cpp + ) +px4_add_unit_gtest(SRC test_atmosphere.cpp LINKLIBS atmosphere) diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp new file mode 100644 index 0000000000..66b98626f7 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 atmosphere.cpp + * + */ + +#include +#include "atmosphere.h" +namespace atmosphere +{ + +static constexpr float kTempRefKelvin = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin +static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter +static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa + +float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius) +{ + return (pressure_pa / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS))); +} +float getPressureFromAltitude(const float altitude_m) +{ + + return kPressRefSeaLevelPa * powf((altitude_m * kTempGradient + kTempRefKelvin) / kTempRefKelvin, + -CONSTANTS_ONE_G / (kTempGradient * CONSTANTS_AIR_GAS_CONST)); +} +float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa) +{ + // calculate altitude using the hypsometric equation + + const float pressure_ratio = pressure_pa / pressure_sealevel_pa; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + return (((powf(pressure_ratio, (-(kTempGradient * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * kTempRefKelvin) - + kTempRefKelvin) / kTempGradient; + +} +float getStandardTemperatureAtAltitude(float altitude_m) +{ + return 15.0f + kTempGradient * altitude_m; +} +} \ No newline at end of file diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h new file mode 100644 index 0000000000..849a430347 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 atmosphere.h + * + */ + +#ifndef PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ +#define PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ + +namespace atmosphere +{ + +// NOTE: We are currently only modelling the first layer of the US Standard Atmosphere 1976. +// This means that the functions are only valid up to an altitude of 11km. + +/** +* Calculate air density given air pressure and temperature. +* +* @param pressure_pa ambient pressure in Pa +* @param temperature_celsius ambient temperature in degrees Celsius +*/ +float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius); + +/** +* Calculate standard airpressure given altitude. +* +* @param altitude_m altitude above MSL in meters in the standard atmosphere +*/ +float getPressureFromAltitude(const float altitude_m); + +/** +* Calculate altitude from air pressure and temperature. +* +* @param pressure_pa ambient pressure in Pa +* @param pressure_sealevel_pa sea level pressure in Pa +*/ +float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa); + +/** +* Get standard temperature at altitude. +* +* @param altitude_m Altitude msl in meters +* @return Standard temperature in degrees Celsius +*/ +float getStandardTemperatureAtAltitude(float altitude_m); +} + +#endif //PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ diff --git a/src/lib/atmosphere/test_atmosphere.cpp b/src/lib/atmosphere/test_atmosphere.cpp new file mode 100644 index 0000000000..0a79c89ca1 --- /dev/null +++ b/src/lib/atmosphere/test_atmosphere.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +/** + * To run this test only use: make tests TESTFILTER=atmosphere + */ + +#include +#include +using namespace atmosphere; + +TEST(TestAtmosphere, pressureFromAltitude) +{ + // GIVEN pressure at seal level in standard atmosphere and sea level altitude + const float pressureAtSeaLevel = 101325.f; // Pa + float altitude = 0.0f; + + // WHEN we calculate pressure based on altitude + float pressure = getPressureFromAltitude(altitude); + + // THEN expect seal level pressure for standard atmosphere + EXPECT_FLOAT_EQ(pressure, pressureAtSeaLevel); + + // WHEN we are at 3000m altitude + altitude = 3000.0f; + pressure = getPressureFromAltitude(altitude); + + // THEN expect standard atmosphere pressure at 3000m (error of 10Pa corresponds to 1m altitude error in standard atmosphere when altitude is between 1000 and 2000m) + EXPECT_NEAR(pressure, 70120.f, 10.0f); +} + +TEST(TestAtmosphere, altitudeFromPressure) +{ + // GIVEN pressure at seal level in standard atmosphere + const float pressureAtSealevel = 101325.f; + float pressure = pressureAtSealevel; + + // WHEN we calculate altitude from pressure + float altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect resulting altitude to be near sea level + EXPECT_FLOAT_EQ(altitude, 0.0f); + + // GIVEN pressure is standard atmosphere pressure at 3000m + pressure = 70109.f; + + // WHEN we calculate altitude from pressure + altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect altitude to be near 3000m + EXPECT_NEAR(altitude, 3000.0f, 0.5f); +} + +TEST(TestAtmosphere, DensityFromPressure) +{ +// GIVEN standard atmosphere at sea level + const float pressureAtSealevel = 101325.f; + const float tempSeaLevel = 15.f; + + // WHEN we calculate density from pressure and temperature + float density = getDensityFromPressureAndTemp(pressureAtSealevel, tempSeaLevel); + + // THEN expect density at sea level in standard atmosphere + EXPECT_NEAR(density, 1.225f, 0.001f); + + // GIVEN standard atmosphere at 3000m + const float pressure = 70109.f; + const float tempAt3000m = -4.5f; + + // WHEN we calculate density from pressure and temperature + density = getDensityFromPressureAndTemp(pressure, tempAt3000m); + + // THEN expect density at 3000m in standard atmosphere + EXPECT_NEAR(density, 0.9091f, 0.001f); +} + +TEST(TestAtmosphere, StandardTemperature) +{ + // GIVEN standard atmosphere at sea level + float altitude = 0.f; + + // WHEN we calculate standard temperature from altitude + float temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at sea level + EXPECT_NEAR(temperature, 15.f, 0.001f); + + // GIVEN standard atmosphere at 3000m + altitude = 3000.f; + + // WHEN we calculate standard temperature from altitude + temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at 3000m + EXPECT_NEAR(temperature, -4.5f, 0.001f); +} \ No newline at end of file diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml index 4cbf1da675..c7750801b6 100644 --- a/src/lib/battery/module.yaml +++ b/src/lib/battery/module.yaml @@ -86,6 +86,7 @@ parameters: type: enum values: + 0: Unknown 1: 1S Battery 2: 2S Battery 3: 3S Battery diff --git a/src/lib/cdrstream/CMakeLists.txt b/src/lib/cdrstream/CMakeLists.txt new file mode 100644 index 0000000000..cfdb6ab090 --- /dev/null +++ b/src/lib/cdrstream/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2023 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(CONFIG_LIB_CDRSTREAM) + set(BUILD_SHARED_LIBS NO) + + # CycloneDDS CDR serializer + include(${CMAKE_CURRENT_LIST_DIR}/cyclonedds/src/core/cdr/CMakeLists.txt) + target_compile_options(cdr PUBLIC + -Wno-cast-align + -Wno-double-promotion + $<$:-Wno-implicit-function-declaration -Wno-nested-externs> + -DNDEBUG) + px4_add_git_submodule(TARGET git_cyclonedds PATH "cyclonedds") + px4_add_git_submodule(TARGET git_rosidl PATH "rosidl") + add_dependencies(cdr git_cyclonedds git_rosidl uorb_headers parameters px4_platform) + target_sources(cdr PRIVATE dds_serializer.c) + target_include_directories(cdr PUBLIC ${CMAKE_CURRENT_LIST_DIR}) +endif() diff --git a/src/lib/cdrstream/Kconfig b/src/lib/cdrstream/Kconfig new file mode 100644 index 0000000000..311443075a --- /dev/null +++ b/src/lib/cdrstream/Kconfig @@ -0,0 +1,5 @@ +config LIB_CDRSTREAM + bool + default n + ---help--- + Enable CDR stream serializer library diff --git a/src/lib/cdrstream/cyclonedds b/src/lib/cdrstream/cyclonedds new file mode 160000 index 0000000000..314887ca40 --- /dev/null +++ b/src/lib/cdrstream/cyclonedds @@ -0,0 +1 @@ +Subproject commit 314887ca403c2fb0a0316add22672102936ed36c diff --git a/src/lib/cdrstream/dds_serializer.c b/src/lib/cdrstream/dds_serializer.c new file mode 100644 index 0000000000..b17cc105ea --- /dev/null +++ b/src/lib/cdrstream/dds_serializer.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "dds_serializer.h" + + +void *dds_malloc(size_t size) +{ + return NULL; +} +void *dds_realloc(void *ptr, size_t new_size) +{ + printf("Error CDR buffer is too small\n"); + return NULL; +} +void dds_free(void *pt) +{ + +} +const struct dds_cdrstream_allocator dds_allocator = { dds_malloc, dds_realloc, dds_free }; + +// CDR Xtypes header {0x00, 0x01} indicates it's Little Endian (CDR_LE representation) +const uint8_t ros2_header[4] = {0x00, 0x01, 0x00, 0x00}; diff --git a/src/lib/cdrstream/dds_serializer.h b/src/lib/cdrstream/dds_serializer.h new file mode 100644 index 0000000000..ac89dfb0b0 --- /dev/null +++ b/src/lib/cdrstream/dds_serializer.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#ifndef DDS_CDRSTREAM_SERDER_H +#define DDS_CDRSTREAM_SERDER_H + +#include +#include + +extern const struct dds_cdrstream_allocator dds_allocator; +extern const uint8_t ros2_header[4]; + +#endif //DDS_CDRSTREAM_SERDER_H diff --git a/src/lib/cdrstream/msg2idl.py b/src/lib/cdrstream/msg2idl.py new file mode 100644 index 0000000000..74a464ab3d --- /dev/null +++ b/src/lib/cdrstream/msg2idl.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +############################################################################ +# +# Copyright (C) 2023 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. +# +############################################################################ + +import argparse +import pathlib +import sys + +from rosidl_adapter.msg import convert_msg_to_idl + +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description=f'Convert px4 .msg files to .idl') + parser.add_argument( + 'interface_files', nargs='+', + help='The interface files to convert') + args = parser.parse_args(sys.argv[1:]) + + for interface_file in args.interface_files: + interface_file = pathlib.Path(interface_file) + package_dir = interface_file.parent.absolute() + + convert_msg_to_idl( + package_dir, 'px4', + interface_file.absolute().relative_to(package_dir), + interface_file.parent) diff --git a/src/lib/cdrstream/rosidl b/src/lib/cdrstream/rosidl new file mode 160000 index 0000000000..7790c70717 --- /dev/null +++ b/src/lib/cdrstream/rosidl @@ -0,0 +1 @@ +Subproject commit 7790c70717e09c003711f6f65015666c223fc283 diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 80a9910d8b..b33e4a8383 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -91,8 +91,10 @@ enum Rotation : uint8_t { ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, ROTATION_PITCH_315 = 39, ROTATION_ROLL_90_PITCH_315 = 40, + ROTATION_MAX, - ROTATION_MAX + // Rotation Enum reserved for custom rotation using Euler Angles + ROTATION_CUSTOM = 100 }; struct rot_lookup_t { diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp index 1fa4820fef..a6b37defa9 100644 --- a/src/lib/dataman_client/DatamanClient.cpp +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -556,6 +556,32 @@ bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uin return success; } +bool DatamanCache::writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = _client.writeSync(item, index, buffer, length, timeout); + + if (success && _items) { + for (uint32_t i = 0; i < _num_items; ++i) { + if ((_items[i].response.item == item) && + (_items[i].response.index == index) && + ((_items[i].cache_state == State::ResponseReceived) || + (_items[i].cache_state == State::RequestPrepared))) { + + memcpy(_items[i].response.data, buffer, length); + _items[i].cache_state = State::ResponseReceived; + break; + } + } + } + + return success; +} + void DatamanCache::update() { if (_item_counter > 0) { diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp index 3fa6d82065..636b41d142 100644 --- a/src/lib/dataman_client/DatamanClient.hpp +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -235,6 +235,19 @@ public: */ bool loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 0); + /** + * @brief Write data back and update it in the cache if stored. + * + * @param[in] item The data item type to write. + * @param[in] index The index of the data item. + * @param[in] buffer The buffer that contains the data to write. + * @param[in] length The length of the data to write. + * @param[in] timeout The maximum time in microseconds to wait for the response. + * + * @return True if the write operation succeeded, false otherwise. + */ + bool writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + /** * @brief Updates the dataman cache by checking for responses from the DatamanClient and processing them. * diff --git a/src/lib/events/CMakeLists.txt b/src/lib/events/CMakeLists.txt index b3540ffb3d..e153e3245f 100644 --- a/src/lib/events/CMakeLists.txt +++ b/src/lib/events/CMakeLists.txt @@ -110,3 +110,12 @@ add_custom_command(OUTPUT ${generated_events_header} ) add_custom_target(events_header DEPENDS ${generated_events_header}) add_dependencies(prebuild_targets events_header) + +# Build the interface(s) +if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) + list(APPEND EXTRA_SRCS events_ioctl.cpp) + add_library(usr_events usr_events.cpp) + add_dependencies(usr_events prebuild_targets) +endif() + +px4_add_library(events events.cpp ${EXTRA_SRCS}) diff --git a/platforms/common/events.cpp b/src/lib/events/events.cpp similarity index 100% rename from platforms/common/events.cpp rename to src/lib/events/events.cpp diff --git a/src/lib/events/events.h b/src/lib/events/events.h new file mode 100644 index 0000000000..929dbc4982 --- /dev/null +++ b/src/lib/events/events.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. 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 + +void events_ioctl_init(void); diff --git a/src/lib/events/events_ioctl.cpp b/src/lib/events/events_ioctl.cpp new file mode 100644 index 0000000000..a3c8260ddd --- /dev/null +++ b/src/lib/events/events_ioctl.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. 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 events_ioctl.cpp + * + * Interface to send events from user space. + */ + +#include + +#include + +#include "events_ioctl.h" + +static int events_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case EVENTSIOCSEND: { + eventiocsend_t *data = (eventiocsend_t *)arg; + events::send(data->event); + } + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + +void events_ioctl_init(void) +{ + px4_register_boardct_ioctl(_EVENTSIOCBASE, events_ioctl); +} diff --git a/src/lib/events/events_ioctl.h b/src/lib/events/events_ioctl.h new file mode 100644 index 0000000000..7b97c66aa4 --- /dev/null +++ b/src/lib/events/events_ioctl.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. 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 events_ioctl.h + * + * User space - kernel space interface for dispatching events. + */ + +#pragma once + +#include +#include +#include + +#define _EVENTSIOC(_n) (_PX4_IOC(_EVENTSIOCBASE, _n)) + +#define EVENTSIOCSEND _EVENTSIOC(1) +typedef struct eventiocsend { + events::EventType &event; +} eventiocsend_t; diff --git a/src/lib/events/usr_events.cpp b/src/lib/events/usr_events.cpp new file mode 100644 index 0000000000..a2f076d065 --- /dev/null +++ b/src/lib/events/usr_events.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. 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 usr_events.cpp + * + * User space interface for dispatching events. + */ + +#include + +#include "events_ioctl.h" + +namespace events +{ + +void send(EventType &event) +{ + eventiocsend_t data = {event}; + boardctl(EVENTSIOCSEND, reinterpret_cast(&data)); +} + +} /* namespace events */ diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 7d72710e17..ce3b374992 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -126,18 +126,43 @@ public: return res; } - Type trace() const + template + Type trace(size_t first) const { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + Type res = 0; const SquareMatrix &self = *this; - for (size_t i = 0; i < M; i++) { + for (size_t i = first; i < (first + Width); i++) { res += self(i, i); } return res; } + Type trace() const + { + const SquareMatrix &self = *this; + return self.trace(0); + } + + // keep the sub covariance matrix and zero all covariance elements related + // to the rest of the matrix + template + void uncorrelateCovarianceBlock(size_t first) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + SquareMatrix cov = self.slice(first, first); + self.slice(0, first) = 0.f; + self.slice(first, 0) = 0.f; + self.slice(first, first) = cov; + } + // zero all offdiagonal elements and keep corresponding diagonal elements template void uncorrelateCovariance(size_t first) diff --git a/src/lib/matrix/test/MatrixSquareTest.cpp b/src/lib/matrix/test/MatrixSquareTest.cpp index 58ceab940f..eafca26d74 100644 --- a/src/lib/matrix/test/MatrixSquareTest.cpp +++ b/src/lib/matrix/test/MatrixSquareTest.cpp @@ -47,6 +47,7 @@ TEST(MatrixSquareTest, Square) EXPECT_EQ(A.diag(), diag_check); EXPECT_FLOAT_EQ(A.trace(), 16); + EXPECT_FLOAT_EQ(A.trace<2>(1), 15); float data_check[9] = { 1.01158503f, 0.02190432f, 0.03238144f, @@ -117,6 +118,16 @@ TEST(MatrixSquareTest, Square) SquareMatrix E_check(data_E_check); EXPECT_EQ(E, E_check); + SquareMatrix A_block(data_4x4); + A_block.uncorrelateCovarianceBlock<2>(1); + float data_A_block_check[16] = {1, 0, 0, 4, + 0, 6, 7, 0, + 0, 10, 11, 0, + 13, 0, 0, 16 + }; + SquareMatrix A_block_check(data_A_block_check); + EXPECT_EQ(A_block, A_block_check); + // test symmetric functions SquareMatrix F(data_4x4); F.makeBlockSymmetric<2>(1); diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index 6873a3919f..fa42c8b63d 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -35,7 +35,7 @@ #include "FunctionProviderBase.hpp" -#include +#include /** * Functions: Motor1 ... MotorMax diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index a4308fb150..196e0b2699 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -59,7 +59,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g const float wind_speed = wind_vel.norm(); const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; - const float signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle); + signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle); // on-track wind triangle projections const float wind_cross_upt = wind_vel.cross(unit_path_tangent); @@ -68,7 +68,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g // calculate the bearing feasibility on the track at the current closest point feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); - const float track_error = fabsf(signed_track_error); + const float track_error = fabsf(signed_track_error_); // update control parameters considering upper and lower stability bounds (if enabled) // must be called before trackErrorBound() as it updates time_const_ @@ -86,7 +86,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g track_proximity_ = trackProximity(look_ahead_ang); - bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error); + bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); // wind triangle projections const float wind_cross_bearing = wind_vel.cross(bearing_vec_); @@ -112,7 +112,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g // lateral acceleration needed to stay on curved track (assuming no heading error) lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, - wind_cross_upt, airspeed, wind_speed, signed_track_error, path_curvature); + wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature); // total lateral acceleration to drive aircaft towards track as well as account // for path curvature. The full effect of the feed-forward acceleration is smoothly diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 5a5603d4cf..b438de23a2 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -42,194 +42,7 @@ bool param_modify_on_import(bson_node_t node) { - // 2022-04-11: translate VT_PTCH_MIN to VT_PITCH_MIN - { - if (strcmp("VT_PTCH_MIN", node->name) == 0) { - strcpy(node->name, "VT_PITCH_MIN"); - PX4_INFO("copying %s -> %s", "VT_PTCH_MIN", "VT_PITCH_MIN"); - return true; - } - } - // 2022-04-11: translate VT_LND_PTCH_MIN to VT_LND_PITCH_MIN - { - if (strcmp("VT_LND_PTCH_MIN", node->name) == 0) { - strcpy(node->name, "VT_LND_PITCH_MIN"); - PX4_INFO("copying %s -> %s", "VT_LND_PTCH_MIN", "VT_LND_PITCH_MIN"); - return true; - } - } - - - // 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R - { - if (strcmp("NAV_GPSF_LT", node->name) == 0) { - strcpy(node->name, "FW_GPSF_LT"); - node->i32 = static_cast(node->d); - node->type = BSON_INT32; - PX4_INFO("copying %s -> %s", "NAV_GPSF_LT", "FW_GPSF_LT"); - return true; - } - - if (strcmp("NAV_GPSF_R", node->name) == 0) { - strcpy(node->name, "FW_GPSF_R"); - PX4_INFO("copying %s -> %s", "NAV_GPSF_R", "FW_GPSF_R"); - return true; - } - } - - // 2022-03-15: translate notch filter IMU_GYRO_NF_FREQ to IMU_GYRO_NF0_FRQ and IMU_GYRO_NF_BW -> IMU_GYRO_NF0_BW - { - if (strcmp("IMU_GYRO_NF_FREQ", node->name) == 0) { - strcpy(node->name, "IMU_GYRO_NF0_FRQ"); - PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_FREQ", "IMU_GYRO_NF0_FRQ"); - return true; - } - - if (strcmp("IMU_GYRO_NF_BW", node->name) == 0) { - strcpy(node->name, "IMU_GYRO_NF0_BW"); - PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_BW", "IMU_GYRO_NF0_BW"); - return true; - } - } - - // 2022-04-25 (v1.13 alpha): translate MS4525->MS4525DO and MS5525->MS5525DSO - { - if (strcmp("SENS_EN_MS4525", node->name) == 0) { - strcpy(node->name, "SENS_EN_MS4525DO"); - PX4_INFO("copying %s -> %s", "SENS_EN_MS4525", "SENS_EN_MS4525DO"); - return true; - } - - if (strcmp("SENS_EN_MS5525", node->name) == 0) { - strcpy(node->name, "SENS_EN_MS5525DS"); - PX4_INFO("copying %s -> %s", "SENS_EN_MS5525", "SENS_EN_MS5525DS"); - return true; - } - } - - // 2022-06-09: migrate EKF2_WIND_NOISE->EKF2_WIND_NSD - { - if (strcmp("EKF2_WIND_NOISE", node->name) == 0) { - node->d /= 10.0; // at 100Hz (EKF2 rate), NSD is sqrt(100) times smaller than std_dev - strcpy(node->name, "EKF2_WIND_NSD"); - PX4_INFO("param migrating EKF2_WIND_NOISE (removed) -> EKF2_WIND_NSD: value=%.3f", node->d); - return true; - } - } - - // 2022-06-09: translate ASPD_SC_P_NOISE->ASPD_SCALE_NSD and ASPD_W_P_NOISE->ASPD_WIND_NSD - { - if (strcmp("ASPD_SC_P_NOISE", node->name) == 0) { - strcpy(node->name, "ASPD_SCALE_NSD"); - PX4_INFO("copying %s -> %s", "ASPD_SC_P_NOISE", "ASPD_SCALE_NSD"); - return true; - } - - if (strcmp("ASPD_W_P_NOISE", node->name) == 0) { - strcpy(node->name, "ASPD_WIND_NSD"); - PX4_INFO("copying %s -> %s", "ASPD_W_P_NOISE", "ASPD_WIND_NSD"); - return true; - } - } - - // 2022-07-07: translate FW_THR_CRUISE->FW_THR_TRIM - { - if (strcmp("FW_THR_CRUISE", node->name) == 0) { - strcpy(node->name, "FW_THR_TRIM"); - PX4_INFO("copying %s -> %s", "FW_THR_CRUISE", "FW_THR_TRIM"); - return true; - } - } - - // 2022-08-04: migrate EKF2_RNG_AID->EKF2_RNG_CTRL and EKF2_HGT_MODE->EKF2_HGT_REF - { - if (strcmp("EKF2_RNG_AID", node->name) == 0) { - strcpy(node->name, "EKF2_RNG_CTRL"); - PX4_INFO("param migrating EKF2_RNG_AID (removed) -> EKF2_RNG_CTRL: value=%" PRId32, node->i32); - return true; - } - - if (strcmp("EKF2_HGT_MODE", node->name) == 0) { - strcpy(node->name, "EKF2_HGT_REF"); - - // If was in range height mode, set range aiding to "always" - if (node->i32 == 2) { - int32_t rng_mode = 2; - param_set_no_notification(param_find("EKF2_RNG_CTRL"), &rng_mode); - } - - PX4_INFO("param migrating EKF2_HGT_MODE (removed) -> EKF2_HGT_REF: value=%" PRId32, node->i32); - return true; - } - } - - // 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y - { - if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) { - strcpy(node->name, "VT_FW_DIFTHR_S_Y"); - PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y"); - return true; - } - } - - // 2022-11-11: translate VT_F_TRANS_THR/VT_PSHER_RMP_DT -> VT_PSHER_SLEW - { - if (strcmp("VT_PSHER_RMP_DT", node->name) == 0) { - strcpy(node->name, "VT_PSHER_SLEW"); - double _param_vt_f_trans_thr = param_find("VT_F_TRANS_THR"); - node->d = _param_vt_f_trans_thr / node->d; - PX4_INFO("copying %s -> %s", "VT_PSHER_RMP_DT", "VT_PSHER_SLEW"); - } - } - - // 2022-11-09: translate several fixed-wing launch parameters - { - if (strcmp("LAUN_ALL_ON", node->name) == 0) { - strcpy(node->name, "FW_LAUN_DETCN_ON"); - PX4_INFO("copying %s -> %s", "LAUN_ALL_ON", "FW_LAUN_DETCN_ON"); - return true; - } - - if (strcmp("LAUN_CAT_A", node->name) == 0) { - strcpy(node->name, "FW_LAUN_AC_THLD"); - PX4_INFO("copying %s -> %s", "LAUN_CAT_A", "FW_LAUN_AC_THLD"); - return true; - } - - if (strcmp("LAUN_CAT_T", node->name) == 0) { - strcpy(node->name, "FW_LAUN_AC_T"); - PX4_INFO("copying %s -> %s", "LAUN_CAT_T", "FW_LAUN_AC_T"); - return true; - } - - if (strcmp("LAUN_CAT_MDEL", node->name) == 0) { - strcpy(node->name, "FW_LAUN_MOT_DEL"); - PX4_INFO("copying %s -> %s", "LAUN_CAT_MDEL", "FW_LAUN_MOT_DEL"); - return true; - } - } return false; - - //2023-02-08: translate L1 parameters after removing l1 control - { - if (strcmp("RWTO_L1_PERIOD", node->name) == 0) { - strcpy(node->name, "RWTO_NPFG_PERIOD"); - PX4_INFO("copying %s -> %s", "RWTO_L1_PERIOD", "RWTO_NPFG_PERIOD"); - return true; - } - - if (strcmp("FW_L1_R_SLEW_MAX", node->name) == 0) { - strcpy(node->name, "FW_PN_R_SLEW_MAX"); - PX4_INFO("copying %s -> %s", "FW_L1_R_SLEW_MAX", "FW_PN_R_SLEW_MAX"); - return true; - } - - if (strcmp("FW_L1_PERIOD", node->name) == 0) { - strcpy(node->name, "NPFG_PERIOD"); - PX4_INFO("copying %s -> %s", "FW_L1_PERIOD", "NPFG_PERIOD"); - return true; - } - } } diff --git a/src/lib/parameters/px4params/jsonout.py b/src/lib/parameters/px4params/jsonout.py index a3ed2e9f09..45f6cf291d 100644 --- a/src/lib/parameters/px4params/jsonout.py +++ b/src/lib/parameters/px4params/jsonout.py @@ -140,7 +140,7 @@ class JsonOutput(): #Json string output. - self.output = json.dumps(all_json, indent=2, sort_keys=True) + self.output = json.dumps(all_json, sort_keys=True) def Save(self, filename): diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 07c45d33c0..5894bf7549 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -359,7 +359,6 @@ bool RCTest::sbus2Test() bool sbus_frame_drop; uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); - int rate_limiter = 0; unsigned last_drop = 0; while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { @@ -390,7 +389,6 @@ bool RCTest::sbus2Test() last_drop = sbus_frame_drops + sbus_frame_resets; } - rate_limiter++; } ut_test(ret == EOF); diff --git a/src/lib/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt new file mode 100644 index 0000000000..78a26de166 --- /dev/null +++ b/src/lib/ringbuffer/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023 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(ringbuffer + Ringbuffer.cpp +) + +target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer) diff --git a/src/lib/ringbuffer/Ringbuffer.cpp b/src/lib/ringbuffer/Ringbuffer.cpp new file mode 100644 index 0000000000..64fc33d836 --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "Ringbuffer.hpp" + +#include +#include +#include + + +Ringbuffer::~Ringbuffer() +{ + deallocate(); +} + +bool Ringbuffer::allocate(size_t buffer_size) +{ + assert(_ringbuffer == nullptr); + + _size = buffer_size; + _ringbuffer = new uint8_t[_size]; + return _ringbuffer != nullptr; +} + +void Ringbuffer::deallocate() +{ + delete[] _ringbuffer; + _ringbuffer = nullptr; + _size = 0; +} + +size_t Ringbuffer::space_available() const +{ + if (_start > _end) { + return _start - _end - 1; + + } else { + return _start - _end - 1 + _size; + } +} + +size_t Ringbuffer::space_used() const +{ + if (_start <= _end) { + return _end - _start; + + } else { + // Potential wrap around. + return _end - _start + _size; + } +} + + +bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len) +{ + if (buf_len == 0 || buf == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + if (_start > _end) { + // Add after end up to start, no wrap around. + + // Leave one byte free so that start don't end up the same + // which signals empty. + const size_t available = _start - _end - 1; + + if (available < buf_len) { + return false; + } + + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + + } else { + // Add after end, maybe wrap around. + const size_t available = _start - _end - 1 + _size; + + if (available < buf_len) { + return false; + } + + const size_t remaining_packet_len = _size - _end; + + if (buf_len > remaining_packet_len) { + memcpy(&_ringbuffer[_end], buf, remaining_packet_len); + _end = 0; + + memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len); + _end += buf_len - remaining_packet_len; + + } else { + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + } + } + + return true; +} + +size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + if (_start == _end) { + // Empty + return 0; + } + + if (_start < _end) { + + // No wrap around. + size_t to_copy_len = math::min(_end - _start, buf_max_len); + + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + + return to_copy_len; + + } else { + // Potential wrap around. + size_t to_copy_len = _end - _start + _size; + + if (to_copy_len > buf_max_len) { + to_copy_len = buf_max_len; + } + + const size_t remaining_buf_len = _size - _start; + + if (to_copy_len > remaining_buf_len) { + + memcpy(buf, &_ringbuffer[_start], remaining_buf_len); + _start = 0; + memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len); + _start += to_copy_len - remaining_buf_len; + + } else { + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + } + + return to_copy_len; + } +} diff --git a/src/lib/ringbuffer/Ringbuffer.hpp b/src/lib/ringbuffer/Ringbuffer.hpp new file mode 100644 index 0000000000..541f322db9 --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + + +// FIFO ringbuffer implementation. +// +// The ringbuffer can store up 1 byte less than allocated as +// start and end marker need to be one byte apart when the buffer +// is full, otherwise it would suddenly be empty. +// +// The buffer is not thread-safe. + +class Ringbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + Ringbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~Ringbuffer(); + + /* @brief Allocate ringbuffer + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Space available to copy bytes into + * + * @returns number of free bytes. + */ + size_t space_available() const; + + /* + * @brief Space used to copy data from + * + * @returns number of used bytes. + */ + size_t space_used() const; + + /* + * @brief Copy data into ringbuffer + * + * @param buf Pointer to buffer to copy from. + * @param buf_len Number of bytes to copy. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *buf, size_t buf_len); + + /* + * @brief Get data from ringbuffer + * + * @param buf Pointer to buffer where data can be copied into. + * @param max_buf_len Max number of bytes to copy. + * + * @returns 0 if buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + size_t _size {0}; + uint8_t *_ringbuffer {nullptr}; + size_t _start{0}; + size_t _end{0}; +}; diff --git a/src/lib/ringbuffer/RingbufferTest.cpp b/src/lib/ringbuffer/RingbufferTest.cpp new file mode 100644 index 0000000000..e5e0a7a57f --- /dev/null +++ b/src/lib/ringbuffer/RingbufferTest.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + + +#include "Ringbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(Ringbuffer, AllocateAndDeallocate) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(Ringbuffer, PushATooBigMessage) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(Ringbuffer, PushAndPopOne) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + EXPECT_EQ(buf.space_used(), 20); + EXPECT_EQ(buf.space_available(), 79); + + // Get everything + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + // Nothing remaining + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(Ringbuffer, PushAndPopSeveral) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{90}; + data.paint(); + + // 9 little chunks in + for (unsigned i = 0; i < 9; ++i) { + EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10)); + } + + // 10 won't because of overhead inside the buffer + EXPECT_FALSE(buf.push_back(data.buf(), 10)); + + TempData out{90}; + // Take it back out in 2 big steps + EXPECT_EQ(buf.pop_front(out.buf(), 50), 50); + EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40); + EXPECT_EQ(data, out); +} + +TEST(Ringbuffer, PushAndTryToPopMore) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData out1{80}; + EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size()); +} + +TEST(Ringbuffer, PushAndPopSeveralInterleaved) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(50); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + TempData out12{80}; + EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size()); + + TempData out12_ref{80}; + out12_ref.paint(); + EXPECT_EQ(out12_ref, out12); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size()); + EXPECT_EQ(data3, out3); +} + +TEST(Ringbuffer, PushEmpty) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(Ringbuffer, PopWithoutBuffer) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(Ringbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + Ringbuffer buf; + // Allocate 1 bytes more than the packet, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(21)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index d4d1ec112d..95079371ec 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -159,12 +159,39 @@ bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal) return false; } -void Magnetometer::set_rotation(Rotation rotation) +void Magnetometer::set_rotation(const Rotation rotation) { - _rotation_enum = rotation; + if (rotation < ROTATION_MAX) { + _rotation_enum = rotation; + + } else { + // invalid rotation, resetting + _rotation_enum = ROTATION_NONE; + } + + // always apply level adjustments + _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum); + + // clear any custom rotation + _rotation_custom_euler.zero(); +} + +void Magnetometer::set_custom_rotation(const Eulerf &rotation) +{ + _rotation_enum = ROTATION_CUSTOM; + + // store custom rotation + _rotation_custom_euler = rotation; // always apply board level adjustments - _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); + _rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler); + + // TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation + // between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot. + // however, since while doing the 'level-calibration', users don't put the vehicle truly *horizontal, the + // measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply + // internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset + // values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective) } bool Magnetometer::set_calibration_index(int calibration_index) @@ -200,13 +227,39 @@ bool Magnetometer::ParametersLoad() // CAL_MAGx_ROT int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); + const float euler_roll_deg = GetCalibrationParamFloat(SensorString(), "ROLL", _calibration_index); + const float euler_pitch_deg = GetCalibrationParamFloat(SensorString(), "PITCH", _calibration_index); + const float euler_yaw_deg = GetCalibrationParamFloat(SensorString(), "YAW", _calibration_index); + if (_external) { - if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { + if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) { // invalid rotation, resetting rotation_value = ROTATION_NONE; } - set_rotation(static_cast(rotation_value)); + // if CAL_MAGx_{ROLL,PITCH,YAW} manually set then CAL_MAGx_ROT needs to be ROTATION_CUSTOM + if ((rotation_value != ROTATION_CUSTOM) + && ((fabsf(euler_roll_deg) > FLT_EPSILON) + || (fabsf(euler_pitch_deg) > FLT_EPSILON) + || (fabsf(euler_yaw_deg) > FLT_EPSILON))) { + + rotation_value = ROTATION_CUSTOM; + SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value); + } + + // Handle custom specified euler angle + if (rotation_value == ROTATION_CUSTOM) { + + const matrix::Eulerf rotation_custom_euler{ + math::radians(euler_roll_deg), + math::radians(euler_pitch_deg), + math::radians(euler_yaw_deg)}; + + set_custom_rotation(rotation_custom_euler); + + } else { + set_rotation(static_cast(rotation_value)); + } } else { // internal sensors follow board rotation @@ -311,6 +364,10 @@ bool Magnetometer::ParametersSave(int desired_calibration_index, bool force) success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal } + success &= SetCalibrationParam(SensorString(), "ROLL", _calibration_index, math::degrees(_rotation_custom_euler(0))); + success &= SetCalibrationParam(SensorString(), "PITCH", _calibration_index, math::degrees(_rotation_custom_euler(1))); + success &= SetCalibrationParam(SensorString(), "YAW", _calibration_index, math::degrees(_rotation_custom_euler(2))); + return success; } diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 047cd3398b..8d328e7a7d 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -65,8 +65,21 @@ public: bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); bool set_offdiagonal(const matrix::Vector3f &offdiagonal); + + /** + * @brief Set the rotation enum & corresponding rotation matrix for Magnetometer + * + * @param rotation Rotation enum + */ void set_rotation(Rotation rotation); + /** + * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer + * + * @param rotation Rotation euler angles + */ + void set_custom_rotation(const matrix::Eulerf &rotation); + bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } @@ -108,7 +121,13 @@ private: Rotation _rotation_enum{ROTATION_NONE}; + /** + * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD) + */ matrix::Dcmf _rotation; + + matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional) + matrix::Vector3f _offset; matrix::Matrix3f _scale; matrix::Vector3f _thermal_offset; diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index ca3a238838..f1c864bf3f 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -222,7 +222,7 @@ enum Rotation GetBoardRotation() int32_t board_rot = -1; param_get(param_find("SENS_BOARD_ROT"), &board_rot); - if (board_rot >= 0 && board_rot <= Rotation::ROTATION_MAX) { + if (board_rot >= 0 && board_rot < Rotation::ROTATION_MAX) { return static_cast(board_rot); } else { diff --git a/src/lib/slew_rate/SlewRate.hpp b/src/lib/slew_rate/SlewRate.hpp index 945276fedc..363f2d2cc7 100644 --- a/src/lib/slew_rate/SlewRate.hpp +++ b/src/lib/slew_rate/SlewRate.hpp @@ -49,6 +49,7 @@ class SlewRate { public: SlewRate() = default; + SlewRate(Type initial_value) { setForcedValue(initial_value); } ~SlewRate() = default; /** diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index f62a8ec853..bf7a680990 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -217,13 +217,11 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1); PARAM_DEFINE_INT32(SYS_HAS_BARO, 1); /** - * Control the number of distance sensors on the vehicle + * Number of distance sensors to check being available * - * If set to the number of distance sensors, the preflight check will check - * for their presence and valid data publication. Disable with 0 if no distance - * sensor present or to disable the preflight check. + * The preflight check will fail if fewer than this number of distance sensors with valid data is present. * - * @reboot_required true + * Disable the check with 0. * * @group System * @min 0 diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 5b6f5f2ffc..38ed565a05 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -156,6 +156,10 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS _alt_control_traj_generator.setMaxAccel(param.vert_accel_limit); _alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate)); + // XXX: this is a bit risky.. .alt_rate here could be NAN (by interface design) - and is only ok to input to the + // setVelSpFeedback() method because it calls the reset in the logic below when it is NAN. + // TODO: stop it with the NAN interfaces, make sure to take care of this when refactoring and separating altitude + // and height rate control loops. _velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate); bool control_altitude = true; diff --git a/src/lib/variable_length_ringbuffer/CMakeLists.txt b/src/lib/variable_length_ringbuffer/CMakeLists.txt new file mode 100644 index 0000000000..2d2bf47067 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2023 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(variable_length_ringbuffer + VariableLengthRingbuffer.cpp +) + +target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer) + +target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer) diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp new file mode 100644 index 0000000000..9b607e208b --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "VariableLengthRingbuffer.hpp" + +#include +#include + + +VariableLengthRingbuffer::~VariableLengthRingbuffer() +{ + deallocate(); +} + +bool VariableLengthRingbuffer::allocate(size_t buffer_size) +{ + return _ringbuffer.allocate(buffer_size); +} + +void VariableLengthRingbuffer::deallocate() +{ + _ringbuffer.deallocate(); +} + +bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len) +{ + if (packet_len == 0 || packet == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + size_t space_required = packet_len + sizeof(Header); + + if (space_required > _ringbuffer.space_available()) { + return false; + } + + Header header{static_cast(packet_len)}; + bool result = _ringbuffer.push_back(reinterpret_cast(&header), sizeof(header)); + assert(result); + + result = _ringbuffer.push_back(packet, packet_len); + assert(result); + + // In case asserts are commented out to prevent unused warnings. + (void)result; + + return true; +} + +size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + // Check next header + Header header; + + if (_ringbuffer.pop_front(reinterpret_cast(&header), sizeof(header)) < sizeof(header)) { + return 0; + } + + // We can't fit the packet into the user supplied buffer. + // This should never happen as the user has to supply a big // enough buffer. + assert(static_cast(header.len) <= buf_max_len); + + size_t bytes_read = _ringbuffer.pop_front(buf, header.len); + assert(bytes_read == header.len); + + return bytes_read; +} diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp new file mode 100644 index 0000000000..89f92eb246 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + + +// FIFO ringbuffer implementation for packets of variable length. +// +// The variable length is implemented using a 4 byte header +// containing a the length. +// +// The buffer is not thread-safe. + +class VariableLengthRingbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + VariableLengthRingbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~VariableLengthRingbuffer(); + + /* @brief Allocate ringbuffer + * + * @note The variable length requires 4 bytes + * of overhead per packet. + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Copy packet into ringbuffer + * + * @param packet Pointer to packet to copy from. + * @param packet_len Length of packet. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *packet, size_t packet_len); + + /* + * @brief Get packet from ringbuffer + * + * @note max_buf_len needs to be bigger equal to any pushed packet. + * + * @param buf Pointer to where next packet can be copied into. + * @param max_buf_len Max size of buf + * + * @returns 0 if packet is bigger than max_len or buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + struct Header { + uint32_t len; + }; + + Ringbuffer _ringbuffer {}; +}; diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp new file mode 100644 index 0000000000..958f36947e --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 + + +#include "VariableLengthRingbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(VariableLengthRingbuffer, AllocateAndDeallocate) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(VariableLengthRingbuffer, PushATooBigMessage) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(VariableLengthRingbuffer, PushAndPopOne) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + // Out buffer is the same size + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + // Out buffer is supposedly bigger + TempData out2{20}; + EXPECT_EQ(buf.pop_front(out2.buf(), 21), 20); + EXPECT_EQ(data, out2); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + // Disabled because it doesn't work reliably. + // For some reason the abort works when tests are filtered using TESTFILTER + // but not when all tests are run. + // + // Out buffer is too small + // Asserts are disabled in release build + //TempData out3{19}; + //EXPECT_DEATH(buf.pop_front(out3.buf(), out3.size()), ".*"); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveral) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + // 4 should fit + for (unsigned i = 0; i < 4; ++i) { + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + } + + // 5 won't because of overhead + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); + + // Take 4 back out + for (unsigned i = 0; i < 4; ++i) { + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), data.size()); + EXPECT_EQ(data, out); + } + + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveralVariableSize) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(42); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + // Supposedly more space + TempData out1{50}; + EXPECT_EQ(buf.pop_front(out1.buf(), 100), data1.size()); + EXPECT_EQ(data1, out1); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + // Supposedly more space + TempData out2{30}; + EXPECT_EQ(buf.pop_front(out2.buf(), 100), data2.size()); + EXPECT_EQ(data2, out2); + + // Supposedly more space + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), 100), data3.size()); + EXPECT_EQ(data3, out3); + + TempData out4{100}; + EXPECT_EQ(buf.pop_front(out4.buf(), out4.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushEmpty) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(VariableLengthRingbuffer, PopWithoutBuffer) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(VariableLengthRingbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + VariableLengthRingbuffer buf; + // Allocate 4+1 bytes more than the packet, 4 for the header, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(25)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py index fdc14ecee5..7d53e699b0 100755 --- a/src/lib/wind_estimator/python/derivation.py +++ b/src/lib/wind_estimator/python/derivation.py @@ -96,10 +96,10 @@ def init_wind_using_airspeed( airspeed_var: sf.Scalar, ) -> (sf.V2, sf.M22): - # Initialise wind states assuming zero side slip and horizontal flight - wind = sf.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading)) - # Zero sideslip, propagate the sideslip variance using partial derivatives w.r.t heading - J = wind.jacobian([v_local[0], v_local[1], heading, heading, airspeed]) + # Initialise wind states assuming horizontal flight + sideslip = sm.Symbol("beta") + wind = sf.V2(v_local[0] - airspeed * sm.cos(heading + sideslip), v_local[1] - airspeed * sm.sin(heading + sideslip)) + J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed]) R = sf.M55() R[0,0] = v_var @@ -110,6 +110,10 @@ def init_wind_using_airspeed( P = J * R * J.T + # Assume zero sideslip + P = P.subs({sideslip: 0.0}) + wind = wind.subs({sideslip: 0.0}) + return (wind, P) generate_px4_function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) diff --git a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h index cbfa6fa152..8fca830af2 100644 --- a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h +++ b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h @@ -34,21 +34,20 @@ void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Sc const Scalar sideslip_var, const Scalar airspeed_var, matrix::Matrix* const wind = nullptr, matrix::Matrix* const P = nullptr) { - // Total ops: 22 - - // Unused inputs - (void)heading_var; + // Total ops: 29 // Input arrays - // Intermediate terms (7) + // Intermediate terms (9) const Scalar _tmp0 = std::cos(heading); const Scalar _tmp1 = std::sin(heading); const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)); - const Scalar _tmp3 = std::pow(airspeed, Scalar(2)) * sideslip_var; - const Scalar _tmp4 = std::pow(_tmp0, Scalar(2)); - const Scalar _tmp5 = _tmp0 * _tmp1; - const Scalar _tmp6 = -_tmp3 * _tmp5 + _tmp5 * airspeed_var; + const Scalar _tmp3 = std::pow(airspeed, Scalar(2)); + const Scalar _tmp4 = _tmp3 * sideslip_var; + const Scalar _tmp5 = _tmp3 * heading_var; + const Scalar _tmp6 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp7 = _tmp0 * _tmp1; + const Scalar _tmp8 = -_tmp4 * _tmp7 - _tmp5 * _tmp7 + _tmp7 * airspeed_var; // Output terms (2) if (wind != nullptr) { @@ -61,10 +60,10 @@ void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Sc if (P != nullptr) { matrix::Matrix& _p = (*P); - _p(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var; - _p(1, 0) = _tmp6; - _p(0, 1) = _tmp6; - _p(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var; + _p(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var; + _p(1, 0) = _tmp8; + _p(0, 1) = _tmp8; + _p(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var; } } // NOLINT(readability/fn_size) diff --git a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp index 4a8d78c7c8..a1210cb8be 100644 --- a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp +++ b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp @@ -47,58 +47,58 @@ static constexpr int LON_DIM = 37; // Magnetic declination data in radians * 10^-4 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.5698, +// Date: 2023.6083, static constexpr const int16_t declination_table[19][37] { // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, - /* LAT: -90 */ { 25961, 24216, 22470, 20725, 18980, 17234, 15489, 13744, 11999, 10253, 8508, 6763, 5017, 3272, 1527, -218, -1964, -3709, -5454, -7200, -8945,-10690,-12436,-14181,-15926,-17672,-19417,-21162,-22908,-24653,-26398,-28144,-29889, 31197, 29452, 27707, 25961, }, - /* LAT: -80 */ { 22525, 20397, 18459, 16687, 15048, 13510, 12047, 10636, 9262, 7912, 6581, 5262, 3953, 2649, 1341, 21, -1321, -2695, -4106, -5560, -7057, -8595,-10175,-11795,-13461,-15181,-16968,-18842,-20826,-22946,-25222,-27657,-30225, 29969, 27354, 24852, 22525, }, - /* LAT: -70 */ { 14984, 13584, 12455, 11491, 10620, 9786, 8942, 8054, 7100, 6080, 5010, 3918, 2834, 1780, 760, -250, -1288, -2396, -3601, -4906, -6291, -7724, -9175,-10619,-12046,-13464,-14894,-16383,-18011,-19933,-22468,-26286, 30626, 24107, 19619, 16854, 14984, }, // WARNING! black out zone - /* LAT: -60 */ { 8445, 8195, 7909, 7630, 7372, 7115, 6804, 6368, 5751, 4929, 3926, 2815, 1697, 674, -202, -955, -1683, -2510, -3521, -4727, -6066, -7441, -8765, -9976,-11040,-11937,-12651,-13145,-13317,-12868,-10758, -3423, 5005, 7715, 8469, 8586, 8445, }, // WARNING! black out zone - /* LAT: -50 */ { 5504, 5539, 5479, 5386, 5309, 5269, 5231, 5102, 4755, 4088, 3074, 1799, 461, -712, -1571, -2124, -2514, -2957, -3655, -4685, -5944, -7229, -8372, -9271, -9863,-10094, -9890, -9124, -7608, -5240, -2331, 418, 2528, 3954, 4828, 5302, 5504, }, - /* LAT: -40 */ { 3969, 4061, 4066, 4017, 3954, 3917, 3920, 3907, 3731, 3191, 2162, 715, -847, -2151, -2999, -3436, -3608, -3662, -3841, -4436, -5439, -6526, -7409, -7938, -8033, -7641, -6739, -5349, -3641, -1945, -490, 730, 1775, 2645, 3304, 3737, 3969, }, - /* LAT: -30 */ { 2995, 3081, 3108, 3089, 3027, 2946, 2884, 2849, 2720, 2237, 1192, -335, -1941, -3183, -3907, -4238, -4304, -4078, -3641, -3446, -3846, -4609, -5302, -5638, -5497, -4896, -3929, -2724, -1526, -591, 80, 679, 1306, 1914, 2429, 2794, 2995, }, - /* LAT: -20 */ { 2353, 2398, 2411, 2408, 2362, 2265, 2154, 2076, 1930, 1429, 362, -1135, -2611, -3658, -4173, -4274, -4057, -3483, -2616, -1836, -1590, -1966, -2617, -3076, -3099, -2729, -2089, -1277, -511, -31, 225, 524, 969, 1455, 1884, 2196, 2353, }, - /* LAT: -10 */ { 1959, 1952, 1927, 1918, 1886, 1798, 1683, 1590, 1404, 849, -222, -1604, -2872, -3685, -3933, -3688, -3102, -2315, -1470, -724, -274, -324, -804, -1306, -1510, -1413, -1097, -601, -112, 121, 162, 319, 694, 1141, 1542, 1836, 1959, }, - /* LAT: 0 */ { 1746, 1711, 1651, 1637, 1622, 1549, 1438, 1320, 1065, 442, -603, -1831, -2877, -3448, -3427, -2913, -2141, -1361, -713, -181, 226, 327, 38, -379, -629, -681, -583, -324, -40, 42, -27, 58, 404, 856, 1285, 1612, 1746, }, - /* LAT: 10 */ { 1607, 1614, 1568, 1580, 1602, 1549, 1421, 1230, 853, 135, -887, -1960, -2777, -3102, -2883, -2267, -1489, -781, -271, 106, 429, 571, 402, 79, -152, -260, -286, -210, -110, -158, -304, -281, 26, 490, 979, 1393, 1607, }, - /* LAT: 20 */ { 1417, 1565, 1623, 1713, 1796, 1772, 1612, 1306, 761, -94, -1134, -2079, -2666, -2763, -2425, -1818, -1107, -461, -5, 301, 558, 700, 604, 360, 163, 43, -54, -125, -210, -405, -646, -706, -468, -13, 534, 1057, 1417, }, - /* LAT: 30 */ { 1107, 1473, 1732, 1955, 2114, 2122, 1932, 1507, 779, -247, -1358, -2226, -2630, -2560, -2156, -1574, -918, -303, 156, 458, 688, 831, 806, 656, 508, 382, 218, -2, -291, -670, -1040, -1199, -1036, -605, -25, 586, 1107, }, - /* LAT: 40 */ { 739, 1324, 1819, 2213, 2463, 2506, 2290, 1757, 847, -374, -1602, -2458, -2770, -2613, -2165, -1571, -918, -290, 217, 579, 847, 1042, 1131, 1118, 1042, 892, 615, 193, -354, -964, -1484, -1725, -1604, -1185, -587, 83, 739, }, - /* LAT: 50 */ { 437, 1182, 1864, 2425, 2795, 2904, 2677, 2027, 892, -593, -2001, -2905, -3194, -3000, -2510, -1864, -1154, -458, 156, 659, 1075, 1426, 1702, 1869, 1885, 1692, 1237, 515, -389, -1293, -1967, -2247, -2118, -1677, -1047, -322, 437, }, - /* LAT: 60 */ { 223, 1072, 1879, 2579, 3090, 3310, 3104, 2305, 811, -1124, -2822, -3795, -4048, -3790, -3220, -2477, -1653, -815, -9, 740, 1430, 2058, 2597, 2987, 3140, 2943, 2288, 1154, -272, -1589, -2448, -2749, -2582, -2093, -1409, -618, 223, }, - /* LAT: 70 */ { -44, 886, 1782, 2584, 3205, 3510, 3278, 2184, 16, -2634, -4572, -5410, -5435, -4965, -4205, -3277, -2258, -1196, -125, 932, 1955, 2919, 3784, 4486, 4926, 4946, 4316, 2812, 593, -1517, -2805, -3242, -3084, -2560, -1821, -962, -44, }, // WARNING! black out zone - /* LAT: 80 */ { -858, 61, 911, 1602, 1994, 1843, 745, -1647, -4659, -6762, -7572, -7500, -6914, -6027, -4957, -3773, -2518, -1221, 99, 1422, 2734, 4015, 5242, 6381, 7379, 8141, 8483, 8021, 6019, 2077, -1609, -3310, -3650, -3308, -2627, -1777, -858, }, // WARNING! black out zone - /* LAT: 90 */ { -29448,-27702,-25957,-24211,-22466,-20721,-18975,-17230,-15485,-13740,-11994,-10249, -8504, -6759, -5014, -3269, -1524, 222, 1967, 3712, 5457, 7202, 8948, 10693, 12438, 14184, 15929, 17675, 19420, 21165, 22911, 24657, 26402, 28148, 29893,-31193,-29448, }, // WARNING! black out zone + /* LAT: -90 */ { 25960, 24215, 22470, 20724, 18979, 17233, 15488, 13743, 11998, 10252, 8507, 6762, 5016, 3271, 1526, -219, -1965, -3710, -5455, -7201, -8946,-10691,-12436,-14182,-15927,-17672,-19418,-21163,-22909,-24654,-26399,-28145,-29890, 31196, 29451, 27706, 25960, }, + /* LAT: -80 */ { 22524, 20396, 18458, 16686, 15047, 13510, 12046, 10636, 9261, 7912, 6580, 5262, 3953, 2648, 1340, 20, -1322, -2696, -4107, -5561, -7058, -8596,-10176,-11796,-13462,-15182,-16969,-18843,-20828,-22948,-25223,-27658,-30226, 29968, 27353, 24851, 22524, }, + /* LAT: -70 */ { 14984, 13584, 12456, 11491, 10620, 9786, 8942, 8053, 7100, 6080, 5010, 3917, 2834, 1780, 760, -250, -1288, -2397, -3602, -4906, -6291, -7725, -9176,-10620,-12048,-13465,-14895,-16384,-18013,-19935,-22470,-26290, 30623, 24105, 19619, 16855, 14984, }, // WARNING! black out zone + /* LAT: -60 */ { 8447, 8196, 7910, 7631, 7373, 7115, 6804, 6368, 5750, 4928, 3926, 2814, 1697, 674, -202, -955, -1683, -2509, -3521, -4728, -6067, -7442, -8766, -9977,-11041,-11938,-12652,-13146,-13318,-12869,-10759, -3420, 5009, 7718, 8471, 8588, 8447, }, // WARNING! black out zone + /* LAT: -50 */ { 5505, 5539, 5480, 5387, 5309, 5269, 5231, 5101, 4754, 4087, 3073, 1799, 460, -712, -1571, -2123, -2513, -2956, -3654, -4686, -5945, -7230, -8373, -9272, -9864,-10094, -9891, -9124, -7608, -5240, -2330, 419, 2529, 3955, 4829, 5303, 5505, }, + /* LAT: -40 */ { 3969, 4061, 4066, 4018, 3954, 3917, 3920, 3907, 3731, 3191, 2161, 714, -848, -2152, -2999, -3436, -3607, -3661, -3840, -4436, -5440, -6527, -7410, -7939, -8033, -7641, -6738, -5348, -3640, -1945, -490, 730, 1775, 2645, 3305, 3737, 3969, }, + /* LAT: -30 */ { 2996, 3082, 3108, 3090, 3027, 2946, 2884, 2849, 2720, 2236, 1191, -337, -1942, -3184, -3907, -4238, -4303, -4077, -3640, -3446, -3847, -4610, -5303, -5639, -5497, -4896, -3928, -2724, -1526, -591, 80, 679, 1306, 1914, 2429, 2795, 2996, }, + /* LAT: -20 */ { 2353, 2398, 2412, 2409, 2362, 2264, 2154, 2076, 1930, 1429, 361, -1137, -2613, -3659, -4174, -4274, -4056, -3481, -2614, -1835, -1590, -1966, -2618, -3076, -3099, -2729, -2088, -1276, -510, -31, 225, 524, 969, 1455, 1884, 2197, 2353, }, + /* LAT: -10 */ { 1960, 1953, 1927, 1918, 1886, 1797, 1683, 1589, 1404, 849, -223, -1606, -2873, -3686, -3932, -3687, -3100, -2313, -1469, -723, -274, -324, -804, -1306, -1510, -1413, -1097, -601, -112, 120, 161, 318, 694, 1141, 1542, 1836, 1960, }, + /* LAT: 0 */ { 1746, 1712, 1651, 1638, 1622, 1549, 1438, 1320, 1065, 441, -604, -1833, -2878, -3448, -3427, -2912, -2139, -1359, -712, -180, 227, 327, 38, -379, -628, -681, -583, -324, -40, 41, -28, 57, 404, 856, 1285, 1612, 1746, }, + /* LAT: 10 */ { 1607, 1614, 1568, 1580, 1602, 1548, 1421, 1229, 852, 134, -888, -1961, -2778, -3102, -2882, -2266, -1488, -780, -271, 107, 430, 572, 402, 79, -152, -259, -286, -210, -111, -158, -305, -281, 25, 489, 979, 1393, 1607, }, + /* LAT: 20 */ { 1417, 1565, 1623, 1713, 1796, 1772, 1612, 1305, 760, -95, -1135, -2080, -2666, -2762, -2424, -1817, -1106, -460, -4, 302, 559, 700, 604, 360, 163, 43, -54, -125, -211, -405, -647, -707, -468, -13, 534, 1057, 1417, }, + /* LAT: 30 */ { 1107, 1473, 1732, 1955, 2114, 2122, 1931, 1506, 778, -247, -1359, -2226, -2630, -2559, -2155, -1573, -917, -302, 157, 459, 688, 831, 806, 656, 508, 382, 218, -2, -292, -670, -1041, -1200, -1036, -605, -25, 586, 1107, }, + /* LAT: 40 */ { 739, 1324, 1819, 2212, 2462, 2506, 2290, 1756, 847, -375, -1602, -2457, -2769, -2612, -2163, -1570, -917, -288, 218, 580, 848, 1042, 1132, 1119, 1043, 892, 615, 193, -355, -965, -1485, -1725, -1605, -1186, -587, 82, 739, }, + /* LAT: 50 */ { 436, 1181, 1863, 2424, 2794, 2903, 2677, 2026, 892, -593, -2001, -2904, -3192, -2999, -2508, -1862, -1153, -456, 157, 660, 1076, 1427, 1703, 1870, 1886, 1693, 1237, 514, -390, -1294, -1967, -2247, -2118, -1678, -1047, -322, 436, }, + /* LAT: 60 */ { 222, 1071, 1877, 2577, 3088, 3309, 3103, 2305, 811, -1123, -2820, -3793, -4046, -3788, -3218, -2475, -1651, -813, -8, 741, 1431, 2059, 2598, 2988, 3141, 2943, 2288, 1153, -273, -1591, -2449, -2750, -2583, -2093, -1409, -619, 222, }, + /* LAT: 70 */ { -47, 883, 1779, 2581, 3202, 3508, 3276, 2184, 17, -2630, -4567, -5406, -5432, -4962, -4202, -3275, -2256, -1194, -123, 934, 1957, 2920, 3785, 4488, 4927, 4947, 4315, 2811, 591, -1519, -2807, -3244, -3086, -2561, -1823, -964, -47, }, // WARNING! black out zone + /* LAT: 80 */ { -864, 55, 904, 1595, 1987, 1837, 743, -1642, -4648, -6750, -7562, -7492, -6908, -6022, -4952, -3769, -2514, -1217, 102, 1425, 2737, 4018, 5245, 6384, 7382, 8144, 8486, 8024, 6018, 2070, -1618, -3319, -3658, -3315, -2633, -1784, -864, }, // WARNING! black out zone + /* LAT: 90 */ { -29433,-27688,-25942,-24197,-22451,-20706,-18961,-17215,-15470,-13725,-11980,-10234, -8489, -6744, -4999, -3254, -1509, 236, 1982, 3727, 5472, 7217, 8962, 10708, 12453, 14199, 15944, 17689, 19435, 21180, 22926, 24671, 26417, 28162, 29908,-31178,-29433, }, // WARNING! black out zone }; -static constexpr float WMM_DECLINATION_MIN_RAD = -3.119; // latitude: 90, longitude: 170 +static constexpr float WMM_DECLINATION_MIN_RAD = -3.118; // latitude: 90, longitude: 170 static constexpr float WMM_DECLINATION_MAX_RAD = 3.120; // latitude: -90, longitude: 150 // Magnetic inclination data in radians * 10^-4 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.5698, +// Date: 2023.6083, static constexpr const int16_t inclination_table[19][37] { // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, /* LAT: -90 */ { -12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565, }, - /* LAT: -80 */ { -13648,-13514,-13353,-13173,-12979,-12778,-12575,-12375,-12184,-12008,-11852,-11718,-11608,-11522,-11458,-11416,-11396,-11399,-11426,-11480,-11564,-11678,-11824,-11998,-12196,-12414,-12643,-12876,-13104,-13316,-13502,-13652,-13755,-13806,-13802,-13747,-13648, }, - /* LAT: -70 */ { -14095,-13777,-13457,-13135,-12803,-12460,-12105,-11749,-11407,-11099,-10848,-10665,-10554,-10502,-10487,-10489,-10494,-10502,-10524,-10580,-10688,-10864,-11112,-11428,-11802,-12221,-12670,-13134,-13600,-14052,-14469,-14812,-14997,-14941,-14709,-14411,-14095, }, // WARNING! black out zone - /* LAT: -60 */ { -13512,-13158,-12819,-12486,-12143,-11771,-11356,-10902,-10436,-10007, -9680, -9508, -9507, -9646, -9849,-10038,-10158,-10196,-10180,-10163,-10211,-10373,-10666,-11078,-11579,-12137,-12724,-13321,-13909,-14471,-14968,-15257,-15075,-14688,-14281,-13886,-13512, }, // WARNING! black out zone - /* LAT: -50 */ { -12493,-12150,-11818,-11496,-11172,-10826,-10427, -9956, -9428, -8909, -8524, -8404, -8611, -9076, -9638,-10145,-10496,-10649,-10610,-10452,-10310,-10321,-10549,-10972,-11519,-12113,-12699,-13236,-13681,-13977,-14084,-14011,-13807,-13522,-13193,-12844,-12493, }, - /* LAT: -40 */ { -11239,-10889,-10540,-10194, -9854, -9517, -9157, -8733, -8213, -7651, -7231, -7200, -7671, -8500, -9421,-10244,-10895,-11319,-11444,-11264,-10916,-10649,-10653,-10944,-11411,-11917,-12359,-12677,-12833,-12837,-12748,-12612,-12434,-12203,-11916,-11587,-11239, }, - /* LAT: -30 */ { -9602, -9220, -8838, -8446, -8054, -7680, -7326, -6937, -6426, -5816, -5373, -5473, -6260, -7471, -8720, -9809,-10714,-11413,-11796,-11769,-11385,-10877,-10553,-10563,-10821,-11143,-11395,-11503,-11437,-11261,-11089,-10959,-10816,-10611,-10330, -9983, -9602, }, - /* LAT: -20 */ { -7373, -6926, -6504, -6074, -5630, -5201, -4814, -4405, -3842, -3159, -2722, -3007, -4143, -5767, -7392, -8749, -9801,-10563,-10985,-11002,-10625,-10004, -9450, -9216, -9271, -9434, -9572, -9584, -9409, -9138, -8949, -8869, -8772, -8568, -8252, -7837, -7373, }, - /* LAT: -10 */ { -4418, -3873, -3412, -2972, -2514, -2065, -1655, -1208, -588, 110, 457, 3, -1355, -3288, -5257, -6846, -7920, -8537, -8787, -8710, -8286, -7592, -6930, -6592, -6558, -6651, -6766, -6785, -6595, -6301, -6153, -6175, -6146, -5935, -5552, -5020, -4418, }, - /* LAT: 0 */ { -911, -276, 197, 603, 1022, 1438, 1824, 2259, 2834, 3402, 3590, 3070, 1748, -179, -2218, -3846, -4828, -5237, -5283, -5102, -4648, -3921, -3216, -2852, -2794, -2861, -2984, -3055, -2924, -2691, -2645, -2805, -2883, -2703, -2278, -1643, -911, }, - /* LAT: 10 */ { 2557, 3193, 3635, 3979, 4335, 4701, 5049, 5427, 5869, 6230, 6254, 5748, 4646, 3066, 1379, 22, -759, -988, -884, -641, -220, 428, 1061, 1394, 1457, 1417, 1319, 1228, 1270, 1370, 1285, 1003, 796, 861, 1214, 1822, 2557, }, - /* LAT: 20 */ { 5413, 5949, 6333, 6630, 6944, 7287, 7626, 7967, 8292, 8485, 8388, 7914, 7067, 5964, 4841, 3945, 3432, 3327, 3487, 3736, 4072, 4544, 5004, 5258, 5316, 5304, 5258, 5200, 5186, 5164, 4983, 4639, 4331, 4236, 4409, 4836, 5413, }, - /* LAT: 30 */ { 7568, 7944, 8264, 8547, 8857, 9203, 9558, 9894, 10165, 10273, 10121, 9693, 9059, 8347, 7689, 7185, 6903, 6870, 7019, 7234, 7485, 7788, 8077, 8251, 8309, 8326, 8331, 8321, 8296, 8210, 7982, 7623, 7262, 7039, 7023, 7220, 7568, }, - /* LAT: 40 */ { 9266, 9487, 9744, 10029, 10355, 10715, 11081, 11416, 11664, 11742, 11588, 11226, 10752, 10280, 9887, 9608, 9464, 9466, 9581, 9745, 9924, 10110, 10283, 10408, 10487, 10550, 10607, 10641, 10622, 10508, 10262, 9908, 9538, 9252, 9109, 9122, 9266, }, - /* LAT: 50 */ { 10802, 10923, 11123, 11392, 11714, 12067, 12419, 12733, 12952, 13009, 12868, 12570, 12207, 11863, 11591, 11407, 11317, 11317, 11387, 11493, 11608, 11726, 11843, 11959, 12078, 12203, 12317, 12390, 12378, 12250, 12001, 11671, 11331, 11046, 10856, 10776, 10802, }, - /* LAT: 60 */ { 12320, 12391, 12540, 12756, 13024, 13323, 13624, 13888, 14063, 14090, 13955, 13708, 13420, 13151, 12932, 12777, 12689, 12659, 12677, 12726, 12796, 12882, 12989, 13123, 13283, 13460, 13625, 13732, 13735, 13612, 13386, 13107, 12829, 12592, 12422, 12330, 12320, }, - /* LAT: 70 */ { 13757, 13798, 13891, 14031, 14208, 14410, 14616, 14797, 14902, 14885, 14751, 14552, 14336, 14133, 13960, 13827, 13733, 13680, 13662, 13675, 13718, 13791, 13896, 14033, 14201, 14389, 14573, 14712, 14756, 14681, 14519, 14321, 14126, 13962, 13841, 13772, 13757, }, // WARNING! black out zone - /* LAT: 80 */ { 14993, 15004, 15040, 15097, 15172, 15255, 15332, 15378, 15364, 15290, 15181, 15059, 14938, 14825, 14726, 14644, 14583, 14542, 14524, 14528, 14555, 14605, 14677, 14770, 14883, 15010, 15146, 15278, 15386, 15429, 15387, 15298, 15201, 15115, 15050, 15008, 14993, }, // WARNING! black out zone - /* LAT: 90 */ { 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, }, // WARNING! black out zone + /* LAT: -80 */ { -13648,-13514,-13353,-13173,-12979,-12778,-12574,-12374,-12184,-12008,-11852,-11718,-11608,-11521,-11458,-11416,-11396,-11399,-11426,-11480,-11564,-11678,-11824,-11997,-12196,-12413,-12643,-12876,-13104,-13316,-13502,-13652,-13755,-13805,-13801,-13747,-13648, }, + /* LAT: -70 */ { -14095,-13776,-13457,-13134,-12803,-12459,-12105,-11749,-11406,-11099,-10848,-10665,-10554,-10502,-10487,-10489,-10494,-10502,-10524,-10580,-10688,-10864,-11112,-11428,-11802,-12221,-12670,-13134,-13600,-14052,-14469,-14812,-14997,-14941,-14709,-14411,-14095, }, // WARNING! black out zone + /* LAT: -60 */ { -13512,-13158,-12819,-12486,-12143,-11771,-11356,-10902,-10436,-10007, -9680, -9508, -9508, -9646, -9849,-10038,-10158,-10196,-10179,-10163,-10211,-10372,-10666,-11078,-11580,-12137,-12725,-13321,-13910,-14471,-14968,-15257,-15076,-14688,-14281,-13886,-13512, }, // WARNING! black out zone + /* LAT: -50 */ { -12493,-12150,-11818,-11495,-11172,-10826,-10427, -9956, -9428, -8909, -8524, -8405, -8612, -9076, -9639,-10145,-10496,-10649,-10609,-10451,-10309,-10320,-10549,-10972,-11519,-12113,-12699,-13236,-13681,-13978,-14084,-14011,-13807,-13523,-13193,-12844,-12493, }, + /* LAT: -40 */ { -11239,-10889,-10540,-10194, -9854, -9517, -9157, -8733, -8213, -7651, -7232, -7201, -7672, -8502, -9422,-10245,-10896,-11319,-11444,-11263,-10915,-10649,-10653,-10944,-11412,-11917,-12360,-12677,-12833,-12837,-12748,-12612,-12434,-12203,-11916,-11587,-11239, }, + /* LAT: -30 */ { -9602, -9220, -8837, -8446, -8054, -7680, -7325, -6937, -6426, -5816, -5373, -5474, -6262, -7473, -8722, -9810,-10716,-11414,-11796,-11768,-11384,-10876,-10552,-10563,-10821,-11143,-11395,-11502,-11436,-11261,-11089,-10959,-10817,-10611,-10330, -9983, -9602, }, + /* LAT: -20 */ { -7373, -6926, -6504, -6073, -5629, -5201, -4813, -4405, -3842, -3159, -2723, -3009, -4145, -5770, -7395, -8751, -9803,-10564,-10985,-11002,-10624,-10004, -9450, -9215, -9271, -9434, -9572, -9584, -9408, -9137, -8948, -8869, -8772, -8568, -8252, -7837, -7373, }, + /* LAT: -10 */ { -4418, -3872, -3412, -2971, -2514, -2065, -1655, -1208, -588, 110, 456, 1, -1358, -3291, -5260, -6848, -7922, -8538, -8787, -8710, -8286, -7591, -6929, -6592, -6557, -6651, -6765, -6785, -6595, -6300, -6153, -6175, -6147, -5936, -5552, -5020, -4418, }, + /* LAT: 0 */ { -911, -276, 198, 604, 1023, 1439, 1825, 2259, 2833, 3402, 3589, 3068, 1745, -183, -2221, -3848, -4830, -5238, -5283, -5101, -4647, -3920, -3215, -2851, -2793, -2860, -2983, -3054, -2923, -2690, -2645, -2805, -2883, -2704, -2279, -1644, -911, }, + /* LAT: 10 */ { 2557, 3194, 3635, 3980, 4336, 4702, 5049, 5427, 5869, 6230, 6253, 5746, 4643, 3064, 1376, 20, -760, -988, -885, -641, -219, 429, 1062, 1394, 1458, 1418, 1320, 1229, 1271, 1371, 1286, 1003, 795, 860, 1213, 1821, 2557, }, + /* LAT: 20 */ { 5413, 5949, 6333, 6631, 6945, 7287, 7627, 7967, 8292, 8485, 8388, 7913, 7065, 5962, 4840, 3943, 3431, 3327, 3487, 3736, 4073, 4544, 5005, 5258, 5317, 5305, 5259, 5201, 5187, 5164, 4983, 4639, 4330, 4235, 4408, 4835, 5413, }, + /* LAT: 30 */ { 7568, 7944, 8264, 8548, 8857, 9203, 9558, 9894, 10165, 10272, 10121, 9692, 9058, 8346, 7688, 7184, 6902, 6870, 7019, 7234, 7485, 7788, 8078, 8251, 8310, 8327, 8332, 8322, 8297, 8210, 7983, 7623, 7262, 7039, 7023, 7220, 7568, }, + /* LAT: 40 */ { 9266, 9487, 9744, 10029, 10355, 10715, 11081, 11416, 11664, 11742, 11588, 11225, 10751, 10279, 9886, 9608, 9464, 9466, 9581, 9746, 9924, 10110, 10283, 10408, 10487, 10550, 10608, 10641, 10622, 10508, 10262, 9908, 9538, 9252, 9109, 9122, 9266, }, + /* LAT: 50 */ { 10802, 10923, 11123, 11392, 11714, 12067, 12419, 12733, 12952, 13009, 12868, 12570, 12206, 11863, 11590, 11407, 11317, 11317, 11387, 11493, 11609, 11726, 11843, 11960, 12079, 12203, 12318, 12390, 12378, 12250, 12001, 11671, 11331, 11046, 10856, 10776, 10802, }, + /* LAT: 60 */ { 12320, 12391, 12539, 12756, 13024, 13323, 13623, 13888, 14063, 14090, 13955, 13707, 13420, 13150, 12931, 12777, 12688, 12659, 12677, 12726, 12796, 12883, 12990, 13123, 13283, 13460, 13625, 13733, 13736, 13613, 13386, 13107, 12829, 12592, 12422, 12330, 12320, }, + /* LAT: 70 */ { 13757, 13798, 13891, 14030, 14208, 14409, 14616, 14796, 14901, 14884, 14751, 14552, 14336, 14133, 13960, 13826, 13733, 13680, 13662, 13675, 13719, 13792, 13896, 14034, 14201, 14389, 14573, 14712, 14756, 14682, 14519, 14321, 14126, 13962, 13841, 13772, 13757, }, // WARNING! black out zone + /* LAT: 80 */ { 14993, 15004, 15039, 15097, 15171, 15255, 15332, 15378, 15364, 15290, 15181, 15059, 14938, 14825, 14726, 14644, 14583, 14542, 14524, 14529, 14556, 14605, 14677, 14771, 14883, 15010, 15146, 15279, 15386, 15429, 15387, 15298, 15201, 15115, 15049, 15008, 14993, }, // WARNING! black out zone + /* LAT: 90 */ { 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, }, // WARNING! black out zone }; static constexpr float WMM_INCLINATION_MIN_RAD = -1.526; // latitude: -60, longitude: 130 static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitude: 110 @@ -107,30 +107,30 @@ static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitu // Magnetic strength data in milli-Gauss * 10 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.5698, +// Date: 2023.6083, static constexpr const int16_t strength_table[19][37] { // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, /* LAT: -90 */ { 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, }, - /* LAT: -80 */ { 6052, 5988, 5908, 5816, 5713, 5601, 5482, 5360, 5238, 5118, 5004, 4899, 4805, 4726, 4663, 4619, 4595, 4593, 4615, 4661, 4732, 4827, 4942, 5074, 5218, 5368, 5518, 5661, 5792, 5906, 5998, 6067, 6111, 6130, 6125, 6098, 6052, }, - /* LAT: -70 */ { 6297, 6162, 6011, 5845, 5666, 5473, 5268, 5053, 4834, 4619, 4417, 4235, 4078, 3947, 3844, 3767, 3721, 3709, 3738, 3815, 3944, 4125, 4355, 4623, 4919, 5227, 5530, 5814, 6064, 6267, 6418, 6513, 6554, 6545, 6495, 6409, 6297, }, - /* LAT: -60 */ { 6182, 5989, 5786, 5576, 5357, 5122, 4865, 4586, 4294, 4005, 3739, 3512, 3334, 3200, 3101, 3028, 2977, 2957, 2984, 3076, 3246, 3499, 3828, 4215, 4638, 5073, 5494, 5878, 6202, 6450, 6612, 6690, 6691, 6627, 6512, 6360, 6182, }, - /* LAT: -50 */ { 5840, 5609, 5376, 5144, 4911, 4666, 4395, 4091, 3763, 3431, 3130, 2891, 2731, 2641, 2592, 2557, 2523, 2496, 2501, 2575, 2751, 3044, 3443, 3917, 4424, 4930, 5405, 5823, 6161, 6402, 6539, 6578, 6535, 6423, 6261, 6061, 5840, }, - /* LAT: -40 */ { 5391, 5144, 4898, 4657, 4422, 4184, 3929, 3645, 3331, 3005, 2708, 2486, 2371, 2346, 2365, 2385, 2388, 2374, 2361, 2391, 2525, 2805, 3227, 3744, 4291, 4814, 5280, 5666, 5958, 6144, 6232, 6234, 6163, 6031, 5849, 5630, 5391, }, - /* LAT: -30 */ { 4877, 4635, 4395, 4160, 3933, 3714, 3494, 3263, 3007, 2731, 2474, 2293, 2225, 2251, 2318, 2388, 2453, 2501, 2521, 2532, 2602, 2807, 3175, 3666, 4199, 4695, 5110, 5425, 5626, 5725, 5751, 5724, 5644, 5511, 5331, 5114, 4877, }, - /* LAT: -20 */ { 4320, 4106, 3896, 3691, 3495, 3312, 3143, 2979, 2801, 2602, 2411, 2279, 2240, 2286, 2376, 2487, 2614, 2740, 2826, 2859, 2886, 2986, 3232, 3620, 4072, 4498, 4843, 5075, 5179, 5186, 5155, 5108, 5026, 4898, 4731, 4534, 4320, }, - /* LAT: -10 */ { 3790, 3628, 3474, 3327, 3191, 3071, 2967, 2875, 2776, 2661, 2539, 2440, 2397, 2424, 2513, 2642, 2798, 2955, 3076, 3136, 3150, 3179, 3306, 3562, 3890, 4212, 4475, 4635, 4668, 4615, 4548, 4485, 4397, 4272, 4123, 3958, 3790, }, - /* LAT: 0 */ { 3412, 3318, 3233, 3160, 3105, 3066, 3039, 3019, 2994, 2945, 2865, 2772, 2693, 2665, 2709, 2814, 2946, 3081, 3194, 3268, 3298, 3321, 3398, 3558, 3769, 3983, 4162, 4266, 4271, 4202, 4114, 4022, 3911, 3780, 3645, 3520, 3412, }, - /* LAT: 10 */ { 3282, 3251, 3230, 3225, 3249, 3296, 3351, 3403, 3436, 3426, 3356, 3242, 3117, 3024, 3001, 3044, 3126, 3224, 3324, 3409, 3472, 3535, 3624, 3744, 3881, 4020, 4140, 4209, 4210, 4147, 4037, 3894, 3732, 3572, 3436, 3339, 3282, }, - /* LAT: 20 */ { 3399, 3401, 3426, 3479, 3570, 3690, 3818, 3934, 4014, 4025, 3952, 3810, 3645, 3507, 3434, 3424, 3460, 3534, 3631, 3728, 3820, 3919, 4030, 4143, 4252, 4365, 4468, 4533, 4543, 4483, 4345, 4142, 3914, 3704, 3540, 3439, 3399, }, - /* LAT: 30 */ { 3722, 3727, 3780, 3878, 4020, 4190, 4365, 4520, 4628, 4654, 4581, 4426, 4239, 4077, 3974, 3930, 3935, 3987, 4075, 4174, 4272, 4379, 4496, 4613, 4734, 4862, 4983, 5069, 5094, 5035, 4877, 4637, 4362, 4107, 3908, 3778, 3722, }, - /* LAT: 40 */ { 4222, 4218, 4281, 4403, 4568, 4754, 4937, 5093, 5198, 5224, 5156, 5008, 4823, 4651, 4525, 4451, 4427, 4451, 4512, 4593, 4680, 4778, 4893, 5027, 5178, 5341, 5493, 5602, 5640, 5584, 5427, 5188, 4914, 4655, 4444, 4298, 4222, }, - /* LAT: 50 */ { 4832, 4822, 4875, 4983, 5128, 5286, 5436, 5557, 5631, 5641, 5579, 5453, 5292, 5130, 4994, 4898, 4845, 4834, 4860, 4911, 4981, 5073, 5192, 5342, 5518, 5703, 5869, 5985, 6028, 5982, 5850, 5655, 5431, 5216, 5036, 4905, 4832, }, - /* LAT: 60 */ { 5393, 5377, 5402, 5463, 5548, 5641, 5729, 5796, 5830, 5823, 5770, 5677, 5557, 5430, 5312, 5217, 5151, 5118, 5117, 5146, 5203, 5290, 5408, 5556, 5722, 5890, 6037, 6139, 6182, 6159, 6076, 5950, 5805, 5663, 5541, 5448, 5393, }, - /* LAT: 70 */ { 5726, 5704, 5699, 5710, 5731, 5757, 5781, 5796, 5797, 5779, 5741, 5685, 5616, 5540, 5467, 5404, 5357, 5329, 5325, 5345, 5391, 5460, 5552, 5661, 5778, 5892, 5990, 6063, 6101, 6104, 6075, 6022, 5955, 5885, 5819, 5765, 5726, }, - /* LAT: 80 */ { 5790, 5772, 5756, 5744, 5733, 5724, 5714, 5703, 5688, 5671, 5649, 5625, 5598, 5572, 5547, 5527, 5514, 5509, 5515, 5530, 5556, 5591, 5634, 5682, 5732, 5779, 5822, 5856, 5880, 5893, 5895, 5888, 5873, 5854, 5832, 5810, 5790, }, + /* LAT: -80 */ { 6052, 5987, 5908, 5816, 5712, 5600, 5482, 5360, 5238, 5118, 5004, 4899, 4805, 4726, 4663, 4619, 4595, 4593, 4615, 4661, 4732, 4827, 4942, 5074, 5218, 5368, 5518, 5661, 5792, 5906, 5998, 6067, 6111, 6130, 6125, 6098, 6052, }, + /* LAT: -70 */ { 6296, 6162, 6011, 5845, 5666, 5473, 5268, 5053, 4834, 4619, 4417, 4235, 4078, 3947, 3843, 3767, 3721, 3709, 3738, 3815, 3944, 4125, 4355, 4623, 4919, 5227, 5530, 5814, 6064, 6267, 6418, 6513, 6554, 6545, 6495, 6409, 6296, }, + /* LAT: -60 */ { 6182, 5988, 5786, 5576, 5356, 5121, 4864, 4586, 4294, 4005, 3738, 3512, 3333, 3200, 3101, 3028, 2977, 2957, 2984, 3075, 3246, 3499, 3828, 4215, 4638, 5073, 5494, 5878, 6202, 6450, 6612, 6690, 6691, 6627, 6512, 6360, 6182, }, + /* LAT: -50 */ { 5840, 5609, 5376, 5144, 4911, 4665, 4394, 4091, 3762, 3431, 3130, 2891, 2731, 2641, 2592, 2557, 2523, 2496, 2501, 2575, 2751, 3044, 3444, 3917, 4425, 4931, 5405, 5823, 6161, 6402, 6539, 6578, 6535, 6423, 6260, 6061, 5840, }, + /* LAT: -40 */ { 5390, 5144, 4898, 4657, 4422, 4184, 3929, 3645, 3331, 3005, 2707, 2486, 2370, 2346, 2365, 2385, 2388, 2374, 2361, 2391, 2524, 2805, 3227, 3744, 4292, 4815, 5280, 5667, 5958, 6144, 6232, 6234, 6163, 6031, 5849, 5630, 5390, }, + /* LAT: -30 */ { 4877, 4635, 4395, 4159, 3933, 3713, 3494, 3263, 3007, 2731, 2474, 2293, 2225, 2251, 2318, 2388, 2452, 2501, 2520, 2532, 2602, 2807, 3175, 3667, 4199, 4695, 5111, 5425, 5626, 5725, 5751, 5724, 5644, 5511, 5331, 5114, 4877, }, + /* LAT: -20 */ { 4320, 4106, 3896, 3691, 3494, 3312, 3143, 2979, 2801, 2602, 2411, 2279, 2240, 2286, 2376, 2487, 2614, 2740, 2826, 2859, 2885, 2986, 3232, 3620, 4072, 4498, 4844, 5075, 5179, 5186, 5155, 5108, 5026, 4898, 4731, 4534, 4320, }, + /* LAT: -10 */ { 3790, 3628, 3474, 3327, 3191, 3071, 2967, 2874, 2776, 2660, 2538, 2440, 2397, 2424, 2513, 2642, 2798, 2955, 3076, 3136, 3150, 3178, 3306, 3562, 3891, 4212, 4475, 4635, 4668, 4615, 4548, 4485, 4397, 4272, 4123, 3958, 3790, }, + /* LAT: 0 */ { 3412, 3318, 3233, 3160, 3104, 3066, 3039, 3019, 2993, 2944, 2865, 2771, 2693, 2665, 2709, 2814, 2946, 3081, 3194, 3268, 3298, 3321, 3398, 3558, 3769, 3983, 4162, 4266, 4271, 4203, 4114, 4022, 3911, 3780, 3645, 3520, 3412, }, + /* LAT: 10 */ { 3282, 3251, 3230, 3225, 3249, 3296, 3350, 3402, 3436, 3425, 3356, 3242, 3116, 3024, 3001, 3044, 3126, 3224, 3324, 3409, 3472, 3535, 3625, 3744, 3881, 4021, 4140, 4209, 4210, 4148, 4037, 3894, 3732, 3572, 3436, 3339, 3282, }, + /* LAT: 20 */ { 3399, 3401, 3426, 3479, 3570, 3690, 3818, 3934, 4013, 4025, 3951, 3810, 3644, 3507, 3434, 3424, 3460, 3534, 3631, 3729, 3820, 3919, 4031, 4143, 4253, 4365, 4468, 4534, 4543, 4483, 4345, 4142, 3914, 3704, 3540, 3439, 3399, }, + /* LAT: 30 */ { 3722, 3727, 3780, 3878, 4020, 4190, 4365, 4520, 4627, 4653, 4580, 4426, 4238, 4076, 3974, 3929, 3935, 3987, 4075, 4174, 4272, 4379, 4496, 4614, 4734, 4862, 4983, 5070, 5094, 5035, 4878, 4637, 4362, 4108, 3908, 3778, 3722, }, + /* LAT: 40 */ { 4222, 4218, 4281, 4402, 4568, 4754, 4937, 5093, 5197, 5223, 5155, 5008, 4823, 4651, 4525, 4451, 4427, 4451, 4513, 4593, 4680, 4778, 4893, 5027, 5178, 5341, 5493, 5602, 5640, 5584, 5427, 5188, 4914, 4655, 4444, 4298, 4222, }, + /* LAT: 50 */ { 4832, 4822, 4875, 4982, 5127, 5286, 5435, 5557, 5631, 5641, 5579, 5453, 5292, 5130, 4994, 4898, 4845, 4834, 4860, 4911, 4981, 5073, 5192, 5343, 5519, 5703, 5869, 5986, 6028, 5982, 5850, 5655, 5431, 5216, 5036, 4905, 4832, }, + /* LAT: 60 */ { 5393, 5377, 5402, 5463, 5547, 5641, 5728, 5795, 5830, 5822, 5770, 5677, 5557, 5430, 5312, 5217, 5152, 5119, 5117, 5146, 5203, 5290, 5408, 5556, 5722, 5890, 6037, 6139, 6182, 6159, 6076, 5950, 5805, 5663, 5541, 5448, 5393, }, + /* LAT: 70 */ { 5726, 5704, 5699, 5709, 5731, 5757, 5781, 5796, 5797, 5779, 5741, 5685, 5616, 5540, 5467, 5404, 5357, 5329, 5325, 5345, 5391, 5461, 5553, 5661, 5778, 5892, 5991, 6063, 6101, 6104, 6075, 6022, 5955, 5885, 5819, 5765, 5726, }, + /* LAT: 80 */ { 5790, 5772, 5756, 5744, 5733, 5724, 5714, 5703, 5688, 5671, 5649, 5625, 5598, 5572, 5547, 5528, 5514, 5510, 5515, 5530, 5556, 5591, 5634, 5682, 5732, 5780, 5822, 5856, 5880, 5893, 5895, 5888, 5873, 5854, 5832, 5810, 5790, }, /* LAT: 90 */ { 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, }, }; -static constexpr float WMM_STRENGTH_MIN_GS = 22.3; // latitude: -30, longitude: -60 +static constexpr float WMM_STRENGTH_MIN_GS = 22.2; // latitude: -30, longitude: -60 static constexpr float WMM_STRENGTH_MAX_GS = 66.9; // latitude: -60, longitude: 140 static constexpr float WMM_STRENGTH_MEAN_GS = 46.4; static constexpr float WMM_STRENGTH_MEDIAN_GS = 48.8; diff --git a/src/lib/world_magnetic_model/test_geo_lookup.cpp b/src/lib/world_magnetic_model/test_geo_lookup.cpp index 26760fe6a5..3d216c3378 100644 --- a/src/lib/world_magnetic_model/test_geo_lookup.cpp +++ b/src/lib/world_magnetic_model/test_geo_lookup.cpp @@ -85,7 +85,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-50, 30), -41.4, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 35), -44.8, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 40), -48.0, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 45), -50.7, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 45), -50.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 50), -53.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 55), -55.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 60), -56.5, 0.41 + 1.0); @@ -135,7 +135,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-45, -85), 18.3, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -80), 15.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -75), 11.3, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -70), 7.3, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -70), 7.2, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -65), 3.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -60), -1.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -55), -5.0, 0.43 + 1.0); @@ -159,7 +159,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-45, 35), -43.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 40), -45.9, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 45), -48.2, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 50), -49.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 50), -50.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 55), -51.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 60), -51.9, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 65), -51.9, 0.41 + 1.0); @@ -183,8 +183,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-45, 155), 20.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 160), 22.6, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 165), 24.1, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 170), 25.2, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 175), 26.0, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 170), 25.3, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 175), 26.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 180), 26.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -180), 22.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -175), 23.1, 0.35 + 1.0); @@ -219,7 +219,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-40, -30), -19.7, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -25), -20.3, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -20), -20.7, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -15), -20.8, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -15), -20.8, 0.60 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -10), -21.0, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -5), -21.3, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 0), -22.0, 0.63 + 1.0); @@ -230,7 +230,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-40, 25), -34.3, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 30), -37.4, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 35), -40.1, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 40), -42.4, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 40), -42.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 45), -44.3, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 50), -45.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 55), -46.1, 0.41 + 1.0); @@ -272,7 +272,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-35, -130), 19.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -125), 19.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -120), 19.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -115), 19.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -115), 19.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -110), 19.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -105), 19.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -100), 18.4, 0.35 + 1.0); @@ -283,7 +283,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-35, -75), 5.5, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -70), 1.0, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -65), -3.7, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.2, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.3, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -55), -12.3, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -50), -15.7, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -45), -18.3, 0.46 + 1.0); @@ -294,11 +294,11 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-35, -20), -23.2, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -15), -23.1, 0.60 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -10), -22.7, 0.62 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.3, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.2, 0.64 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 0), -22.0, 0.66 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 5), -22.3, 0.66 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 10), -23.3, 0.64 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 15), -25.1, 0.61 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 15), -25.2, 0.61 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 20), -27.6, 0.58 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 25), -30.3, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 30), -32.9, 0.50 + 1.0); @@ -330,7 +330,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-35, 160), 16.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 165), 17.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 170), 18.4, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 175), 19.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 175), 19.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 180), 19.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -180), 17.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -175), 17.5, 0.33 + 1.0); @@ -355,16 +355,16 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-30, -80), 6.8, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -75), 2.7, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -70), -1.9, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -65), -6.6, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -65), -6.7, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -60), -11.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -55), -15.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -55), -15.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -50), -18.2, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -45), -20.7, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -40), -22.4, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -35), -23.5, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -30), -24.3, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -25), -24.7, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.7, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.7, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.2, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -10), -23.4, 0.60 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -22.1, 0.62 + 1.0); @@ -427,7 +427,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, -85), 7.7, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -80), 4.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -75), 0.1, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -70), -4.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -70), -4.5, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -65), -9.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -60), -13.4, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -55), -17.1, 0.40 + 1.0); @@ -448,7 +448,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, 20), -15.3, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 25), -16.7, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 30), -18.6, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 35), -20.6, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 35), -20.7, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 40), -22.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 45), -23.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 50), -24.7, 0.39 + 1.0); @@ -499,11 +499,11 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-20, -90), 8.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -85), 5.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -80), 2.1, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -75), -2.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -75), -2.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -70), -6.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -65), -10.9, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -60), -15.0, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -55), -18.3, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -55), -18.4, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -50), -21.0, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -45), -22.8, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -40), -23.9, 0.41 + 1.0); @@ -512,7 +512,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-20, -25), -24.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -20), -23.2, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -15), -21.9, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -20.0, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -19.9, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -5), -17.6, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 0), -15.0, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 5), -12.5, 0.51 + 1.0); @@ -583,7 +583,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, -35), -23.6, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -30), -23.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -25), -22.2, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -20), -20.8, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -20), -20.7, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -15), -18.9, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -10), -16.7, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -5), -14.1, 0.44 + 1.0); @@ -595,14 +595,14 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, 25), -4.7, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 30), -5.6, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 35), -7.2, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 40), -8.9, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 40), -9.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 45), -10.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 50), -11.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 55), -12.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 60), -12.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 65), -12.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 70), -11.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 75), -10.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 75), -10.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 80), -8.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 85), -6.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 90), -5.0, 0.31 + 1.0); @@ -661,7 +661,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-10, -10), -13.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -5), -10.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 0), -8.4, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 5), -6.2, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 5), -6.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 10), -4.1, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 15), -2.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 20), -1.6, 0.37 + 1.0); @@ -702,7 +702,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-5, -170), 10.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -165), 10.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -160), 10.1, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -155), 10.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -155), 10.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -150), 10.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -145), 10.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -140), 9.9, 0.32 + 1.0); @@ -734,7 +734,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-5, -10), -10.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -5), -8.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 0), -6.0, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 5), -4.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 5), -4.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 10), -2.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 15), -0.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 20), 0.2, 0.34 + 1.0); @@ -846,7 +846,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, -180), 9.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -175), 9.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -170), 9.5, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -165), 9.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -165), 9.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -160), 9.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -155), 9.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -150), 9.1, 0.32 + 1.0); @@ -856,7 +856,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, -130), 8.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -125), 8.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -120), 8.0, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -115), 7.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -115), 7.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -110), 7.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -105), 6.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -100), 5.4, 0.33 + 1.0); @@ -871,15 +871,15 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, -55), -17.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -50), -18.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -45), -18.8, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -40), -18.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -40), -18.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -35), -16.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -30), -14.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -25), -12.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -20), -10.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -15), -7.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -10), -5.9, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -5), -4.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 0), -2.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -5), -4.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 0), -2.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 5), -1.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 10), -0.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 15), 1.0, 0.31 + 1.0); @@ -893,7 +893,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, 55), -1.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 60), -2.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 65), -2.4, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 70), -2.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 70), -2.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 75), -2.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 80), -2.4, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 85), -2.0, 0.29 + 1.0); @@ -1003,8 +1003,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(15, -125), 9.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -120), 8.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -115), 8.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -110), 7.2, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -105), 6.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -110), 7.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -105), 6.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -100), 4.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -95), 2.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -90), 0.1, 0.33 + 1.0); @@ -1046,7 +1046,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(15, 90), -1.0, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 95), -0.9, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 100), -0.9, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 105), -1.1, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 105), -1.2, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 110), -1.6, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 115), -2.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 120), -2.7, 0.30 + 1.0); @@ -1124,7 +1124,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(20, 115), -3.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 120), -3.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 125), -4.1, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 130), -4.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 130), -4.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 135), -3.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 140), -2.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 145), -1.5, 0.31 + 1.0); @@ -1163,14 +1163,14 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(25, -55), -15.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -50), -15.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -45), -14.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -40), -13.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -40), -12.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -35), -11.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -30), -9.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -25), -7.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -20), -5.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -15), -3.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -10), -2.1, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -5), -0.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -5), -0.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 0), 0.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 5), 1.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 10), 2.2, 0.31 + 1.0); @@ -1236,7 +1236,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, -55), -15.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -50), -14.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -45), -13.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -40), -12.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -40), -12.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -35), -10.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -30), -9.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -25), -7.2, 0.32 + 1.0); @@ -1273,13 +1273,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, 130), -6.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 135), -6.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 140), -5.9, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 145), -4.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 145), -4.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 150), -3.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 155), -1.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 160), -0.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 165), 1.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 170), 3.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 175), 5.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 175), 4.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 180), 6.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -180), 5.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -175), 6.7, 0.34 + 1.0); @@ -1297,7 +1297,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, -115), 10.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -110), 9.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -105), 7.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -100), 4.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -100), 4.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -95), 1.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -90), -1.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -85), -5.2, 0.36 + 1.0); @@ -1309,7 +1309,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, -55), -15.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -50), -14.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -45), -13.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -40), -12.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -40), -12.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -35), -10.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -30), -8.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -25), -7.0, 0.33 + 1.0); @@ -1317,7 +1317,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, -15), -3.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -10), -1.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -5), -0.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 0), 1.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 0), 1.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 5), 2.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 10), 3.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 15), 3.7, 0.33 + 1.0); @@ -1390,7 +1390,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, -15), -3.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -10), -1.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -5), -0.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 0), 1.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 0), 1.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 5), 2.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 10), 3.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 15), 4.1, 0.34 + 1.0); @@ -1429,10 +1429,10 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, 180), 4.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -180), 3.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -175), 5.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -170), 7.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -170), 7.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -165), 8.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -160), 10.6, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -155), 12.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -155), 12.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -150), 13.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -145), 14.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -140), 15.1, 0.37 + 1.0); @@ -1445,7 +1445,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, -105), 8.2, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -100), 5.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -95), 1.3, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -90), -2.6, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -90), -2.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -85), -6.6, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -80), -10.1, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -75), -13.0, 0.40 + 1.0); @@ -1477,7 +1477,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, 55), 8.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 60), 8.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 65), 7.8, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 70), 7.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 70), 7.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 75), 6.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 80), 5.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 85), 3.6, 0.35 + 1.0); @@ -1485,7 +1485,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, 95), -0.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 100), -2.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 105), -4.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 110), -6.4, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 110), -6.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 115), -8.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 120), -9.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 125), -10.9, 0.34 + 1.0); @@ -1576,7 +1576,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, -180), 1.9, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -175), 4.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -170), 6.4, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -165), 8.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -165), 8.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -160), 10.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -155), 12.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -150), 14.4, 0.40 + 1.0); @@ -1601,7 +1601,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, -55), -20.0, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -50), -19.1, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -45), -17.8, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.2, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -35), -14.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -30), -12.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -25), -10.1, 0.41 + 1.0); @@ -1673,8 +1673,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(60, -60), -23.2, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -55), -22.7, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -50), -21.7, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.3, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.5, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.2, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.4, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -35), -16.4, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -30), -14.2, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.9, 0.45 + 1.0); @@ -1682,7 +1682,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(60, -15), -7.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -10), -4.7, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -5), -2.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 0), -0.1, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 0), -0.0, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 5), 2.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 10), 4.2, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 15), 6.3, 0.46 + 1.0); @@ -1810,7 +1810,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-45, -125), -57.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -120), -56.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -115), -55.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -110), -53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -110), -53.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -105), -52.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -100), -50.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -95), -49.2, 0.21 + 1.2); @@ -1828,7 +1828,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-45, -35), -56.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -30), -58.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -25), -60.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -20), -61.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -20), -61.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -15), -62.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -10), -63.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -5), -63.3, 0.21 + 1.2); @@ -1844,7 +1844,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-45, 45), -61.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 50), -62.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 55), -64.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 60), -65.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 60), -65.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 65), -67.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 70), -69.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 75), -70.7, 0.21 + 1.2); @@ -1971,7 +1971,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-35, -50), -46.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -45), -49.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -40), -52.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -30), -57.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -25), -60.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -20), -62.4, 0.21 + 1.2); @@ -1991,7 +1991,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-35, 50), -62.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 55), -63.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 60), -64.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 65), -65.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 65), -65.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 70), -66.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 75), -67.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 80), -68.6, 0.21 + 1.2); @@ -2006,7 +2006,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-35, 125), -68.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 130), -67.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 135), -67.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 140), -66.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 140), -66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 145), -66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 150), -65.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 155), -64.9, 0.21 + 1.2); @@ -2056,7 +2056,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-30, 10), -67.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 15), -66.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 20), -65.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 25), -63.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 25), -63.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 30), -62.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 35), -61.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 40), -60.5, 0.21 + 1.2); @@ -2113,17 +2113,17 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-25, -70), -24.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -65), -27.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -50), -38.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -45), -42.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -40), -46.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -30), -53.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -25), -56.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -15), -61.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -10), -63.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -5), -65.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -5), -65.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 0), -66.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 5), -66.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 10), -66.3, 0.21 + 1.2); @@ -2185,9 +2185,9 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, -75), -15.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -70), -17.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -65), -20.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -23.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -23.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -55), -28.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -45), -37.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -40), -42.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -35), -46.5, 0.21 + 1.2); @@ -2256,14 +2256,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-15, -85), -7.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -80), -6.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -75), -7.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -70), -8.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -70), -8.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -65), -11.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -50), -26.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -31.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -31.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -36.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -25), -48.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -20), -51.6, 0.21 + 1.2); @@ -2276,7 +2276,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-15, 15), -56.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 20), -55.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 25), -53.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 30), -51.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 30), -51.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 35), -49.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 40), -47.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 45), -46.8, 0.21 + 1.2); @@ -2333,8 +2333,8 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -7.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -13.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -18.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -18.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -40), -30.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -35), -35.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -30), -39.2, 0.21 + 1.2); @@ -2358,7 +2358,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, 60), -37.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 65), -37.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 70), -38.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 75), -38.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 75), -38.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 80), -38.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 85), -38.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 90), -38.9, 0.21 + 1.2); @@ -2387,13 +2387,13 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, -160), -9.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -155), -8.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -150), -6.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -145), -5.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -145), -5.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -140), -4.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -135), -3.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -130), -1.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -125), -0.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -120), 0.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -115), 1.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -115), 1.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -110), 3.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -105), 4.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -100), 6.5, 0.21 + 1.2); @@ -2402,11 +2402,11 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, -85), 11.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -80), 11.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -75), 11.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 9.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 8.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -65), 5.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -60), 1.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -55), -4.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -45), -16.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -40), -22.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -35), -27.1, 0.21 + 1.2); @@ -2424,7 +2424,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, 25), -35.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 30), -33.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 35), -31.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 40), -29.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 40), -29.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 45), -28.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 50), -27.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 55), -27.4, 0.21 + 1.2); @@ -2432,7 +2432,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, 65), -27.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 70), -27.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 75), -28.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 80), -28.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 80), -28.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 85), -28.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 90), -28.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 95), -28.5, 0.21 + 1.2); @@ -2474,7 +2474,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(0, -90), 19.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -85), 20.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -80), 20.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -75), 19.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -75), 19.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -70), 17.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -65), 14.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 10.0, 0.21 + 1.2); @@ -2509,8 +2509,8 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(0, 85), -17.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 90), -17.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 95), -17.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 100), -16.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 105), -16.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 100), -16.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 105), -16.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 110), -15.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 115), -15.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 120), -15.2, 0.21 + 1.2); @@ -2570,7 +2570,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(5, 25), -12.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 30), -10.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 35), -8.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 40), -6.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 40), -6.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 45), -5.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 50), -4.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 55), -3.9, 0.21 + 1.2); @@ -2582,7 +2582,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(5, 85), -5.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 90), -5.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 95), -5.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 100), -4.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 100), -4.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 105), -4.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 110), -3.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 115), -3.7, 0.21 + 1.2); @@ -2628,10 +2628,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(10, -50), 17.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -30), 0.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -25), -2.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -20), -4.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -25), -2.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -20), -4.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -15), -5.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -10), -5.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -5), -5.5, 0.21 + 1.2); @@ -2647,17 +2647,17 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(10, 45), 7.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 50), 8.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 55), 8.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 60), 8.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 60), 8.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 65), 8.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 70), 8.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 75), 7.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 80), 7.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 85), 7.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 90), 7.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 95), 7.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 95), 7.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 100), 7.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 105), 7.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 110), 7.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 110), 7.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 115), 7.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 120), 7.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 125), 6.6, 0.21 + 1.2); @@ -2700,7 +2700,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(15, -55), 30.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -50), 26.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 22.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 18.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 18.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -35), 14.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -30), 11.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -25), 9.4, 0.21 + 1.2); @@ -2788,24 +2788,24 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(20, 20), 23.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 25), 24.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 30), 26.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 35), 27.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 35), 27.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 40), 28.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 45), 29.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 50), 30.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 55), 30.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 60), 30.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 65), 30.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 65), 30.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 70), 30.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 75), 30.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 80), 30.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 85), 29.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 85), 30.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 90), 29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 95), 29.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 100), 29.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 105), 29.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 110), 29.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 115), 29.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 120), 28.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 120), 28.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 125), 27.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 130), 26.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 135), 25.6, 0.21 + 1.2); @@ -2842,14 +2842,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(25, -75), 52.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -70), 50.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -65), 48.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -60), 46.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -55), 44.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -50), 41.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -60), 46.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -55), 43.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -50), 41.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -45), 38.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -40), 36.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -35), 34.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -30), 32.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -25), 31.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -25), 31.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -20), 30.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -15), 29.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -10), 29.9, 0.21 + 1.2); @@ -2862,14 +2862,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(25, 25), 34.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 30), 36.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 35), 37.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 40), 38.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 40), 38.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 45), 38.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 50), 39.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 55), 39.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 60), 39.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 65), 39.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 70), 39.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 75), 39.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 75), 39.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 80), 39.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 85), 39.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 90), 39.4, 0.21 + 1.2); @@ -2929,7 +2929,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(30, -5), 39.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 0), 40.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 5), 40.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 10), 41.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 10), 41.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 15), 42.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 20), 42.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 25), 43.7, 0.21 + 1.2); @@ -3008,7 +3008,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(35, 25), 51.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 30), 51.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 35), 52.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 40), 53.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 40), 53.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 45), 53.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 50), 54.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 55), 54.2, 0.21 + 1.2); @@ -3067,7 +3067,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(40, -45), 57.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -40), 56.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -35), 55.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -30), 55.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -30), 55.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -25), 54.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -20), 54.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -15), 54.1, 0.21 + 1.2); @@ -3244,7 +3244,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(50, 110), 70.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 115), 69.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 120), 68.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 125), 67.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 125), 67.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 130), 66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 135), 65.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 140), 64.9, 0.21 + 1.2); @@ -3319,7 +3319,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(55, 120), 72.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 125), 72.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 130), 71.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 135), 70.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 135), 70.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 140), 69.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 145), 68.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 150), 67.9, 0.21 + 1.2); @@ -3348,7 +3348,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(60, -100), 80.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -95), 80.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -90), 80.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -85), 80.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -85), 80.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -80), 80.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -75), 79.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -70), 78.5, 0.21 + 1.2); @@ -3370,7 +3370,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(60, 10), 72.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 15), 73.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 20), 73.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 25), 73.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 25), 73.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 30), 73.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 35), 74.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 40), 74.4, 0.21 + 1.2); @@ -3406,288 +3406,288 @@ TEST(GeoLookupTest, inclination) TEST(GeoLookupTest, strength) { - EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58402, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57253, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56090, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54924, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53758, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52598, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51442, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50283, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49110, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47907, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46656, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45340, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43947, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42470, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40913, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39291, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37627, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35954, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34314, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32748, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31302, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 30014, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28913, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 28014, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27312, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26788, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26408, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26134, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25924, 145 + 259); - EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25746, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25575, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25401, 145 + 254); - EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25229, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25073, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24959, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24924, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 25012, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25272, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25750, 145 + 258); - EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26488, 145 + 265); - EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27512, 145 + 275); + EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58400, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57251, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56088, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54921, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53756, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52596, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51439, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50280, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49107, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47904, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46653, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45337, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43943, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42466, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40909, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39287, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37623, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35951, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34310, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32745, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31299, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 30012, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28911, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 28011, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27309, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26786, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26407, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26132, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25923, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25744, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25573, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25399, 145 + 254); + EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25227, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25070, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24956, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24922, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 25010, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25270, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25748, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26486, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27511, 145 + 275); EXPECT_NEAR(get_mag_strength_tesla(-50, 25) * 1e9, 28833, 145 + 288); EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30445, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32324, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34434, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36732, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39166, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41686, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44245, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46797, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49304, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51732, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54049, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56224, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58228, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60032, 145 + 600); - EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61610, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62942, 145 + 629); - EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64016, 145 + 640); - EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64830, 145 + 648); - EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65387, 145 + 654); - EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65699, 145 + 657); - EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65784, 145 + 658); + EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32325, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34436, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36734, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39168, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41689, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44248, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46800, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49307, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51734, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54051, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56226, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58230, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60034, 145 + 600); + EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61612, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62944, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64018, 145 + 640); + EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64831, 145 + 648); + EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65388, 145 + 654); + EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65700, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65785, 145 + 658); EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65660, 145 + 657); EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65347, 145 + 653); EXPECT_NEAR(get_mag_strength_tesla(-50, 145) * 1e9, 64865, 145 + 649); - EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64235, 145 + 642); - EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63475, 145 + 635); - EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62605, 145 + 626); - EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61645, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60613, 145 + 606); - EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59526, 145 + 595); - EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58402, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56247, 145 + 956); - EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55043, 145 + 936); - EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53831, 145 + 915); - EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52619, 145 + 895); - EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51414, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50220, 145 + 854); - EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49038, 145 + 834); - EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47864, 145 + 814); - EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46688, 145 + 794); - EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45492, 145 + 773); - EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44258, 145 + 752); - EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42965, 145 + 730); - EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41597, 145 + 707); - EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40144, 145 + 682); - EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38607, 145 + 656); - EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36994, 145 + 629); - EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35328, 145 + 601); - EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33642, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31979, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30392, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28934, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27657, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26599, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25782, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25201, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24830, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24622, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24524, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24481, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24451, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24404, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24326, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24216, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24087, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23962, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23878, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23884, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24038, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24406, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25047, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 26008, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27311, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28954, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30911, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33136, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35572, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38152, 145 + 649); - EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40811, 145 + 694); - EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43488, 145 + 739); - EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46130, 145 + 784); - EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48694, 145 + 828); - EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51147, 145 + 869); - EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53459, 145 + 909); - EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55603, 145 + 945); - EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57554, 145 + 978); - EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59283, 145 + 1008); - EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60770, 145 + 1033); - EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 61998, 145 + 1054); - EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62960, 145 + 1070); - EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63658, 145 + 1082); - EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64105, 145 + 1090); - EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64314, 145 + 1093); - EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64305, 145 + 1093); - EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64097, 145 + 1090); - EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63708, 145 + 1083); + EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64234, 145 + 642); + EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63474, 145 + 635); + EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62604, 145 + 626); + EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61644, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60612, 145 + 606); + EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59525, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58400, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56245, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55042, 145 + 936); + EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53829, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52617, 145 + 894); + EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51411, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50217, 145 + 854); + EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49035, 145 + 834); + EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47862, 145 + 814); + EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46685, 145 + 794); + EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45489, 145 + 773); + EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44255, 145 + 752); + EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42962, 145 + 730); + EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41594, 145 + 707); + EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40141, 145 + 682); + EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38603, 145 + 656); + EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36990, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35324, 145 + 601); + EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33638, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31976, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30389, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28931, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27654, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26597, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25780, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25199, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24828, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24621, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24523, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24480, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24449, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24402, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24323, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24214, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24084, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23960, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23876, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23881, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24035, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24403, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25045, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 26007, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27310, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28955, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30912, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33138, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35574, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38155, 145 + 649); + EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40814, 145 + 694); + EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43491, 145 + 739); + EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46133, 145 + 784); + EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48697, 145 + 828); + EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51149, 145 + 870); + EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53461, 145 + 909); + EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55606, 145 + 945); + EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57556, 145 + 978); + EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59285, 145 + 1008); + EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60772, 145 + 1033); + EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 62000, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62961, 145 + 1070); + EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63660, 145 + 1082); + EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64106, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64315, 145 + 1093); + EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64306, 145 + 1093); + EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64098, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63709, 145 + 1083); EXPECT_NEAR(get_mag_strength_tesla(-45, 145) * 1e9, 63157, 145 + 1074); EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62460, 145 + 1062); - EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61636, 145 + 1048); - EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60701, 145 + 1032); + EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61635, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60700, 145 + 1032); EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59675, 145 + 1014); - EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58579, 145 + 996); - EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57430, 145 + 976); - EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56247, 145 + 956); - EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53906, 145 + 916); - EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52674, 145 + 895); - EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51437, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50203, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48977, 145 + 833); - EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47766, 145 + 812); - EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46571, 145 + 792); - EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45392, 145 + 772); - EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44221, 145 + 752); - EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43044, 145 + 732); - EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41844, 145 + 711); - EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40600, 145 + 690); - EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39294, 145 + 668); - EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37913, 145 + 645); - EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36452, 145 + 620); - EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34913, 145 + 594); - EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33312, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31677, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30054, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28499, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27075, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25846, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24862, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24150, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23706, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23494, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23457, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23529, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23647, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23764, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23849, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23889, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23880, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23828, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23745, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23658, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23612, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23670, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23909, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24412, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25246, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58578, 145 + 996); + EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57429, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56245, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53904, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52673, 145 + 895); + EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51436, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50201, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48975, 145 + 833); + EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47763, 145 + 812); + EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46568, 145 + 792); + EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45389, 145 + 772); + EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44218, 145 + 752); + EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43042, 145 + 732); + EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41841, 145 + 711); + EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40597, 145 + 690); + EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39291, 145 + 668); + EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37910, 145 + 644); + EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36449, 145 + 620); + EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34910, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33308, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31674, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30051, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28496, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27072, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25844, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24860, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24148, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23704, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23493, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23456, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23528, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23646, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23762, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23847, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23887, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23878, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23825, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23742, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23655, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23609, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23667, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23906, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24409, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25244, 145 + 429); EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26454, 145 + 450); EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28048, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30003, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32268, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34773, 145 + 591); - EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37436, 145 + 636); - EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40174, 145 + 683); - EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42912, 145 + 729); - EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45584, 145 + 775); - EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48144, 145 + 818); - EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50556, 145 + 859); - EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52796, 145 + 898); - EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54839, 145 + 932); - EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56663, 145 + 963); - EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58248, 145 + 990); - EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59576, 145 + 1013); - EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60640, 145 + 1031); - EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61443, 145 + 1045); - EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 61996, 145 + 1054); - EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62317, 145 + 1059); - EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62424, 145 + 1061); + EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30004, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32270, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34776, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37439, 145 + 636); + EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40178, 145 + 683); + EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42915, 145 + 730); + EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45588, 145 + 775); + EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48147, 145 + 819); + EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50560, 145 + 860); + EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52798, 145 + 898); + EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54841, 145 + 932); + EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56666, 145 + 963); + EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58250, 145 + 990); + EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59578, 145 + 1013); + EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60642, 145 + 1031); + EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61444, 145 + 1045); + EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 61997, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62318, 145 + 1059); + EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62425, 145 + 1061); EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62336, 145 + 1060); - EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62067, 145 + 1055); + EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62068, 145 + 1055); EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61632, 145 + 1048); EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61043, 145 + 1038); EXPECT_NEAR(get_mag_strength_tesla(-40, 150) * 1e9, 60314, 145 + 1025); EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59458, 145 + 1011); - EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58492, 145 + 994); - EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57433, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58491, 145 + 994); + EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57432, 145 + 976); EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56302, 145 + 957); - EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55120, 145 + 937); - EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53906, 145 + 916); - EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51407, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50173, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48937, 145 + 832); - EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47704, 145 + 811); - EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46480, 145 + 790); - EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45270, 145 + 770); - EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44077, 145 + 749); - EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42905, 145 + 729); - EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41750, 145 + 710); - EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40603, 145 + 690); - EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39451, 145 + 671); - EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38275, 145 + 651); - EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37060, 145 + 630); - EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35790, 145 + 608); - EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34452, 145 + 586); - EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33041, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31562, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30038, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28510, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27037, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25692, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24546, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23659, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23062, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22750, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22682, 145 + 386); - EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22791, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 23003, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23255, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23502, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23721, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23900, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24034, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24118, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24149, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24136, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24106, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24118, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24253, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24606, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25268, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26310, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27761, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29614, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31820, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34302, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 36966, 145 + 628); - EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39712, 145 + 675); - EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42446, 145 + 722); - EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45092, 145 + 767); - EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47594, 145 + 809); - EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49917, 145 + 849); - EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52034, 145 + 885); - EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53925, 145 + 917); - EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55570, 145 + 945); - EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56954, 145 + 968); - EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58068, 145 + 987); - EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58920, 145 + 1002); - EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59526, 145 + 1012); - EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59910, 145 + 1018); - EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60099, 145 + 1022); - EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60110, 145 + 1022); - EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59958, 145 + 1019); + EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55119, 145 + 937); + EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53904, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51406, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50172, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48936, 145 + 832); + EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47703, 145 + 811); + EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46479, 145 + 790); + EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45268, 145 + 770); + EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44075, 145 + 749); + EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42903, 145 + 729); + EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41748, 145 + 710); + EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40601, 145 + 690); + EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39448, 145 + 671); + EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38273, 145 + 651); + EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37058, 145 + 630); + EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35787, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34449, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33038, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31559, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30035, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28507, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27034, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25689, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24544, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23657, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23061, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22749, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22681, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22790, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 23002, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23254, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23501, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23719, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23898, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24032, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24116, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24147, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24133, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24103, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24115, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24250, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24603, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25267, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26309, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27762, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29615, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31822, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34306, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 36970, 145 + 628); + EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39716, 145 + 675); + EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42450, 145 + 722); + EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45095, 145 + 767); + EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47598, 145 + 809); + EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49920, 145 + 849); + EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52037, 145 + 885); + EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53927, 145 + 917); + EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55572, 145 + 945); + EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56955, 145 + 968); + EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58070, 145 + 987); + EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58922, 145 + 1002); + EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59527, 145 + 1012); + EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59911, 145 + 1018); + EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60100, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60111, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59959, 145 + 1019); EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59650, 145 + 1014); EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59191, 145 + 1006); EXPECT_NEAR(get_mag_strength_tesla(-35, 145) * 1e9, 58589, 145 + 996); @@ -3695,145 +3695,145 @@ TEST(GeoLookupTest, strength) EXPECT_NEAR(get_mag_strength_tesla(-35, 155) * 1e9, 56992, 145 + 969); EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56020, 145 + 952); EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54955, 145 + 934); - EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53817, 145 + 915); - EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52627, 145 + 895); - EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51407, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48766, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53816, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52626, 145 + 895); + EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51406, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48765, 145 + 488); EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47556, 145 + 476); - EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46348, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45144, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43948, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42763, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41595, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40449, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39327, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38225, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37136, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36047, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34944, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33811, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32629, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31384, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30070, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28701, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27314, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25969, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24741, 145 + 247); - EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23708, 145 + 237); - EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22932, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22448, 145 + 224); - EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22251, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22295, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22511, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46346, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45143, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43946, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42761, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41593, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40447, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39325, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38223, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37133, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36044, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34942, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33808, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32627, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31382, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30068, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28699, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27312, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25966, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24739, 145 + 247); + EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23705, 145 + 237); + EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22930, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22447, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22250, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22294, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22510, 145 + 225); EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22825, 145 + 228); - EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23179, 145 + 232); - EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23537, 145 + 235); - EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23884, 145 + 239); - EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24216, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24526, 145 + 245); - EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24798, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 25008, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25142, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25206, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25241, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25322, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25547, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 26024, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26844, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23178, 145 + 232); + EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23536, 145 + 235); + EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23883, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24215, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24525, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24796, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 25006, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25139, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25203, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25238, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25318, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25544, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 26022, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26843, 145 + 268); EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28068, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29713, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31747, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34098, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36664, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39330, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 41988, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44548, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46947, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49142, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51105, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52813, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54247, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55395, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56262, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56869, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57254, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57458, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57514, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57440, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57239, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56907, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56440, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29714, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31750, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34102, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36668, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39335, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 41993, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44553, 145 + 446); + EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46951, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49145, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51108, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52815, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54249, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55397, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56263, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56870, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57255, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57459, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57515, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57441, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57240, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56908, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56441, 145 + 564); EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55840, 145 + 558); EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55112, 145 + 551); EXPECT_NEAR(get_mag_strength_tesla(-30, 155) * 1e9, 54264, 145 + 543); EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53309, 145 + 533); EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52260, 145 + 523); EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51138, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49966, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48766, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49965, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48765, 145 + 488); EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 46010, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44853, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43701, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42556, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41419, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40292, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39179, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38089, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37027, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35994, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34988, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 34003, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33030, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32051, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31044, 145 + 310); - EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29986, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28863, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27681, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26470, 145 + 265); - EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25288, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24208, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23307, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22646, 145 + 226); - EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22260, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22146, 145 + 221); + EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44852, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43700, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42555, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41418, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40290, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39177, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38087, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37025, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35992, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34986, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 34001, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33028, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32049, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31042, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29984, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28861, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27678, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26467, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25286, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24206, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23304, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22644, 145 + 226); + EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22259, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22145, 145 + 221); EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22262, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22545, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22544, 145 + 225); EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22926, 145 + 229); EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23356, 145 + 234); EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23808, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24277, 145 + 243); - EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24763, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25258, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25735, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26152, 145 + 262); - EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26473, 145 + 265); - EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26680, 145 + 267); - EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26795, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26876, 145 + 269); - EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 27015, 145 + 270); - EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27320, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27899, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24276, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24762, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25257, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25734, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26151, 145 + 262); + EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26470, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26678, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26792, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26873, 145 + 269); + EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 27012, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27318, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27897, 145 + 279); EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28840, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30193, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31956, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34070, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36436, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38931, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41435, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43848, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46097, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48134, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49923, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51437, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52650, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53552, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54159, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54512, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54672, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30194, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31958, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34073, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36440, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38935, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41439, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43852, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46101, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48137, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49926, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51439, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52651, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53553, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54160, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54513, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54673, 145 + 547); EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54699, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54632, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54633, 145 + 546); EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54486, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54249, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54250, 145 + 542); EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53904, 145 + 539); EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53437, 145 + 534); EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52844, 145 + 528); @@ -3841,441 +3841,441 @@ TEST(GeoLookupTest, strength) EXPECT_NEAR(get_mag_strength_tesla(-25, 155) * 1e9, 51309, 145 + 513); EXPECT_NEAR(get_mag_strength_tesla(-25, 160) * 1e9, 50386, 145 + 504); EXPECT_NEAR(get_mag_strength_tesla(-25, 165) * 1e9, 49375, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48293, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48292, 145 + 483); EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47163, 145 + 472); EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 46010, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43201, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42126, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41061, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 40008, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38965, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37930, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36911, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35914, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34947, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 34014, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33119, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32261, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31432, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30617, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29792, 145 + 298); - EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28930, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 28010, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27033, 145 + 270); - EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 26022, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 25027, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24114, 145 + 241); - EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23349, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22792, 145 + 228); - EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22476, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22405, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43200, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42125, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41060, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 40007, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38963, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37929, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36909, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35912, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34945, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 34013, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33118, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32259, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31430, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30615, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29790, 145 + 298); + EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28928, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 28008, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27030, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 26019, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 25025, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24111, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23347, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22790, 145 + 228); + EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22475, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22404, 145 + 224); EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22548, 145 + 225); EXPECT_NEAR(get_mag_strength_tesla(-20, -50) * 1e9, 22857, 145 + 229); EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23275, 145 + 233); EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23760, 145 + 238); EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24291, 145 + 243); EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24867, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25488, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25489, 145 + 255); EXPECT_NEAR(get_mag_strength_tesla(-20, -20) * 1e9, 26144, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26800, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26799, 145 + 268); EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27402, 145 + 274); - EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27900, 145 + 279); - EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28257, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28475, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28594, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28688, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28856, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29211, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27898, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28255, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28473, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28592, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28685, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28854, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29209, 145 + 292); EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29860, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30885, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32315, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34116, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36198, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38440, 145 + 384); - EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40717, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42921, 145 + 429); - EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 44977, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46828, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48433, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49752, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50753, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51423, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51786, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30886, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32317, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34119, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36202, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38444, 145 + 384); + EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40721, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42925, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 44980, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46832, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48436, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49754, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50754, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51424, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51787, 145 + 518); EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51904, 145 + 519); EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51858, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51725, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51726, 145 + 517); EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51553, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51346, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51347, 145 + 513); EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51083, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50729, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50730, 145 + 507); EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50263, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49678, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49679, 145 + 497); EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48985, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48193, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48194, 145 + 482); EXPECT_NEAR(get_mag_strength_tesla(-20, 160) * 1e9, 47314, 145 + 473); EXPECT_NEAR(get_mag_strength_tesla(-20, 165) * 1e9, 46358, 145 + 464); EXPECT_NEAR(get_mag_strength_tesla(-20, 170) * 1e9, 45338, 145 + 453); EXPECT_NEAR(get_mag_strength_tesla(-20, 175) * 1e9, 44278, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43201, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40445, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43200, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40444, 145 + 404); EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39482, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38537, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37608, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36691, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35787, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34898, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34035, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33202, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32408, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31655, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30945, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30275, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29630, 145 + 296); - EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28989, 145 + 290); - EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28323, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27610, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26845, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26044, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25244, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24496, 145 + 245); - EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23857, 145 + 239); - EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23376, 145 + 234); - EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23091, 145 + 231); - EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23014, 145 + 230); - EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23136, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38536, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37607, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36690, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35785, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34897, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34033, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33201, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32406, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31654, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30944, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30273, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29628, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28987, 145 + 290); + EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28321, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27608, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26843, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26041, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25241, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24494, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23854, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23374, 145 + 234); + EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23089, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23013, 145 + 230); + EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23135, 145 + 231); EXPECT_NEAR(get_mag_strength_tesla(-15, -50) * 1e9, 23426, 145 + 234); EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23847, 145 + 238); EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24362, 145 + 244); - EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24951, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25606, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26323, 145 + 263); - EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27085, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24952, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25607, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26324, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27086, 145 + 271); EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27856, 145 + 279); EXPECT_NEAR(get_mag_strength_tesla(-15, -10) * 1e9, 28580, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29201, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29675, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29988, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30160, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30243, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30320, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30499, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30895, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29200, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29674, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29986, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30157, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30240, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30318, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30497, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30894, 145 + 309); EXPECT_NEAR(get_mag_strength_tesla(-15, 35) * 1e9, 31611, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32703, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34162, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35915, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37847, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39838, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41784, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43606, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45249, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46660, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47792, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48602, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49073, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32704, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34164, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35918, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37851, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39842, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41788, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43610, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45252, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46663, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47794, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48603, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49074, 145 + 491); EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49235, 145 + 492); EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49160, 145 + 492); EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48948, 145 + 489); - EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48687, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48688, 145 + 487); EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48429, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48172, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47883, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47517, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47046, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46466, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45790, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48173, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47884, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47518, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47047, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46467, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45791, 145 + 458); EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45033, 145 + 450); EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44206, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43318, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43319, 145 + 433); EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42382, 145 + 424); EXPECT_NEAR(get_mag_strength_tesla(-15, 175) * 1e9, 41417, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40445, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40444, 145 + 404); EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37896, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37076, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36278, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35500, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34738, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33993, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33269, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32574, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31913, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31290, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30710, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30172, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29673, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29204, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28745, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28272, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27763, 145 + 278); - EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27206, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26606, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25988, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25386, 145 + 254); - EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24843, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24403, 145 + 244); - EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24104, 145 + 241); - EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23973, 145 + 240); + EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37075, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36277, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35499, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34737, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33992, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33268, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32572, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31911, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31289, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30708, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30170, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29671, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29202, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28743, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28270, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27760, 145 + 278); + EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27203, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26604, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25985, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25383, 145 + 254); + EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24841, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24401, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24102, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23971, 145 + 240); EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24020, 145 + 240); - EXPECT_NEAR(get_mag_strength_tesla(-10, -50) * 1e9, 24243, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-10, -50) * 1e9, 24242, 145 + 242); EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24620, 145 + 246); EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25126, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25733, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26423, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27178, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27976, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28782, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29547, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25734, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26424, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27179, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27977, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28783, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29548, 145 + 295); EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30220, 145 + 302); EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30756, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31135, 145 + 311); - EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31358, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31458, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31497, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31568, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31786, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31134, 145 + 311); + EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31357, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31456, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31495, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31566, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31785, 145 + 318); EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32260, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33062, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34202, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35623, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37226, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38902, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40558, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42120, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43535, 145 + 435); - EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44748, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45703, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46349, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46667, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46684, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46478, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33063, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34203, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35625, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37228, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38905, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40561, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42124, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43538, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44750, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45704, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46350, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46668, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46685, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46479, 145 + 465); EXPECT_NEAR(get_mag_strength_tesla(-10, 110) * 1e9, 46154, 145 + 462); EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45804, 145 + 458); EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45477, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45171, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44846, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45172, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44847, 145 + 448); EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44455, 145 + 445); EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43968, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43384, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42720, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43385, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42721, 145 + 427); EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 41997, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41226, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41227, 145 + 412); EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40418, 145 + 404); EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39583, 145 + 396); EXPECT_NEAR(get_mag_strength_tesla(-10, 175) * 1e9, 38736, 145 + 387); EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37896, 145 + 379); EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35735, 145 + 357); EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35078, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34448, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33839, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33250, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32682, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32143, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31640, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31176, 145 + 312); - EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30753, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30368, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30020, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29705, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29417, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29140, 145 + 291); - EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28853, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28535, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28168, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27745, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27277, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26784, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26299, 145 + 263); - EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25861, 145 + 259); - EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25509, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25280, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25207, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25310, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25589, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34447, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33838, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33249, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32681, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32142, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31639, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31175, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30752, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30367, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30018, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29704, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29415, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29138, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28851, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28533, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28165, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27742, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27274, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26781, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26296, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25858, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25507, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25279, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25206, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25309, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25590, 145 + 256); EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26029, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26597, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27261, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 27991, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28757, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29526, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26598, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27262, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 27992, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28758, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29527, 145 + 295); EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30261, 145 + 303); EXPECT_NEAR(get_mag_strength_tesla(-5, -5) * 1e9, 30920, 145 + 309); EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31468, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31881, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32150, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32288, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32339, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32380, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32513, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31880, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32148, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32286, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32338, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32379, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32512, 145 + 325); EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32842, 145 + 328); EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33440, 145 + 334); - EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34322, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35448, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36739, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38103, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39464, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40762, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41944, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42958, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43747, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44258, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44469, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44403, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34323, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35450, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36741, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38106, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39467, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40764, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41946, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42960, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43748, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44260, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44470, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44404, 145 + 444); EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44131, 145 + 441); - EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43749, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43750, 145 + 437); EXPECT_NEAR(get_mag_strength_tesla(-5, 115) * 1e9, 43344, 145 + 433); EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42959, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42593, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42210, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42594, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42211, 145 + 422); EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41769, 145 + 418); EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41246, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40644, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39984, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40645, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39985, 145 + 400); EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39289, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38574, 145 + 386); - EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37851, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37129, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38575, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37852, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37130, 145 + 371); EXPECT_NEAR(get_mag_strength_tesla(-5, 175) * 1e9, 36420, 145 + 364); EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35735, 145 + 357); EXPECT_NEAR(get_mag_strength_tesla(0, -180) * 1e9, 34118, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33635, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33181, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33180, 145 + 332); EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32747, 145 + 327); EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32334, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31949, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31601, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31299, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31045, 145 + 310); - EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30835, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30660, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30515, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30394, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30290, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30193, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30083, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29937, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29730, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29446, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29083, 145 + 291); - EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28654, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28187, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27716, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27282, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26930, 145 + 269); - EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26705, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31948, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31600, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31298, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31044, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30834, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30659, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30514, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30392, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30288, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30191, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30081, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29934, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29727, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29443, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29080, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28651, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28184, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27714, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27280, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26928, 145 + 269); + EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26704, 145 + 267); EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26648, 145 + 266); EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26780, 145 + 268); EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27094, 145 + 271); - EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27559, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28135, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28781, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29461, 145 + 295); - EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30146, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30806, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27560, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28136, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28782, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29462, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30147, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30807, 145 + 308); EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31415, 145 + 314); EXPECT_NEAR(get_mag_strength_tesla(0, 0) * 1e9, 31944, 145 + 319); EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32370, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32679, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32874, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32678, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32873, 145 + 329); EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32982, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33066, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33207, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33065, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33206, 145 + 332); EXPECT_NEAR(get_mag_strength_tesla(0, 35) * 1e9, 33492, 145 + 335); EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33981, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34688, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35582, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36604, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37690, 145 + 377); - EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38781, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39830, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40793, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41620, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42259, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42662, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42806, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42707, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42421, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34689, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35583, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36606, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37693, 145 + 377); + EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38784, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39833, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40795, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41622, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42261, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42664, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42807, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42708, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42422, 145 + 424); EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42025, 145 + 420); EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41586, 145 + 416); EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41142, 145 + 411); EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40695, 145 + 407); EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40222, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39697, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39698, 145 + 397); EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39110, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38468, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38469, 145 + 385); EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37796, 145 + 378); EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37117, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36450, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36451, 145 + 365); EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35810, 145 + 358); EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35204, 145 + 352); EXPECT_NEAR(get_mag_strength_tesla(0, 175) * 1e9, 34639, 145 + 346); EXPECT_NEAR(get_mag_strength_tesla(0, 180) * 1e9, 34118, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(5, -180) * 1e9, 33142, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32826, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32825, 145 + 328); EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32539, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32273, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32028, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32272, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32027, 145 + 320); EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31816, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31654, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31550, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31506, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31512, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31553, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31617, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31694, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31777, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31856, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31911, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31914, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31834, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31647, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31345, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30938, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30452, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29921, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29388, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28902, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28517, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28287, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28243, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31653, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31549, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31505, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31511, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31552, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31616, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31692, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31776, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31854, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31909, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31911, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31830, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31643, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31342, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30935, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30449, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29918, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29385, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28899, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28516, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28285, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28242, 145 + 282); EXPECT_NEAR(get_mag_strength_tesla(5, -40) * 1e9, 28391, 145 + 284); - EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28706, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29145, 145 + 291); - EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29665, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28707, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29146, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29666, 145 + 297); EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30229, 145 + 302); EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30810, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31386, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31937, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32441, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31387, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31938, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32442, 145 + 324); EXPECT_NEAR(get_mag_strength_tesla(5, 5) * 1e9, 32876, 145 + 329); EXPECT_NEAR(get_mag_strength_tesla(5, 10) * 1e9, 33226, 145 + 332); EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33490, 145 + 335); EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33688, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33866, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33865, 145 + 339); EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34086, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34409, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34872, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35481, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36212, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37028, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37888, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38756, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39597, 145 + 396); - EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40373, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41042, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41556, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41876, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 41983, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41884, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41615, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41224, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40758, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34873, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35482, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36213, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37029, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37890, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38758, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39599, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40376, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41044, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41558, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41878, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 41984, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41885, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41616, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41225, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40759, 145 + 408); EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40243, 145 + 402); EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39688, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39090, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38445, 145 + 384); - EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37758, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37047, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36337, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39091, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38446, 145 + 384); + EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37759, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37048, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36338, 145 + 363); EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35653, 145 + 357); EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35016, 145 + 350); EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34441, 145 + 344); @@ -4284,71 +4284,71 @@ TEST(GeoLookupTest, strength) EXPECT_NEAR(get_mag_strength_tesla(5, 180) * 1e9, 33142, 145 + 331); EXPECT_NEAR(get_mag_strength_tesla(10, -180) * 1e9, 32824, 145 + 328); EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32647, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32507, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32389, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32296, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32244, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32253, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32336, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32492, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32707, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32959, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33230, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33506, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33776, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 34025, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34232, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34362, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34379, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34255, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33980, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33563, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 33030, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32422, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31782, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31167, 145 + 312); - EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30635, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30243, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30031, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32506, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32388, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32295, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32243, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32252, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32335, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32491, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32705, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32958, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33229, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33504, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33774, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 34023, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34229, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34358, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34375, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34252, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33976, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33559, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 33027, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32418, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31779, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31164, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30633, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30241, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30030, 145 + 300); EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 30008, 145 + 300); EXPECT_NEAR(get_mag_strength_tesla(10, -35) * 1e9, 30156, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30438, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30439, 145 + 304); EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30816, 145 + 308); EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31256, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31737, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31738, 145 + 317); EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32242, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32750, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33242, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33693, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32751, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33243, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33694, 145 + 337); EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34088, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34425, 145 + 344); EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34723, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35017, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35350, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35755, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36245, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36813, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37442, 145 + 374); - EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38113, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38810, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39514, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40203, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40844, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41397, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41824, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42091, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42183, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42100, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41857, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41474, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40973, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40372, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39687, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38935, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38137, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37316, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35018, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35351, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35756, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36246, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36814, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37444, 145 + 374); + EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38115, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38812, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39516, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40205, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40846, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41400, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41826, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42093, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42185, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42102, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41858, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41475, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40974, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40373, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39688, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38936, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38138, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37317, 145 + 373); EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36502, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35721, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35722, 145 + 357); EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 35001, 145 + 350); EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34363, 145 + 344); EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33823, 145 + 338); @@ -4357,217 +4357,217 @@ TEST(GeoLookupTest, strength) EXPECT_NEAR(get_mag_strength_tesla(10, 180) * 1e9, 32824, 145 + 328); EXPECT_NEAR(get_mag_strength_tesla(15, -180) * 1e9, 33126, 145 + 331); EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33048, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33020, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33019, 145 + 330); EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33024, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33064, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33156, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33322, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33576, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33915, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34323, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34773, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35241, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35706, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36154, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36564, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36908, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37147, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37242, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37162, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36894, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36450, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35860, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35166, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34424, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33694, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33038, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32515, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32164, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31995, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31994, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33063, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33155, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33320, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33574, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33914, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34321, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34771, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35239, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35704, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36152, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36561, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36905, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37144, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37239, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37158, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36890, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36446, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35856, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35163, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34421, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33690, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33036, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32513, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32162, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31994, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31993, 145 + 320); EXPECT_NEAR(get_mag_strength_tesla(15, -30) * 1e9, 32130, 145 + 321); EXPECT_NEAR(get_mag_strength_tesla(15, -25) * 1e9, 32373, 145 + 324); EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32701, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33097, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33548, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34034, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33098, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33549, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34035, 145 + 340); EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34528, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35003, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35442, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35847, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36235, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36636, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37076, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37566, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38100, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38660, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39235, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39819, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40413, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41017, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41615, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42178, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42670, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43055, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43304, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43403, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43343, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43122, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42737, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42192, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41496, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40672, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39754, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38784, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35004, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35443, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35848, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36236, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36637, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37078, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37568, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38101, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38662, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39237, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39821, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40416, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41020, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41618, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42181, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42673, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43057, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43307, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43406, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43346, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43124, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42739, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42193, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41497, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40673, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39755, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38785, 145 + 388); EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37806, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36858, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35975, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35184, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36859, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35976, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35185, 145 + 352); EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34508, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33962, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33963, 145 + 340); EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33557, 145 + 336); EXPECT_NEAR(get_mag_strength_tesla(15, 175) * 1e9, 33285, 145 + 333); EXPECT_NEAR(get_mag_strength_tesla(15, 180) * 1e9, 33126, 145 + 331); EXPECT_NEAR(get_mag_strength_tesla(20, -180) * 1e9, 33991, 145 + 340); EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33964, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34010, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34109, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34261, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34482, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34791, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35198, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35701, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36279, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36904, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37546, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38181, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38788, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39340, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39804, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40138, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40298, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40253, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39989, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39517, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38872, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38103, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37271, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36446, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35694, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35072, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34618, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34338, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34219, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34009, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34108, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34260, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34481, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34789, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35197, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35700, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36278, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36902, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37544, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38179, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38785, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39337, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39801, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40134, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40294, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40249, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39985, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39513, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38868, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38099, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37267, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36442, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35691, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35070, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34616, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34337, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34218, 145 + 342); EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34236, 145 + 342); EXPECT_NEAR(get_mag_strength_tesla(20, -25) * 1e9, 34368, 145 + 344); EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34602, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34929, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35339, 145 + 353); + EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34930, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35340, 145 + 353); EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35810, 145 + 358); EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36310, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36808, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37285, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37741, 145 + 377); - EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38195, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38672, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39186, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39736, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40305, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40872, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41426, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 41972, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42523, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43086, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43652, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44194, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44678, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45066, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45333, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45458, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45427, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45222, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44828, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44234, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43447, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42492, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41417, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40280, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39139, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38045, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36809, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37286, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37742, 145 + 377); + EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38196, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38673, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39187, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39738, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40307, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40874, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41428, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 41974, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42526, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43089, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43655, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44198, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44681, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45070, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45336, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45461, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45429, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45224, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44830, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44236, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43448, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42494, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41419, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40281, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39140, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38046, 145 + 380); EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37040, 145 + 370); EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36152, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35404, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34814, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35405, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34815, 145 + 348); EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34389, 145 + 344); EXPECT_NEAR(get_mag_strength_tesla(20, 175) * 1e9, 34122, 145 + 341); EXPECT_NEAR(get_mag_strength_tesla(20, 180) * 1e9, 33991, 145 + 340); EXPECT_NEAR(get_mag_strength_tesla(25, -180) * 1e9, 35368, 145 + 354); EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35349, 145 + 353); - EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35432, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35598, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35845, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36180, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36618, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37162, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37806, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38526, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39294, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40077, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40848, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41581, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42246, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42805, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43214, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43428, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43414, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43158, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42672, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41989, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41164, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40267, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39370, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38547, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37853, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37322, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36958, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36749, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35431, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35597, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35844, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36179, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36616, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37160, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37804, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38524, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39291, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40074, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40845, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41578, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42243, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42802, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43210, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43424, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43410, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43154, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42668, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41985, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41160, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40263, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39367, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38544, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37851, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37320, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36957, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36748, 145 + 367); EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36673, 145 + 367); EXPECT_NEAR(get_mag_strength_tesla(25, -25) * 1e9, 36716, 145 + 367); EXPECT_NEAR(get_mag_strength_tesla(25, -20) * 1e9, 36871, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37137, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37506, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37955, 145 + 380); - EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38451, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38959, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39456, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39942, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40431, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40944, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41492, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42069, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42656, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43233, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43793, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44345, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44904, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45480, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46065, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46635, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47154, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47583, 145 + 476); - EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47894, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48061, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48062, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47871, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47465, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46829, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45967, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44908, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43707, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42430, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41147, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39919, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38792, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37138, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37507, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37956, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38452, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38960, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39458, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39943, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40432, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40946, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41494, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42071, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42658, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43235, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43795, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44347, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44906, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45483, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46068, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46639, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47157, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47587, 145 + 476); + EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47897, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48064, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48064, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47873, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47468, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46831, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45969, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44910, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43708, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42431, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41148, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39920, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38793, 145 + 388); EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37800, 145 + 378); EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36965, 145 + 370); EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36303, 145 + 363); @@ -4575,514 +4575,514 @@ TEST(GeoLookupTest, strength) EXPECT_NEAR(get_mag_strength_tesla(25, 175) * 1e9, 35516, 145 + 355); EXPECT_NEAR(get_mag_strength_tesla(25, 180) * 1e9, 35368, 145 + 354); EXPECT_NEAR(get_mag_strength_tesla(30, -180) * 1e9, 37222, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37181, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37273, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37482, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37802, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38236, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38784, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39444, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40202, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 41032, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41903, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42786, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43649, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44465, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45202, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45820, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46276, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46526, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46537, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46295, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45808, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45112, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44261, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43327, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42388, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41516, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40767, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40172, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39737, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39450, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37180, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37272, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37481, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37801, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38234, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38782, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39442, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40199, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 41029, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41900, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42782, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43646, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44462, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45198, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45816, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46272, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46522, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46533, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46291, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45804, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45108, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44257, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43324, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42385, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41513, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40764, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40170, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39736, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39449, 145 + 394); EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39295, 145 + 393); EXPECT_NEAR(get_mag_strength_tesla(30, -25) * 1e9, 39261, 145 + 393); EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39347, 145 + 393); EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39552, 145 + 396); - EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39870, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40281, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40751, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41245, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41739, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42228, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42722, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43239, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43788, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44364, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44955, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45547, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46135, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46727, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47337, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 47969, 145 + 480); - EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48615, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49248, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49830, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50322, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50691, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50907, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50940, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50762, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50349, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49684, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48774, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47650, 145 + 476); - EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46368, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45000, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43621, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39871, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40282, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40753, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41247, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41741, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42230, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42724, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43241, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43790, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44367, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44958, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45549, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46137, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46730, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47340, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 47972, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48618, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49251, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49833, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50326, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50695, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50910, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50943, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50765, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50351, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49686, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48775, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47651, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46369, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45001, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43622, 145 + 436); EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42296, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41074, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 39993, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39076, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38337, 145 + 383); + EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41076, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 39994, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39077, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38338, 145 + 383); EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37785, 145 + 378); EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37417, 145 + 374); EXPECT_NEAR(get_mag_strength_tesla(30, 180) * 1e9, 37222, 145 + 372); EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39523, 145 + 395); EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39446, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39529, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39760, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40132, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40638, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41271, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 42016, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42853, 145 + 429); - EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43752, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44684, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45617, 145 + 456); - EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46522, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47370, 145 + 474); - EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48130, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48764, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49231, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49492, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49515, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49286, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48812, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48125, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47278, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46340, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45387, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44487, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43696, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43045, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42543, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42182, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39528, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39759, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40130, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40636, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41268, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 42013, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42850, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43749, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44681, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45614, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46518, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47367, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48126, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48760, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49227, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49488, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49511, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49281, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48807, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48120, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47274, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46337, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45384, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44485, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43694, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43044, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42542, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42181, 145 + 422); EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41951, 145 + 420); EXPECT_NEAR(get_mag_strength_tesla(35, -25) * 1e9, 41844, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41858, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 41996, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42250, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42602, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43021, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43474, 145 + 435); - EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43939, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44406, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44883, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45381, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45912, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46476, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47067, 145 + 471); - EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47681, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48314, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 48973, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49662, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50379, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51109, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51822, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52479, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53040, 145 + 530); - EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53468, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53728, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53789, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53624, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53209, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52534, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51608, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50465, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49160, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47764, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46349, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44983, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43716, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42585, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41613, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41859, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 41997, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42251, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42603, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43022, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43476, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43941, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44409, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44885, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45384, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45914, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46478, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47070, 145 + 471); + EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47684, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48317, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 48976, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49664, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50382, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51112, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51825, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52483, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53044, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53472, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53731, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53792, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53627, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53211, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52536, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51610, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50466, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49161, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47765, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46351, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44984, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43717, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42586, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41614, 145 + 416); EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40816, 145 + 408); EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40201, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39771, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39772, 145 + 398); EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39523, 145 + 395); EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42220, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42111, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42177, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42412, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42808, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43351, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 44026, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44812, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45682, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46603, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47545, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48476, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49370, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50199, 145 + 502); - EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50932, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51537, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51977, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52219, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52235, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 52013, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51559, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50900, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50084, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49172, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48233, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47329, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46513, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45815, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45250, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44818, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42110, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42176, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42411, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42806, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43349, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 44024, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44810, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45678, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46600, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47541, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48473, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49366, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50195, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50928, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51532, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51973, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52215, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52231, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 52009, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51555, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50896, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50080, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49169, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48230, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47327, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46511, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45814, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45249, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44817, 145 + 448); EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44512, 145 + 445); EXPECT_NEAR(get_mag_strength_tesla(40, -25) * 1e9, 44330, 145 + 443); EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44270, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44331, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44506, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44779, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45124, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45514, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45927, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46355, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46799, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47270, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47779, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48332, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48931, 145 + 489); - EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49576, 145 + 496); - EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50266, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51001, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51779, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52588, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53406, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54198, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54925, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55545, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56020, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56313, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56395, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56242, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55835, 145 + 558); - EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55172, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54266, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53152, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51882, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50523, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49143, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47802, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46550, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45423, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44442, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43623, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42976, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44332, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44507, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44781, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45126, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45516, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45929, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46357, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46801, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47273, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47782, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48335, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48934, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49578, 145 + 496); + EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50269, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51004, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51782, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52592, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53409, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54202, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54929, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55549, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56023, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56316, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56398, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56244, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55837, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55174, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54268, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53153, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51884, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50525, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49144, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47804, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46552, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45424, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44443, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43624, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42977, 145 + 430); EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42508, 145 + 425); EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42220, 145 + 422); EXPECT_NEAR(get_mag_strength_tesla(45, -180) * 1e9, 45210, 145 + 452); EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45082, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45131, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45352, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45738, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46274, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46939, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47709, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48553, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49438, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50334, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51212, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 52044, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52804, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53467, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 54004, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54386, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54584, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54577, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54355, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53924, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53308, 145 + 533); - EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52544, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51684, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50787, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49906, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49086, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48361, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47748, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45130, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45351, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45736, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46271, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46936, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47706, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48549, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49435, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50331, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51208, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 52040, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52800, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53463, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 54000, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54382, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54580, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54573, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54352, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53921, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53304, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52541, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51682, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50784, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49904, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49085, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48360, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47747, 145 + 477); EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47253, 145 + 473); EXPECT_NEAR(get_mag_strength_tesla(45, -30) * 1e9, 46878, 145 + 469); EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46622, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46484, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46463, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46550, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46732, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 46990, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47301, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47649, 145 + 476); - EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48026, 145 + 480); - EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48434, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48880, 145 + 489); - EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49374, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49926, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50541, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51223, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 51970, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52777, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53633, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54519, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55405, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56256, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57031, 145 + 570); - EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57688, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58189, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58501, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58597, 145 + 586); - EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58458, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58074, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57448, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56599, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55561, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54383, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53123, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51841, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50591, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49417, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48350, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47413, 145 + 474); - EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46621, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46485, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46464, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46552, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46734, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 46991, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47302, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47651, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48028, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48436, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48882, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49376, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49928, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50544, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51226, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 51973, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52780, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53636, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54522, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55409, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56260, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57034, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57691, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58192, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58503, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58599, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58460, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58076, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57450, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56600, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55562, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54384, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53124, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51842, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50592, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49418, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48352, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47415, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46622, 145 + 466); EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 45985, 145 + 460); EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45512, 145 + 455); EXPECT_NEAR(get_mag_strength_tesla(45, 180) * 1e9, 45210, 145 + 452); EXPECT_NEAR(get_mag_strength_tesla(50, -180) * 1e9, 48319, 145 + 483); EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48187, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48217, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48406, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48748, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49228, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49827, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50519, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51276, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52066, 145 + 521); - EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52861, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53634, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54358, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 55012, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55571, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 56013, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56314, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56453, 145 + 565); - EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56414, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56191, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55789, 145 + 558); - EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55226, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54533, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53749, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52920, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52090, 145 + 521); - EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51299, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50575, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49940, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48216, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48404, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48746, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49226, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49824, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50516, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51272, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52062, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52857, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53630, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54354, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 55008, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55568, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 56009, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56310, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56449, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56411, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56188, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55786, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55223, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54530, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53747, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52918, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52089, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51298, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50574, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49939, 145 + 499); EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49404, 145 + 494); EXPECT_NEAR(get_mag_strength_tesla(50, -30) * 1e9, 48975, 145 + 490); EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48656, 145 + 487); EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48446, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48343, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48342, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48432, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48599, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48828, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49109, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49436, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49810, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50237, 145 + 502); - EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50725, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51285, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51920, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52635, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53424, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54279, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55182, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56109, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57027, 145 + 570); - EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57900, 145 + 579); - EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58687, 145 + 587); - EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59350, 145 + 594); - EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59855, 145 + 599); - EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60171, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60278, 145 + 603); - EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60162, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59820, 145 + 598); - EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59260, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48344, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48343, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48433, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48600, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48830, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49111, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49438, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49812, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50239, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50728, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51287, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51923, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52638, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53427, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54282, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55185, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56112, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57030, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57903, 145 + 579); + EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58690, 145 + 587); + EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59353, 145 + 594); + EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59857, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60174, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60280, 145 + 603); + EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60163, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59821, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59261, 145 + 593); EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58504, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57584, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56546, 145 + 565); - EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55437, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54307, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53202, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52158, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51204, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50359, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49638, 145 + 496); - EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49052, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57585, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56547, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55438, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54309, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53203, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52160, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51205, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50360, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49639, 145 + 496); + EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49053, 145 + 491); EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48610, 145 + 486); EXPECT_NEAR(get_mag_strength_tesla(50, 180) * 1e9, 48319, 145 + 483); EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51311, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51181, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51186, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51326, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51594, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51979, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52463, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 53026, 145 + 530); - EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53642, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54286, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54933, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55559, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56141, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56659, 145 + 567); - EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57094, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57426, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57638, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57714, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57644, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57425, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57059, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56562, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55955, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55267, 145 + 553); - EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54533, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53785, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53055, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51180, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51185, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51324, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51592, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51976, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52460, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 53022, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53638, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54282, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54929, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55555, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56137, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56656, 145 + 567); + EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57091, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57423, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57635, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57711, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57642, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57422, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57057, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56560, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55953, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55266, 145 + 553); + EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54531, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53784, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53054, 145 + 531); EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52369, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51748, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(55, -35) * 1e9, 51205, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51747, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(55, -35) * 1e9, 51204, 145 + 512); EXPECT_NEAR(get_mag_strength_tesla(55, -30) * 1e9, 50750, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50388, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50389, 145 + 504); EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50124, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49955, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49878, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49888, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49976, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50134, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50356, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50639, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 50985, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51398, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51883, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52448, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53095, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53822, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54624, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55486, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56389, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57306, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58206, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59053, 145 + 591); - EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59812, 145 + 598); - EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60449, 145 + 604); - EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60935, 145 + 609); - EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61244, 145 + 612); - EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61362, 145 + 614); - EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61281, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49956, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49879, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49889, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49977, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50136, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50358, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50641, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 50987, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51400, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51886, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52451, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53097, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53825, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54626, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55489, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56392, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57309, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58209, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59056, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59815, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60452, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60937, 145 + 609); + EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61246, 145 + 612); + EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61364, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61282, 145 + 613); EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 61002, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60536, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59906, 145 + 599); - EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59142, 145 + 591); - EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58279, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60537, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59907, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59143, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58280, 145 + 583); EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57358, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56417, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55493, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54616, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53808, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53089, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52470, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51962, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51573, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56418, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55494, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54617, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53810, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53090, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52471, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51963, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51574, 145 + 516); EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51311, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53925, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53797, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53772, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53849, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 54024, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54289, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54630, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 55031, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55476, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55942, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56413, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56867, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57287, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57656, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57958, 145 + 580); - EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58177, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58302, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58321, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58227, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 58019, 145 + 580); - EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57699, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57277, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56769, 145 + 568); - EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56194, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55575, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53926, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53796, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53770, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53847, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 54022, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54286, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54627, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 55028, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55472, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55939, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56409, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56864, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57284, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57653, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57954, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58174, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58299, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58318, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58225, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 58017, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57697, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57276, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56768, 145 + 568); + EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56193, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55574, 145 + 556); EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54936, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54302, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53692, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54301, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53691, 145 + 537); EXPECT_NEAR(get_mag_strength_tesla(60, -40) * 1e9, 53124, 145 + 531); EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52614, 145 + 526); EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52171, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51803, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51515, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51308, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51184, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51139, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51172, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51278, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51456, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51705, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52026, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52422, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52896, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53450, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54081, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54787, 145 + 548); - EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55556, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56374, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57222, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58074, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58902, 145 + 589); - EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59676, 145 + 597); - EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60367, 145 + 604); - EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60947, 145 + 609); - EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61393, 145 + 614); - EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61687, 145 + 617); - EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61819, 145 + 618); - EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61783, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51804, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51516, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51310, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51185, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51141, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51173, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51280, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51458, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51707, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52028, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52425, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52899, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53452, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54084, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54789, 145 + 548); + EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55559, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56377, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57225, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58077, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58905, 145 + 589); + EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59679, 145 + 597); + EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60369, 145 + 604); + EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60949, 145 + 609); + EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61395, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61689, 145 + 617); + EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61820, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61784, 145 + 618); EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61586, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61237, 145 + 612); - EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60757, 145 + 608); - EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60169, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59502, 145 + 595); - EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58787, 145 + 588); - EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58053, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57327, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56632, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 55987, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55407, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54903, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54483, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61238, 145 + 612); + EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60758, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60170, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59504, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58788, 145 + 588); + EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58054, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57328, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56633, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 55989, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55408, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54904, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54484, 145 + 545); EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54156, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53925, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53926, 145 + 539); } diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index cdbeeb2162..cdf5e4108e 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -1,6 +1,6 @@ /** - * Airspeed Selector: Wind estimator wind process noise noise spectral density + * Wind estimator wind process noise spectral density * * Wind process noise of the internal wind estimator(s) of the airspeed selector. * When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second. @@ -14,7 +14,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f); /** - * Airspeed Selector: Wind estimator true airspeed scale process noise spectral density + * Wind estimator true airspeed scale process noise spectral density * * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector. * When unaided, the scale uncertainty (1-sigma, unitless) increases by this amount every second. @@ -28,7 +28,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f); PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f); /** - * Airspeed Selector: Wind estimator true airspeed measurement noise + * Wind estimator true airspeed measurement noise * * True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector. * @@ -41,7 +41,7 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f); PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f); /** - * Airspeed Selector: Wind estimator sideslip measurement noise + * Wind estimator sideslip measurement noise * * Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector. * @@ -54,7 +54,7 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f); PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f); /** - * Airspeed Selector: Gate size for true airspeed fusion + * Gate size for true airspeed fusion * * Sets the number of standard deviations used by the innovation consistency test. * @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f); PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3); /** - * Airspeed Selector: Gate size for sideslip angle fusion + * Gate size for sideslip angle fusion * * Sets the number of standard deviations used by the innovation consistency test. * @@ -148,7 +148,6 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); * Enable checks on airspeed sensors * * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0. - * Note that the data missing check is enabled if any of the options is set. * * @min 0 * @max 15 @@ -232,7 +231,7 @@ PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); * Horizontal wind uncertainty threshold for synthetic airspeed. * * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid - * as soon and as long the horizontal wind uncertainty drops below this value. + * as soon and as long the horizontal wind uncertainty is below this value. * * @unit m/s * @min 0.001 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d4b8d539d4..1397156c91 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -719,8 +719,14 @@ Commander::handle_command(const vehicle_command_s &cmd) // to not require navigator and command to receive / process // the data at the exact same time. - // Check if a mode switch had been requested - if ((((uint32_t)cmd.param2) & 1) > 0) { + const uint32_t change_mode_flags = uint32_t(cmd.param2); + const bool mode_switch_not_requested = (change_mode_flags & 1) == 0; + const bool unsupported_bits_set = (change_mode_flags & ~1) != 0; + + if (mode_switch_not_requested || unsupported_bits_set) { + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + + } else { if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -728,9 +734,6 @@ Commander::handle_command(const vehicle_command_s &cmd) printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - } else { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } } break; @@ -991,6 +994,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF: +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF /* ok, home set, use it to take off */ if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) { @@ -1001,6 +1005,9 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } +#else + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; +#endif // CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 6f617f2728..460ed302ed 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -95,6 +95,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // option is to check if ANY of them have a warning, and specifically find which one has the most // urgent warning. uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + float worst_battery_remaining = 1.f; // To make sure that all connected batteries are being regularly reported, we check which one has the // oldest timestamp. hrt_abstime oldest_update = hrt_absolute_time(); @@ -154,6 +155,10 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) worst_warning = battery.warning; } + if (battery.remaining < worst_battery_remaining) { + worst_battery_remaining = battery.remaining; + } + if (battery.timestamp < oldest_update) { oldest_update = battery.timestamp; } @@ -195,6 +200,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) } if (context.isArmed()) { + // if armed, only allow increase of battery warning severity if (worst_warning > reporter.failsafeFlags().battery_warning) { reporter.failsafeFlags().battery_warning = worst_warning; } @@ -205,18 +211,42 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) { - bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; + const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning; /* EVENT + * @description + * The battery state of charge of the worst battery is below the warning threshold. + * + * + * This check can be configured via BAT_LOW_THR, BAT_CRIT_THR and BAT_EMERGEN_THR parameters. + * */ reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level, "Low battery"); if (reporter.mavlink_log_pub()) { - mavlink_log_emergency(reporter.mavlink_log_pub(), "Preflight Fail: low battery\t"); + mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); } + } else if (!context.isArmed() && _param_arm_battery_level_min.get() > FLT_EPSILON + && worst_battery_remaining < _param_arm_battery_level_min.get()) { + // if not armed, additionally check if the battery is below the separately configurable preflight threshold + /* EVENT + * @description + * The battery state of charge of the worst battery is below the preflight threshold. + * + * + * This check can be configured via COM_ARM_BAT_MIN parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_preflight_low"), + events::Log::Critical, + "Low battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); + } } rtlEstimateCheck(context, reporter, worst_battery_time_s); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp index 2d5330f5ae..81f1058d35 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp @@ -56,4 +56,7 @@ private: bool _last_armed{false}; bool _battery_connected_at_arming[battery_status_s::MAX_INSTANCES] {}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamFloat) _param_arm_battery_level_min + ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp index 1136c3f282..89ba247ed2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp @@ -122,7 +122,7 @@ void SdCardChecks::checkAndReport(const Context &context, Report &reporter) events::Log::Error, "Crash dumps present on SD card"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD"); } } diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 0a07c2989d..a8b12edc50 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -47,6 +47,7 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode_s &vehicle_control_mode) { vehicle_control_mode.flag_armed = armed; + vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default switch (nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 76b925f934..623e8994f6 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -123,9 +123,10 @@ PARAM_DEFINE_INT32(COM_HLDL_LOSS_T, 120); PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0); /** - * RC loss time threshold + * Manual control loss timeout * - * After this amount of seconds without RC connection it's considered lost and not used anymore + * The time in seconds without a new setpoint from RC or Joystick, after which the connection is considered lost. + * This must be kept short as the vehicle will use the last supplied setpoint until the timeout triggers. * * @group Commander * @unit s @@ -262,8 +263,8 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); * * Before entering failsafe (RTL, Land, Hold), wait COM_FAIL_ACT_T seconds in Hold mode * for the user to realize. - * During that time the user cannot take over control. - * Afterwards the configured failsafe action is triggered and the user may take over. + * During that time the user cannot take over control via the stick override feature (see COM_RC_OVERRIDE). + * Afterwards the configured failsafe action is triggered and the user may use stick override. * * A zero value disables the delay and the user cannot take over via stick movements (switching modes is still allowed). * @@ -1097,3 +1098,20 @@ PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f); * @group Commander */ PARAM_DEFINE_INT32(COM_ARMABLE, 1); + +/** + * Minimum battery level for arming + * + * Additional battery level check that only allows arming if the state of charge of the emptiest + * connected battery is above this value. + * + * A value of 0 disables the check. + * + * @unit norm + * @min 0 + * @max 0.9 + * @decimal 2 + * @increment 0.01 + * @group Commander + */ +PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 961f955470..f8d64308d4 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -404,7 +404,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery - CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow)); + CHECK_FAILSAFE(status_flags, battery_low_remaining_time, + ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm)); CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn); switch (status_flags.battery_warning) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 184e10f876..3fff697d49 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -771,6 +771,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // FALLTHROUGH case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90 + + // FALLTHROUGH + case ROTATION_CUSTOM: // Skip, as we currently don't support detecting arbitrary euler angle orientation + MSE[r] = FLT_MAX; break; @@ -831,6 +835,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma worker_data.calibration[cur_mag].device_id(), worker_data.calibration[cur_mag].rotation_enum()); continue; + case ROTATION_CUSTOM: + PX4_INFO("[cal] External Mag: %d (%" PRIu32 "), not setting rotation enum since it's specified by Euler Angle", + cur_mag, worker_data.calibration[cur_mag].device_id()); + continue; // Continue to the next mag loop + default: break; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp index 099adb3120..901c469746 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -91,6 +91,8 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo const uint32_t motor_mask = (1u << actuator_idx); if (stoppable_motors_mask & motor_mask) { + + // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy) if (fabsf(actuator_sp(actuator_idx)) < .02f) { _stopped_motors_mask |= motor_mask; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 8d3401a6f8..f403424b67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -104,8 +104,7 @@ void ActuatorEffectivenessHelicopter::updateParams() _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; } -bool -ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, +bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { @@ -221,7 +220,6 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { - // Note: the values '-1', '1' and '0' are just to indicate a negative, // positive or no saturation to the rate controller. The actual magnitude is not used. if (_saturation_flags.roll_pos) { @@ -229,6 +227,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; } if (_saturation_flags.pitch_pos) { @@ -236,6 +237,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; } if (_saturation_flags.yaw_pos) { @@ -243,6 +247,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; } if (_saturation_flags.thrust_pos) { @@ -250,5 +257,8 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp new file mode 100644 index 0000000000..0c06f5963f --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "ActuatorEffectivenessHelicopterCoaxial.hpp" +#include + +using namespace matrix; +using namespace time_literals; + +ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i); + _param_handles.swash_plate_servos[i].angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i); + _param_handles.swash_plate_servos[i].arm_length = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles.swash_plate_servos[i].trim = param_find(buffer); + } + + _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); + _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); +} + +void ActuatorEffectivenessHelicopterCoaxial::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + float angle_deg{}; + param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg); + _geometry.swash_plate_servos[i].angle = math::radians(angle_deg); + param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length); + param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim); + } + + param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); +} + +bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // As the allocation is non-linear, we use updateSetpoint() instead of the matrix + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Clockwise rotor + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor + + // N swash plate servos + _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{}); + configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim; + } + + return true; +} + +void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + _saturation_flags = {}; + + // throttle/collective pitch curve + const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress(); + const float yaw = control_sp(ControlAxis::YAW); + + // actuator mapping + actuator_sp(0) = throttle - yaw; // Clockwise + actuator_sp(1) = throttle + yaw; // Counter-clockwise + + // Saturation check for yaw + if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); + + } else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } + + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + actuator_sp(_first_swash_plate_servo_index + i) = + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + + _geometry.swash_plate_servos[i].trim; + + // Saturation check for roll & pitch + if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos); + + } else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg); + } + } +} + +float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f; + const float spoolup_progress = time_since_arming / _geometry.spoolup_time; + + if (_armed && spoolup_progress < 1.f) { + return spoolup_progress; + } + + return 1.f; +} + + +void ActuatorEffectivenessHelicopterCoaxial::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag) +{ + if (coeff > 0.f) { + // A positive change in given axis will increase saturation + positive_flag = true; + + } else if (coeff < 0.f) { + // A negative change in given axis will increase saturation + negative_flag = true; + } +} + +void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_saturation_flags.roll_pos) { + status.unallocated_torque[0] = 1.f; + + } else if (_saturation_flags.roll_neg) { + status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; + } + + if (_saturation_flags.pitch_pos) { + status.unallocated_torque[1] = 1.f; + + } else if (_saturation_flags.pitch_neg) { + status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; + } + + if (_saturation_flags.yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_saturation_flags.yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } + + if (_saturation_flags.thrust_pos) { + status.unallocated_thrust[2] = 1.f; + + } else if (_saturation_flags.thrust_neg) { + status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp new file mode 100644 index 0000000000..d5316bf498 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "ActuatorEffectiveness.hpp" + +#include + +#include +#include +#include + +class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; + + struct SwashPlateGeometry { + float angle; + float arm_length; + float trim; + }; + + struct Geometry { + SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + int num_swash_plate_servos{0}; + float spoolup_time; + }; + + ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent); + virtual ~ActuatorEffectivenessHelicopterCoaxial() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Helicopter"; } + + + const Geometry &geometry() const { return _geometry; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; +private: + float throttleSpoolupProgress(); + + void updateParams() override; + + struct SaturationFlags { + bool roll_pos; + bool roll_neg; + bool pitch_pos; + bool pitch_neg; + bool yaw_pos; + bool yaw_neg; + bool thrust_pos; + bool thrust_neg; + }; + static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag); + + struct ParamHandlesSwashPlate { + param_t angle; + param_t arm_length; + param_t trim; + }; + struct ParamHandles { + ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + param_t num_swash_plate_servos; + param_t spoolup_time; + }; + ParamHandles _param_handles{}; + + Geometry _geometry{}; + + int _first_swash_plate_servo_index{}; + SaturationFlags _saturation_flags; + + // Throttle spoolup state + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; + + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 41005d38a7..a8effa8cb3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2023 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 @@ -55,7 +55,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); // Tilts - int first_tilt_idx = configuration.num_actuators_matrix[0]; + _first_tilt_idx = configuration.num_actuators_matrix[0]; _tilts.updateTorqueSign(_mc_rotors.geometry()); const bool tilts_added_successfully = _tilts.addActuators(configuration); @@ -69,7 +69,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration if (delta_angle > FLT_EPSILON) { float trim = -1.f - 2.f * _tilts.config(i).min_angle / delta_angle; - _tilt_offsets(first_tilt_idx + i) = trim; + _tilt_offsets(_first_tilt_idx + i) = trim; } } @@ -82,4 +82,49 @@ void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } + } + + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; +} + +void ActuatorEffectivenessMCTilt::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_yaw_tilt_saturation_flags.tilt_yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 15c12957ed..848c8b8853 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2023 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 @@ -61,8 +61,18 @@ public: const char *name() const override { return "MC Tilt"; } + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + protected: ActuatorVector _tilt_offsets; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessTilts _tilts; + int _first_tilt_idx{0}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 592c157f5a..5fe261b27f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -48,8 +48,19 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true), _control_surfaces(this), _tilts(this) { + _param_handles.com_spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); setFlightPhase(FlightPhase::HOVER_FLIGHT); } + +void ActuatorEffectivenessTiltrotorVTOL::updateParams() +{ + ModuleParams::updateParams(); + + param_get(_param_handles.com_spoolup_time, &_param_spoolup_time); +} + bool ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) @@ -137,12 +148,57 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } } } + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; + // in FW directly use throttle sp if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { for (int i = 0; i < _first_tilt_idx; ++i) { @@ -155,21 +211,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 1.f) { - _yaw_tilt_saturation_flags.tilt_yaw_pos = true; - } - } } void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) @@ -213,3 +254,15 @@ void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, status.unallocated_torque[2] = 0.f; } } + +bool ActuatorEffectivenessTiltrotorVTOL::throttleSpoolupFinished() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + return _armed && hrt_elapsed_time(&_armed_time) > _param_spoolup_time * 1_s; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index f8dd985290..310d937064 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -46,9 +46,11 @@ #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" +#include + #include #include - +#include #include class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness @@ -111,4 +113,22 @@ protected: YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; + +private: + + void updateParams() override; + + struct ParamHandles { + param_t com_spoolup_time; + }; + + ParamHandles _param_handles{}; + + float _param_spoolup_time{1.f}; + + // Tilt handling during motor spoolup: leave the tilts in their disarmed position unitil 1s after arming + bool throttleSpoolupFinished(); + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index a6f1733c10..d885a09155 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -79,6 +79,8 @@ public: bool hasYawControl() const; + float getYawTorqueOfTilt(int tilt_index) const { return _torque[tilt_index](2); } + private: void updateParams() override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 18c0679d10..994b566270 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -44,6 +44,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessFixedWing.hpp ActuatorEffectivenessHelicopter.cpp ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp ActuatorEffectivenessMCTilt.cpp ActuatorEffectivenessMCTilt.hpp ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 14c3193bfd..0fad1b5f62 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -266,6 +266,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS); break; + case EffectivenessSource::HELICOPTER_COAXIAL: + tmp = new ActuatorEffectivenessHelicopterCoaxial(this); + break; + default: PX4_ERR("Unknown airframe"); break; @@ -359,6 +363,14 @@ ControlAllocator::Run() } } + { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _publish_controls = vehicle_control_mode.flag_control_allocation_enabled; + } + } + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); @@ -641,6 +653,10 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) void ControlAllocator::publish_actuator_controls() { + if (!_publish_controls) { + return; + } + actuator_motors_s actuator_motors; actuator_motors.timestamp = hrt_absolute_time(); actuator_motors.timestamp_sample = _timestamp_sample; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 18132cbe55..25904f0551 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -73,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -156,6 +158,7 @@ private: CUSTOM = 9, HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, + HELICOPTER_COAXIAL = 12, }; enum class FailureMode { @@ -186,10 +189,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; + bool _publish_controls{true}; // Reflects motor failures that are currently handled, not motor failures that are reported. // For example, the system might report two motor failures, but only the first one is handled by CA diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index e5467515cb..2c506cc4e2 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -29,6 +29,7 @@ parameters: 9: Custom 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) default: 0 CA_METHOD: @@ -124,7 +125,7 @@ parameters: default: 0 CA_ROTOR${i}_PX: description: - short: Position of rotor ${i} along X body axis + short: Position of rotor ${i} along X body axis relative to center of gravity type: float decimal: 2 increment: 0.1 @@ -135,7 +136,7 @@ parameters: default: 0.0 CA_ROTOR${i}_PY: description: - short: Position of rotor ${i} along Y body axis + short: Position of rotor ${i} along Y body axis relative to center of gravity type: float decimal: 2 increment: 0.1 @@ -146,7 +147,7 @@ parameters: default: 0.0 CA_ROTOR${i}_PZ: description: - short: Position of rotor ${i} along Z body axis + short: Position of rotor ${i} along Z body axis relative to center of gravity type: float decimal: 2 increment: 0.1 @@ -416,6 +417,7 @@ parameters: short: Number of swash plates servos type: enum values: + 2: '2' 3: '3' 4: '4' default: 3 @@ -1079,6 +1081,8 @@ mixer: - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' parameters: + - label: 'Collective pitch offset to align least amount of rotor drag' + name: CA_HELI_YAW_CP_O - label: 'Yaw compensation scale based on collective pitch' name: CA_HELI_YAW_CP_S - label: 'Yaw compensation scale based on throttle' @@ -1116,3 +1120,23 @@ mixer: name: CA_HELI_YAW_CCW - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + + 12: # Helicopter (Coaxial) + actuators: + - actuator_type: 'motor' + count: 2 + item_label_prefix: ['Clockwise Rotor', 'Counter-clockwise Rotor'] + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index eb25328a5b..fc42770a96 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -529,6 +529,8 @@ _file_clear(dm_item_t item) static int _file_initialize(unsigned max_offset) { + const bool file_existed = (access(k_data_manager_device_path, F_OK) == 0); + /* Open or create the data manager file */ dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); @@ -553,7 +555,7 @@ _file_initialize(unsigned max_offset) dm_operations_data.silence = false; - if (compat_state.key != DM_COMPAT_KEY) { + if (!file_existed || (compat_state.key != DM_COMPAT_KEY)) { /* Write current compat info */ compat_state.key = DM_COMPAT_KEY; diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 0f1c095890..2854fc3b0d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -33,42 +33,88 @@ add_subdirectory(Utility) +option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF) + # Symforce code generation TODO:fixme -# execute_process( -# COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic -# RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE -# OUTPUT_QUIET -# ) -# if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0) +execute_process( + COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic + RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE + OUTPUT_QUIET +) -# set(EKF_DERIVATION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation) +# for now only provide symforce target helper if derivation.py generation isn't default +if((NOT CONFIG_EKF2_MAGNETOMETER) OR (NOT CONFIG_EKF2_WIND)) + set(EKF2_SYMFORCE_GEN ON) +endif() -# set(EKF_GENERATED_SRC_FILES -# ${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h -# ${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h -# ${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h -# ${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h -# ${EKF_DERIVATION_DIR}/generated/covariance_prediction.h -# ) +set(EKF_DERIVATION_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation) -# add_custom_command( -# OUTPUT ${EKF_GENERATED_SRC_FILES} -# COMMAND ${PYTHON_EXECUTABLE} derivation.py -# DEPENDS -# ${EKF_DERIVATION_DIR}/derivation.py -# ${EKF_DERIVATION_DIR}/derivation_utils.py +set(EKF_GENERATED_FILES ${EKF_DERIVATION_SRC_DIR}/generated/state.h) +set(EKF_GENERATED_DERIVATION_INCLUDE_PATH "${EKF_DERIVATION_SRC_DIR}/..") -# WORKING_DIRECTORY ${EKF_DERIVATION_DIR} -# COMMENT "Symforce code generation" -# USES_TERMINAL -# ) +if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)) -# add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES}) -# endif() + # regenerate default in tree + add_custom_command( + OUTPUT + ${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h + ${EKF_DERIVATION_SRC_DIR}/generated/state.h + COMMAND + ${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py + DEPENDS + ${EKF_DERIVATION_SRC_DIR}/derivation.py + ${EKF_DERIVATION_SRC_DIR}/derivation_utils.py + + WORKING_DIRECTORY ${EKF_DERIVATION_SRC_DIR} + COMMENT "Symforce code generation (default)" + USES_TERMINAL + ) + + # generate to build directory + set(EKF_DERIVATION_DST_DIR ${CMAKE_CURRENT_BINARY_DIR}/ekf_derivation) + file(MAKE_DIRECTORY ${EKF_DERIVATION_DST_DIR}) + + set(EKF_GENERATED_FILES ${EKF_DERIVATION_DST_DIR}/generated/state.h) + set(EKF_GENERATED_DERIVATION_INCLUDE_PATH ${CMAKE_CURRENT_BINARY_DIR}) + + set(SYMFORCE_ARGS) + + if(NOT CONFIG_EKF2_MAGNETOMETER) + message(STATUS "ekf2: symforce disabling mag") + list(APPEND SYMFORCE_ARGS "--disable_mag") + endif() + + if(NOT CONFIG_EKF2_WIND) + message(STATUS "ekf2: symforce disabling wind") + list(APPEND SYMFORCE_ARGS "--disable_wind") + endif() + + add_custom_command( + OUTPUT + ${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h + ${EKF_DERIVATION_DST_DIR}/generated/state.h + COMMAND + ${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS} + DEPENDS + ${EKF_DERIVATION_SRC_DIR}/derivation.py + ${EKF_DERIVATION_SRC_DIR}/derivation_utils.py + + WORKING_DIRECTORY ${EKF_DERIVATION_DST_DIR} + COMMENT "Symforce code generation" + USES_TERMINAL + ) + + + + add_custom_target(ekf2_symforce_generate + DEPENDS + ${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h + ${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h + ) +endif() set(EKF_SRCS) list(APPEND EKF_SRCS - EKF/baro_height_control.cpp EKF/bias_estimator.cpp EKF/control.cpp EKF/covariance.cpp @@ -78,21 +124,15 @@ list(APPEND EKF_SRCS EKF/estimator_interface.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp - EKF/gnss_height_control.cpp - EKF/gps_checks.cpp - EKF/gps_control.cpp - EKF/gravity_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp - EKF/mag_3d_control.cpp - EKF/mag_control.cpp - EKF/mag_fusion.cpp - EKF/mag_heading_control.cpp EKF/output_predictor.cpp EKF/vel_pos_fusion.cpp + EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp - EKF/zero_gyro_update.cpp - EKF/zero_velocity_update.cpp + + EKF/aid_sources/ZeroGyroUpdate.cpp + EKF/aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) @@ -103,6 +143,12 @@ if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp) endif() +if(CONFIG_EKF2_BAROMETER) + list(APPEND EKF_SRCS + EKF/baro_height_control.cpp + ) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS EKF/drag_fusion.cpp) endif() @@ -117,10 +163,31 @@ if(CONFIG_EKF2_EXTERNAL_VISION) ) endif() +if(CONFIG_EKF2_GNSS) + list(APPEND EKF_SRCS + EKF/gnss_height_control.cpp + EKF/gps_checks.cpp + EKF/gps_control.cpp + ) +endif() + if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_GRAVITY_FUSION) + list(APPEND EKF_SRCS EKF/gravity_fusion.cpp) +endif() + +if(CONFIG_EKF2_MAGNETOMETER) + list(APPEND EKF_SRCS + EKF/mag_3d_control.cpp + EKF/mag_control.cpp + EKF/mag_fusion.cpp + EKF/mag_heading_control.cpp + ) +endif() + if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS EKF/optical_flow_control.cpp @@ -133,7 +200,6 @@ if(CONFIG_EKF2_RANGE_FINDER) EKF/range_finder_consistency_check.cpp EKF/range_height_control.cpp EKF/sensor_range_finder.cpp - EKF/terrain_estimator.cpp ) endif() @@ -141,6 +207,10 @@ if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) endif() +if(CONFIG_EKF2_TERRAIN) + list(APPEND EKF_SRCS EKF/terrain_estimator.cpp) +endif() + px4_add_module( MODULE modules__ekf2 MAIN ekf2 @@ -151,6 +221,8 @@ px4_add_module( #-O0 INCLUDES EKF + EKF/aid_sources + ${EKF_GENERATED_DERIVATION_INCLUDE_PATH} PRIORITY "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads STACK_MAX @@ -163,6 +235,8 @@ px4_add_module( EKF2Selector.cpp EKF2Selector.hpp + ${EKF_GENERATED_FILES} + DEPENDS geo hysteresis diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 64230774af..cdb0a56ba5 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2023 ECL Development Team. All rights reserved. +# Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name ECL nor the names of its contributors may be +# 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. # @@ -33,31 +33,23 @@ set(EKF_SRCS) list(APPEND EKF_SRCS - baro_height_control.cpp bias_estimator.cpp control.cpp covariance.cpp ekf.cpp ekf_helper.cpp - EKFGSF_yaw.cpp estimator_interface.cpp fake_height_control.cpp fake_pos_control.cpp - gnss_height_control.cpp - gps_checks.cpp - gps_control.cpp - gravity_fusion.cpp height_control.cpp imu_down_sampler.cpp - mag_3d_control.cpp - mag_control.cpp - mag_fusion.cpp - mag_heading_control.cpp output_predictor.cpp vel_pos_fusion.cpp + yaw_fusion.cpp zero_innovation_heading_update.cpp - zero_gyro_update.cpp - zero_velocity_update.cpp + + aid_sources/ZeroGyroUpdate.cpp + aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) @@ -68,6 +60,12 @@ if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS auxvel_fusion.cpp) endif() +if(CONFIG_EKF2_BAROMETER) + list(APPEND EKF_SRCS + baro_height_control.cpp + ) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS drag_fusion.cpp) endif() @@ -82,10 +80,32 @@ if(CONFIG_EKF2_EXTERNAL_VISION) ) endif() +if(CONFIG_EKF2_GNSS) + list(APPEND EKF_SRCS + gnss_height_control.cpp + gps_checks.cpp + gps_control.cpp + EKFGSF_yaw.cpp + ) +endif() + if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_GRAVITY_FUSION) + list(APPEND EKF_SRCS gravity_fusion.cpp) +endif() + +if(CONFIG_EKF2_MAGNETOMETER) + list(APPEND EKF_SRCS + mag_3d_control.cpp + mag_control.cpp + mag_fusion.cpp + mag_heading_control.cpp + ) +endif() + if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS optical_flow_control.cpp @@ -98,7 +118,6 @@ if(CONFIG_EKF2_RANGE_FINDER) range_finder_consistency_check.cpp range_height_control.cpp sensor_range_finder.cpp - terrain_estimator.cpp ) endif() @@ -106,10 +125,15 @@ if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS sideslip_fusion.cpp) endif() +if(CONFIG_EKF2_TERRAIN) + list(APPEND EKF_SRCS terrain_estimator.cpp) +endif() + add_library(ecl_EKF ${EKF_SRCS} ) add_dependencies(ecl_EKF prebuild_targets) +target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PATH}) target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model) target_compile_options(ecl_EKF PRIVATE -fno-associative-math) diff --git a/src/modules/ekf2/EKF/Sensor.hpp b/src/modules/ekf2/EKF/Sensor.hpp index 5f18340c77..e1b83e708b 100644 --- a/src/modules/ekf2/EKF/Sensor.hpp +++ b/src/modules/ekf2/EKF/Sensor.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp new file mode 100644 index 0000000000..6d6c9e8ec2 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2023 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. + * + ****************************************************************************/ + +#ifndef EKF_ESTIMATOR_AID_SOURCE_HPP +#define EKF_ESTIMATOR_AID_SOURCE_HPP + +#include + +#include + +// forward declarations +class Ekf; + +namespace estimator +{ +struct imuSample; +}; + +class EstimatorAidSource +{ +public: + EstimatorAidSource() = default; + virtual ~EstimatorAidSource() = default; + + virtual bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) = 0; + + virtual void reset() = 0; + +private: + + +}; + +#endif // !EKF_ESTIMATOR_AID_SOURCE_HPP diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp similarity index 69% rename from src/modules/ekf2/EKF/zero_gyro_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp index 51551fec93..ea9d3f11a8 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp @@ -31,21 +31,25 @@ * ****************************************************************************/ -/** - * @file zero_gyro_update.cpp - * Control function for ekf zero gyro update - */ +#include "ZeroGyroUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) +ZeroGyroUpdate::ZeroGyroUpdate() { - if (!(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias))) { - return; - } + reset(); +} +void ZeroGyroUpdate::reset() +{ + _zgup_delta_ang.setZero(); + _zgup_delta_ang_dt = 0.f; +} + +bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ // When at rest, fuse the gyro data as a direct observation of the gyro bias - if (_control_status.flags.vehicle_at_rest) { + if (ekf.control_status_flags().vehicle_at_rest) { // Downsample gyro data to run the fusion at a lower rate _zgup_delta_ang += imu_delayed.delta_ang; _zgup_delta_ang_dt += imu_delayed.delta_ang_dt; @@ -54,41 +58,38 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt; if (zero_gyro_update_data_ready) { + Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; - Vector3f innovation = _state.gyro_bias - gyro_bias; + Vector3f innovation = ekf.state().gyro_bias - gyro_bias; - const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f)); + const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); - Vector3f innov_var{ - P(10, 10) + obs_var, - P(11, 11) + obs_var, - P(12, 12) + obs_var}; + const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; for (int i = 0; i < 3; i++) { - fuseDeltaAngBias(innovation(i), innov_var(i), i); + Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. + const unsigned state_index = State::gyro_bias.idx + i; + + // calculate kalman gain K = PHS, where S = 1/innovation variance + for (int row = 0; row < State::size; row++) { + K(row) = ekf.stateCovariance(row, state_index) / innov_var(i); + } + + ekf.measurementUpdate(K, innov_var(i), innovation(i)); } // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; + + return true; } - } else if (_control_status_prev.flags.vehicle_at_rest) { + } else if (ekf.control_status_prev_flags().vehicle_at_rest) { // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; } -} - -void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) -{ - Vector24f K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + 10; - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < _k_num_states; row++) { - K(row) = P(row, state_index) / innov_var; - } - - measurementUpdate(K, innov_var, innov); + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp new file mode 100644 index 0000000000..1ad488e539 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#ifndef EKF_ZERO_GYRO_UPDATE_HPP +#define EKF_ZERO_GYRO_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroGyroUpdate : public EstimatorAidSource +{ +public: + ZeroGyroUpdate(); + virtual ~ZeroGyroUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + matrix::Vector3f _zgup_delta_ang{}; + float _zgup_delta_ang_dt{0.f}; +}; + +#endif // !EKF_ZERO_GYRO_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp similarity index 61% rename from src/modules/ekf2/EKF/zero_velocity_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index 4863918a13..d091e5aadb 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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,40 +31,50 @@ * ****************************************************************************/ -/** - * @file zero_velocity_update.cpp - * Control function for ekf zero velocity update - */ +#include "ZeroVelocityUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroVelocityUpdate() +ZeroVelocityUpdate::ZeroVelocityUpdate() +{ + reset(); +} + +void ZeroVelocityUpdate::reset() +{ + _time_last_zero_velocity_fuse = 0; +} + +bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { // Fuse zero velocity at a limited rate (every 200 milliseconds) - const bool zero_velocity_update_data_ready = isTimedOut(_time_last_zero_velocity_fuse, (uint64_t)2e5); + const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us); if (zero_velocity_update_data_ready) { - const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest - && _control_status_prev.flags.vehicle_at_rest - && (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift + const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest + && ekf.control_status_prev_flags().vehicle_at_rest + && (!ekf.isVerticalVelocityAidingActive() + || !ekf.control_status_flags().tilt_align); // otherwise the filter is "too rigid" to follow a position drift if (continuing_conditions_passing) { - Vector3f vel_obs{0, 0, 0}; - Vector3f innovation = _state.vel - vel_obs; + Vector3f vel_obs{0.f, 0.f, 0.f}; + Vector3f innovation = ekf.state().vel - vel_obs; // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements - const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f); - Vector3f innov_var{ - P(4, 4) + obs_var, - P(5, 5) + obs_var, - P(6, 6) + obs_var}; - fuseVelPosHeight(innovation(0), innov_var(0), 0); - fuseVelPosHeight(innovation(1), innov_var(1), 1); - fuseVelPosHeight(innovation(2), innov_var(2), 2); + const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f); + Vector3f innov_var = ekf.getVelocityVariance() + obs_var; - _time_last_zero_velocity_fuse = _time_delayed_us; + for (unsigned i = 0; i < 3; i++) { + ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + } + + _time_last_zero_velocity_fuse = imu_delayed.time_us; + + return true; } } + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp new file mode 100644 index 0000000000..0016c584c6 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 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. + * + ****************************************************************************/ + +#ifndef EKF_ZERO_VELOCITY_UPDATE_HPP +#define EKF_ZERO_VELOCITY_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroVelocityUpdate : public EstimatorAidSource +{ +public: + ZeroVelocityUpdate(); + virtual ~ZeroVelocityUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) + +}; + +#endif // !EKF_ZERO_VELOCITY_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 62afda7447..fde836232b 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -44,8 +44,9 @@ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h" -#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h" +#include +#include +#include #include @@ -61,6 +62,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _control_status.flags.wind = false; } +#if defined(CONFIG_EKF2_GNSS) // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) if (_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) { @@ -72,6 +74,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _yawEstimator.setTrueAirspeed(0.f); } } +#endif // CONFIG_EKF2_GNSS if (_params.arsp_thr <= 0.f) { stopAirspeedFusion(); @@ -98,7 +101,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) fuseAirspeed(airspeed_sample, _aid_src_airspeed); } +#if defined(CONFIG_EKF2_GNSS) _yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed); +#endif // CONFIG_EKF2_GNSS const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); @@ -144,15 +149,13 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so float innov = 0.f; float innov_var = 0.f; - sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); + sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); aid_src.observation = airspeed_sample.true_airspeed; aid_src.observation_variance = R; aid_src.innovation = innov; aid_src.innovation_variance = innov_var; - aid_src.fusion_enabled = _control_status.flags.fuse_aspd; - aid_src.timestamp_sample = airspeed_sample.time_us; const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f); @@ -194,15 +197,15 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour _fault_status.flags.bad_airspeed = false; - Vector24f H; // Observation jacobian - Vector24f K; // Kalman gain vector + VectorState H; // Observation jacobian + VectorState K; // Kalman gain vector - sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); + sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K); if (update_wind_only) { - for (unsigned row = 0; row <= 21; row++) { - K(row) = 0.f; - } + const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); + K.setZero(); + K.slice(State::wind_vel.idx, 0) = K_wind; } const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation); @@ -220,53 +223,27 @@ void Ekf::stopAirspeedFusion() if (_control_status.flags.fuse_aspd) { ECL_INFO("stopping airspeed fusion"); resetEstimatorAidStatus(_aid_src_airspeed); - _yawEstimator.setTrueAirspeed(NAN); _control_status.flags.fuse_aspd = false; + +#if defined(CONFIG_EKF2_GNSS) + _yawEstimator.setTrueAirspeed(NAN); +#endif // CONFIG_EKF2_GNSS } } void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) { - const float euler_yaw = getEulerYaw(_R_to_earth); + constexpr float sideslip_var = sq(math::radians(15.0f)); - // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available - _state.wind_vel(0) = _state.vel(0) - airspeed_sample.true_airspeed * cosf(euler_yaw); - _state.wind_vel(1) = _state.vel(1) - airspeed_sample.true_airspeed * sinf(euler_yaw); + const float euler_yaw = getEulerYaw(_R_to_earth); + const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); + + matrix::SquareMatrix P_wind; + sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind); + + resetStateCovariance(P_wind); ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1)); - resetWindCovarianceUsingAirspeed(airspeed_sample); - _aid_src_airspeed.time_last_fuse = _time_delayed_us; } - -void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample) -{ - // Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py - // TODO: explicitly include the sideslip angle in the derivation - const float euler_yaw = getEulerYaw(_R_to_earth); - const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); - constexpr float initial_sideslip_uncertainty = math::radians(15.0f); - const float initial_wind_var_body_y = sq(airspeed_sample.true_airspeed * sinf(initial_sideslip_uncertainty)); - constexpr float R_yaw = sq(math::radians(10.0f)); - - const float cos_yaw = cosf(euler_yaw); - const float sin_yaw = sinf(euler_yaw); - - // rotate wind velocity into earth frame aligned with vehicle yaw - const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw; - const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw; - - // it is safer to remove all existing correlations to other states at this time - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); - - P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw); - P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) - - initial_wind_var_body_y * sin_yaw * cos_yaw; - P(23, 22) = P(22, 23); - P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw); - - // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed - P(22, 22) += P(4, 4); - P(23, 23) += P(5, 5); -} diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/auxvel_fusion.cpp index 680f40cc7d..dbe88ccb87 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/auxvel_fusion.cpp @@ -45,7 +45,6 @@ void Ekf::controlAuxVelFusion() updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); if (isHorizontalAidingActive()) { - _aid_src_aux_vel.fusion_enabled = true; fuseVelocity(_aid_src_aux_vel); } } diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index fb649d5c5f..f3c5b0df64 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -51,7 +51,12 @@ void Ekf::controlBaroHeightFusion() if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { +#if defined(CONFIG_EKF2_BARO_COMPENSATION) const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt); +#else + const float measurement = baro_sample.hgt; +#endif + const float measurement_var = sq(_params.baro_noise); const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); @@ -103,7 +108,7 @@ void Ekf::controlBaroHeightFusion() if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding @@ -116,7 +121,6 @@ void Ekf::controlBaroHeightFusion() && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); if (_control_status.flags.baro_hgt) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp index 53f82dc03c..d810fc1d07 100644 --- a/src/modules/ekf2/EKF/bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -47,7 +47,8 @@ * @author Mathieu Bresciani */ -#pragma once +#ifndef EKF_BIAS_ESTIMATOR_HPP +#define EKF_BIAS_ESTIMATOR_HPP #include #include @@ -133,3 +134,5 @@ private: static constexpr float _innov_sequence_monitnoring_time_constant{10.f}; ///< in seconds static constexpr float _process_var_boost_gain{1.0e3f}; }; + +#endif // !EKF_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 15de4bebd5..a6b20b3978 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -91,6 +91,7 @@ enum class VelocityFrame : uint8_t { BODY_FRAME_FRD = 2 }; +#if defined(CONFIG_EKF2_MAGNETOMETER) enum GeoDeclinationMask : uint8_t { // Bit locations for mag_declination_source USE_GEO_DECL = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value @@ -104,13 +105,14 @@ enum MagFuseType : uint8_t { HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg NONE = 5 ///< Do not use magnetometer under any circumstance.. }; +#endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) enum TerrainFusionMask : uint8_t { TerrainFuseRangeFinder = (1 << 0), TerrainFuseOpticalFlow = (1 << 1) }; -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN enum HeightSensor : uint8_t { BARO = 0, @@ -269,17 +271,6 @@ struct systemFlagUpdate { bool gnd_effect{false}; }; -struct stateSample { - Quatf quat_nominal{}; ///< quaternion defining the rotation from body to earth frame - Vector3f vel{}; ///< NED velocity in earth frame in m/s - Vector3f pos{}; ///< NED position in earth frame in m - Vector3f gyro_bias{}; ///< gyro bias estimate in rad/s - Vector3f accel_bias{}; ///< accel bias estimate in m/s^2 - Vector3f mag_I{}; ///< NED earth magnetic field in gauss - Vector3f mag_B{}; ///< magnetometer bias estimate in body frame in gauss - Vector2f wind_vel{}; ///< horizontal wind velocity in earth frame in m/s -}; - struct parameters { int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds @@ -289,16 +280,9 @@ struct parameters { // measurement source control int32_t height_sensor_ref{HeightSensor::BARO}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; - int32_t baro_ctrl{1}; - int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) - // measurement time delays - float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) - float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) - float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) - // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) float accel_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2) @@ -306,45 +290,103 @@ struct parameters { // process noise float gyro_bias_p_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) - float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) - float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) + +#if defined(CONFIG_EKF2_WIND) + const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity +#endif // CONFIG_EKF2_WIND // initialization errors float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) - const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) + +#if defined(CONFIG_EKF2_BAROMETER) + int32_t baro_ctrl{1}; + float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) + float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) + float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) + float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) + + float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) + float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) + +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + // static barometer pressure position error coefficient along body axes + float static_pressure_coef_xp{0.0f}; // (-) + float static_pressure_coef_xn{0.0f}; // (-) + float static_pressure_coef_yp{0.0f}; // (-) + float static_pressure_coef_yn{0.0f}; // (-) + float static_pressure_coef_z{0.0f}; // (-) + + // upper limit on airspeed used for correction (m/s**2) + float max_correction_airspeed{20.0f}; +# endif // CONFIG_EKF2_BARO_COMPENSATION +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_GNSS) + int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; + float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) + + Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) // position and velocity fusion float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz)) - float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) - float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) - float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) - float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) float gps_pos_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) float gps_vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD) - float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) - float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) + + // these parameters control the strictness of GPS quality checks used to determine if the GPS is + // good enough to set a local origin and commence aiding + int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used + float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m) + float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m) + float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) + int32_t req_nsats{6}; ///< minimum acceptable satellite count + float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision + float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) + float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) + +# if defined(CONFIG_EKF2_GNSS_YAW) + // GNSS heading fusion + float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) +# endif // CONFIG_EKF2_GNSS_YAW + + // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value + float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) + const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value + const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) + +#endif // CONFIG_EKF2_GNSS + + float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) + + float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) + float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) + +#if defined(CONFIG_EKF2_MAGNETOMETER) + float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) + + float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) + float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) // magnetometer fusion - float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss) float mag_declination_deg{0.0f}; ///< magnetic declination (degrees) - float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) float mag_innov_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD) int32_t mag_declination_source{7}; ///< bitmask used to control the handling of declination data int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) float mag_yaw_rate_gate{0.20f}; ///< yaw rate threshold used by mode select logic (rad/sec) -#if defined(CONFIG_EKF2_GNSS_YAW) - // GNSS heading fusion - float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) -#endif // CONFIG_EKF2_GNSS_YAW + // compute synthetic magnetomter Z value if possible + int32_t synthesize_mag_z{0}; + int32_t mag_check{0}; + float mag_check_strength_tolerance_gs{0.2f}; + float mag_check_inclination_tolerance_deg{20.f}; +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) // airspeed fusion @@ -362,18 +404,27 @@ struct parameters { const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_TERRAIN) + int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | + TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator + + float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) + float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) + const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion int32_t rng_ctrl{RngCtrl::CONDITIONAL}; - int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | - TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator - float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz)) - float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance @@ -385,11 +436,6 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) - - float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) - float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) - const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) - #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -408,8 +454,10 @@ struct parameters { Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity fusion float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_OPTICAL_FLOW) int32_t flow_ctrl{0}; @@ -425,20 +473,8 @@ struct parameters { Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) #endif // CONFIG_EKF2_OPTICAL_FLOW - // these parameters control the strictness of GPS quality checks used to determine if the GPS is - // good enough to set a local origin and commence aiding - int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used - float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m) - float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m) - float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) - int32_t req_nsats{6}; ///< minimum acceptable satellite count - float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision - float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) - float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) - // XYZ offset of sensors in body axes (m) Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m) - Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) // accel bias learning control float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2) @@ -454,18 +490,6 @@ struct parameters { int32_t valid_timeout_max{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - // static barometer pressure position error coefficient along body axes - float static_pressure_coef_xp{0.0f}; // (-) - float static_pressure_coef_xn{0.0f}; // (-) - float static_pressure_coef_yp{0.0f}; // (-) - float static_pressure_coef_yn{0.0f}; // (-) - float static_pressure_coef_z{0.0f}; // (-) - - // upper limit on airspeed used for correction (m/s**2) - float max_correction_airspeed {20.0f}; -#endif // CONFIG_EKF2_BARO_COMPENSATION - #if defined(CONFIG_EKF2_DRAG_FUSION) // multi-rotor drag specific force fusion int32_t drag_ctrl{0}; @@ -487,16 +511,6 @@ struct parameters { const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) #endif // CONFIG_EKF2_AUXVEL - // compute synthetic magnetomter Z value if possible - int32_t synthesize_mag_z{0}; - int32_t mag_check{0}; - float mag_check_strength_tolerance_gs{0.2f}; - float mag_check_inclination_tolerance_deg{20.f}; - - // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value - float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) - const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value - const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) }; union fault_status_u { @@ -600,6 +614,7 @@ union filter_control_status_u { uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used + uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter } flags; uint64_t value; @@ -624,7 +639,7 @@ union ekf_solution_status_u { uint16_t value; }; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) union terrain_fusion_status_u { struct { bool range_finder : 1; ///< 0 - true if we are fusing range finder data @@ -632,7 +647,7 @@ union terrain_fusion_status_u { } flags; uint8_t value; }; -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN // define structure used to communicate information events union information_event_status_u { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c35563135c..5c7aa5790b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -92,7 +92,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) } ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", - (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); + (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f", (double)matrix::Eulerf(_state.quat_nominal).phi(), @@ -102,14 +102,18 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) } } +#if defined(CONFIG_EKF2_MAGNETOMETER) // control use of observations for aiding controlMagFusion(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_OPTICAL_FLOW) controlOpticalFlowFusion(imu_delayed); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) controlGpsFusion(imu_delayed); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AIRSPEED) controlAirDataFusion(imu_delayed); @@ -120,11 +124,14 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) - controlDragFusion(); + controlDragFusion(imu_delayed); #endif // CONFIG_EKF2_DRAG_FUSION controlHeightFusion(imu_delayed); + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) controlGravityFusion(imu_delayed); +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_EXTERNAL_VISION) // Additional data odometry data from an external estimator can be fused. @@ -138,8 +145,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlZeroInnovationHeadingUpdate(); - controlZeroVelocityUpdate(); - controlZeroGyroUpdate(imu_delayed); + _zero_velocity_update.update(*this, imu_delayed); + + if (_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)) { + _zero_gyro_update.update(*this, imu_delayed); + } // Fake position measurement for constraining drift when no other velocity or position measurements controlFakePosFusion(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 9f8371b533..28c5815f18 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -42,7 +42,8 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/predict_covariance.h" +#include +#include #include #include @@ -56,266 +57,166 @@ void Ekf::initialiseCovariance() resetQuatCov(); // velocity - P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); - P(5,5) = P(4,4); - P(6,6) = sq(1.5f) * P(4,4); +#if defined(CONFIG_EKF2_GNSS) + const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f)); +#else + const float vel_var = sq(0.5f); +#endif + P.uncorrelateCovarianceSetVariance(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var)); // position - P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f)); - P(8,8) = P(7,7); - P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); +#if defined(CONFIG_EKF2_BAROMETER) + float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f)); +#else + float z_pos_var = sq(1.f); +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_GNSS) + const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f)); if (_control_status.flags.gps_hgt) { - P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); + z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } +#else + const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f)); +#endif #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { - P(9,9) = sq(fmaxf(_params.range_noise, 0.01f)); + z_pos_var = sq(fmaxf(_params.range_noise, 0.01f)); } #endif // CONFIG_EKF2_RANGE_FINDER - // gyro bias - _prev_gyro_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias); - _prev_gyro_bias_var(1) = P(11,11) = P(10,10); - _prev_gyro_bias_var(2) = P(12,12) = P(10,10); + P.uncorrelateCovarianceSetVariance(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var)); - // accel bias - _prev_accel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias); - _prev_accel_bias_var(1) = P(14,14) = P(13,13); - _prev_accel_bias_var(2) = P(15,15) = P(13,13); + resetGyroBiasCov(); + resetAccelBiasCov(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) resetMagCov(); +#endif // CONFIG_EKF2_MAGNETOMETER - // wind - P(22,22) = sq(_params.initial_wind_uncertainty); - P(23,23) = P(22,22); - +#if defined(CONFIG_EKF2_WIND) + resetWindCov(); +#endif // CONFIG_EKF2_WIND } void Ekf::predictCovariance(const imuSample &imu_delayed) { // Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values const float dt = _dt_ekf_avg; - const float dt_inv = 1.f / dt; - // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected - // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector - const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); - const float beta = 1.0f - alpha; - _ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); - _accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); - _accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt; - - const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim - || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim; - - // gyro bias inhibit - const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); - - for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) { - const unsigned index = stateIndex - 10; - - bool is_bias_observable = true; - - // TODO: gyro bias conditions - - const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable; - - if (do_inhibit_axis) { - // store the bias state variances to be reinstated later - if (!_gyro_bias_inhibit[index]) { - _prev_gyro_bias_var(index) = P(stateIndex, stateIndex); - _gyro_bias_inhibit[index] = true; - } - - } else { - if (_gyro_bias_inhibit[index]) { - // reinstate the bias state variances - P(stateIndex, stateIndex) = _prev_gyro_bias_var(index); - _gyro_bias_inhibit[index] = false; - } - } - } - - // accel bias inhibit - const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) - || is_manoeuvre_level_high - || _fault_status.flags.bad_acc_vertical; - - for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) { - const unsigned index = stateIndex - 13; - - bool is_bias_observable = true; - - if (_control_status.flags.vehicle_at_rest) { - is_bias_observable = true; - - } else if (_control_status.flags.fake_hgt) { - is_bias_observable = false; - - } else if (_control_status.flags.fake_pos) { - // when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector - is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966 - } - - const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable; - - if (do_inhibit_axis) { - // store the bias state variances to be reinstated later - if (!_accel_bias_inhibit[index]) { - _prev_accel_bias_var(index) = P(stateIndex, stateIndex); - _accel_bias_inhibit[index] = true; - } - - } else { - if (_accel_bias_inhibit[index]) { - // reinstate the bias state variances - P(stateIndex, stateIndex) = _prev_accel_bias_var(index); - _accel_bias_inhibit[index] = false; - } - } - } - - // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned - float mag_I_sig; - - if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) { - mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); - - } else { - mag_I_sig = 0.0f; - } - - // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned - float mag_B_sig; - - if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) { - mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); - - } else { - mag_B_sig = 0.0f; - } - - float wind_vel_nsd_scaled; - - // Calculate low pass filtered height rate - float alpha_height_rate_lpf = 0.1f * dt; // 10 seconds time constant - _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; - - // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned - if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) { - wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - - } else { - wind_vel_nsd_scaled = 0.0f; - } - - - // assign IMU noise variances - // inputs to the system are 3 delta angles and 3 delta velocities - float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); - const float d_ang_var = sq(dt * gyro_noise); - - float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); + // delta angle noise variance + float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f); + const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise); + // delta velocity noise variance + float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f); Vector3f d_vel_var; - for (int i = 0; i <= 2; i++) { + for (unsigned i = 0; i < 3; i++) { if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) { // Increase accelerometer process noise if bad accel data is detected - d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE); + d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE); } else { - d_vel_var(i) = sq(dt * accel_noise); + d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise); } } // predict the covariance - SquareMatrix24f nextP; - // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP); - - // compute noise variance for stationary processes - Vector24f process_noise; + P = sym::PredictCovariance(_state.vector(), P, + imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var, + imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var); // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances - // earth frame magnetic field states - process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig); - // body frame magnetic field states - process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig); - // wind velocity states - process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt; - - // add process noise that is not from the IMU - for (unsigned i = 16; i <= 23; i++) { - nextP(i, i) += process_noise(i); - } - // gyro bias: add process noise, or restore previous gyro bias var if state inhibited const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f); const float gyro_bias_process_noise = sq(gyro_bias_sig); - for (unsigned i = 10; i <= 12; i++) { - const int axis_index = i - 10; + for (unsigned index = 0; index < State::gyro_bias.dof; index++) { + const unsigned i = State::gyro_bias.idx + index; - if (!_gyro_bias_inhibit[axis_index]) { - nextP(i, i) += gyro_bias_process_noise; + if (!_gyro_bias_inhibit[index]) { + P(i, i) += gyro_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(axis_index)); + P.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index)); } } // accel bias: add process noise, or restore previous accel bias var if state inhibited const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f); const float accel_bias_process_noise = sq(accel_bias_sig); - for (int i = 13; i <= 15; i++) { - const int axis_index = i - 13; + for (unsigned index = 0; index < State::accel_bias.dof; index++) { + const unsigned i = State::accel_bias.idx + index; - if (!_accel_bias_inhibit[axis_index]) { - nextP(i, i) += accel_bias_process_noise; + if (!_accel_bias_inhibit[index]) { + P(i, i) += accel_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(axis_index)); + P.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index)); } } +#if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag) { + // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + if (P.trace(State::mag_I.idx) < 0.1f) { + + float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); + float mag_I_process_noise = sq(mag_I_sig); + + for (unsigned index = 0; index < State::mag_I.dof; index++) { + unsigned i = State::mag_I.idx + index; + P(i, i) += mag_I_process_noise; + } + } + + // Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + if (P.trace(State::mag_B.idx) < 0.1f) { + + float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); + float mag_B_process_noise = sq(mag_B_sig); + + for (unsigned index = 0; index < State::mag_B.dof; index++) { + unsigned i = State::mag_B.idx + index; + P(i, i) += mag_B_process_noise; + } + } + } +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) + if (_control_status.flags.wind) { + // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned + if (P.trace(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) { + + float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + + const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + unsigned i = State::wind_vel.idx + index; + P(i, i) += wind_vel_process_noise; + } + } + } +#endif // CONFIG_EKF2_WIND + // covariance matrix is symmetrical, so copy upper half to lower half - for (unsigned row = 0; row <= 15; row++) { + for (unsigned row = 0; row < State::size; row++) { for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); - } - - if (_control_status.flags.mag) { - for (unsigned row = 16; row <= 21; row++) { - for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); - } - } - - if (_control_status.flags.wind) { - for (unsigned row = 22; row <= 23; row++) { - for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); + P(row, column) = P(column, row); } } // fix gross errors in the covariance matrix and ensure rows and // columns for un-used states are zero fixCovarianceErrors(false); - } void Ekf::fixCovarianceErrors(bool force_symmetry) @@ -325,40 +226,15 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // and set corresponding entries in Q to zero when states exceed 50% of the limit // Covariance diagonal limits. Use same values for states which // belong to the same group (e.g. vel_x, vel_y, vel_z) - float P_lim[8] = {}; - P_lim[0] = 1.0f; // quaternion max var - P_lim[1] = 1e6f; // velocity max var - P_lim[2] = 1e6f; // position max var - P_lim[3] = 1.0f; // gyro bias max var - P_lim[4] = 1.0f; // delta velocity z bias max var - P_lim[5] = 1.0f; // earth mag field max var - P_lim[6] = 1.0f; // body mag field max var - P_lim[7] = 1e6f; // wind max var + const float quat_var_max = 1.0f; + const float vel_var_max = 1e6f; + const float pos_var_max = 1e6f; + const float gyro_bias_var_max = 1.0f; - for (int i = 0; i <= 3; i++) { - // quaternion states - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]); - } - - for (int i = 4; i <= 6; i++) { - // NED velocity states - P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]); - } - - for (int i = 7; i <= 9; i++) { - // NED position states - P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]); - } - - for (int i = 10; i <= 12; i++) { - // gyro bias states - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]); - } - - // force symmetry on the quaternion, velocity and position state covariances - if (force_symmetry) { - P.makeRowColSymmetric<13>(0); - } + constrainStateVar(State::quat_nominal, 0.f, quat_var_max); + constrainStateVar(State::vel, 1e-6f, vel_var_max); + constrainStateVar(State::pos, 1e-6f, pos_var_max); + constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max); // the following states are optional and are deactivated when not required // by ensuring the corresponding covariance matrix values are kept at zero @@ -370,8 +246,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) float maxStateVar = minSafeStateVar; bool resetRequired = false; - for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - if (_accel_bias_inhibit[stateIndex - 13]) { + for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) { + const unsigned stateIndex = State::accel_bias.idx + axis; + + if (_accel_bias_inhibit[axis]) { // Skip the check for the inhibited axis continue; } @@ -390,8 +268,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg); float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); - for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - if (_accel_bias_inhibit[stateIndex - 13]) { + for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) { + const unsigned stateIndex = State::accel_bias.idx + axis; + + if (_accel_bias_inhibit[axis]) { // Skip the check for the inhibited axis continue; } @@ -401,122 +281,68 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - P.uncorrelateCovariance<3>(13); + resetAccelBiasCov(); } - - // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong - // calculate accel bias term aligned with the gravity vector - const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; - const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg; - const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); - - // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation - bool bad_acc_bias = false; - if (fabsf(down_dvel_bias) > dVel_bias_lim) { - - bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f); -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f); -#else - bool bad_vz_ev = false; -#endif // CONFIG_EKF2_EXTERNAL_VISION - - if (bad_vz_gps || bad_vz_ev) { - bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f); - bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f); - -#if defined(CONFIG_EKF2_RANGE_FINDER) - bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f); -#else - bool bad_z_rng = false; -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f); -#else - bool bad_z_ev = false; -#endif // CONFIG_EKF2_EXTERNAL_VISION - - if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) { - bad_acc_bias = true; - } - } - } - - // record the pass/fail - if (!bad_acc_bias) { - _fault_status.flags.bad_acc_bias = false; - _time_acc_bias_check = _time_delayed_us; - - } else { - _fault_status.flags.bad_acc_bias = true; - } - - // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of - // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue - if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { - - P.uncorrelateCovariance<3>(13); - - _time_acc_bias_check = _time_delayed_us; - _fault_status.flags.bad_acc_bias = false; - _warning_events.flags.invalid_accel_bias_cov_reset = true; - ECL_WARN("invalid accel bias - covariance reset"); - - } else if (force_symmetry) { - // ensure the covariance values are symmetrical - P.makeRowColSymmetric<3>(13); - } - } + if (force_symmetry) { + P.makeRowColSymmetric(State::quat_nominal.idx); + P.makeRowColSymmetric(State::vel.idx); + P.makeRowColSymmetric(State::pos.idx); + P.makeRowColSymmetric(State::gyro_bias.idx); + P.makeRowColSymmetric(State::accel_bias.idx); + } + +#if defined(CONFIG_EKF2_MAGNETOMETER) // magnetic field states if (!_control_status.flags.mag) { - P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.0f); + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.0f); } else { - // constrain variances - for (int i = 16; i <= 18; i++) { - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]); - } + const float mag_I_var_max = 1.f; + constrainStateVar(State::mag_I, 0.f, mag_I_var_max); - for (int i = 19; i <= 21; i++) { - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]); - } + const float mag_B_var_max = 1.f; + constrainStateVar(State::mag_B, 0.f, mag_B_var_max); - // force symmetry if (force_symmetry) { - P.makeRowColSymmetric<3>(16); - P.makeRowColSymmetric<3>(19); + P.makeRowColSymmetric(State::mag_I.idx); + P.makeRowColSymmetric(State::mag_B.idx); } - } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) // wind velocity states if (!_control_status.flags.wind) { - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); } else { - // constrain variances - for (int i = 22; i <= 23; i++) { - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]); - } + const float wind_vel_var_max = 1e6f; + constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max); - // force symmetry if (force_symmetry) { - P.makeRowColSymmetric<2>(22); + P.makeRowColSymmetric(State::wind_vel.idx); } } +#endif // CONFIG_EKF2_WIND +} + +void Ekf::constrainStateVar(const IdxDof &state, float min, float max) +{ + for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { + P(i, i) = math::constrain(P(i, i), min, max); + } } // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected -bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) +bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP) { bool healthy = true; - for (int i = 0; i < _k_num_states; i++) { + for (int i = 0; i < State::size; i++) { if (P(i, i) < KHP(i, i)) { P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); healthy = false; @@ -526,29 +352,29 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) return healthy; } -void Ekf::resetQuatCov() +void Ekf::resetQuatCov(const float yaw_noise) { - zeroQuatCov(); - - // define the initial angle uncertainty as variances for a rotation vector - Vector3f rot_vec_var; - rot_vec_var.setAll(sq(_params.initial_tilt_err)); - - initialiseQuatCovariances(rot_vec_var); + const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f)); + float yaw_var = sq(0.01f); // update the yaw angle variance using the variance of the measurement - if (_params.mag_fusion_type != MagFuseType::NONE) { + if (PX4_ISFINITE(yaw_noise)) { // using magnetic heading tuning parameter - increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); + yaw_var = math::max(sq(yaw_noise), yaw_var); } + + resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var)); } -void Ekf::zeroQuatCov() +void Ekf::resetQuatCov(const Vector3f &rot_var_ned) { - P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); + matrix::SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov); + q_cov.copyLowerToUpperTriangle(); + resetStateCovariance(q_cov); } +#if defined(CONFIG_EKF2_MAGNETOMETER) void Ekf::resetMagCov() { if (_mag_decl_cov_reset) { @@ -556,15 +382,16 @@ void Ekf::resetMagCov() _mag_decl_cov_reset = false; } - P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise)); - P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, sq(_params.mag_noise)); + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); saveMagCovData(); } +#endif // CONFIG_EKF2_MAGNETOMETER void Ekf::resetGyroBiasZCov() { const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias); - P.uncorrelateCovarianceSetVariance<1>(12, init_gyro_bias_var); + P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var); } diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index ce7730d938..ca858cd06b 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -37,17 +37,16 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h" -#include "python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h" +#include +#include #include -void Ekf::controlDragFusion() +void Ekf::controlDragFusion(const imuSample &imu_delayed) { - if ((_params.drag_ctrl > 0) && _drag_buffer && - !_control_status.flags.fake_pos && _control_status.flags.in_air) { + if ((_params.drag_ctrl > 0) && _drag_buffer) { - if (!_control_status.flags.wind) { + if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) { // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; resetWindToZero(); @@ -55,7 +54,7 @@ void Ekf::controlDragFusion() dragSample drag_sample; - if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) { + if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) { fuseDrag(drag_sample); } } @@ -85,16 +84,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample) _state.vel(2)); const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); const float rel_wind_speed = rel_wind_body.norm(); - const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector(); + const VectorState state_vector_prev = _state.vector(); - Vector2f bcoef_inv; + Vector2f bcoef_inv{0.f, 0.f}; if (using_bcoef_x) { - bcoef_inv(0) = 1.0f / _params.bcoef_x; + bcoef_inv(0) = 1.f / _params.bcoef_x; } if (using_bcoef_y) { - bcoef_inv(1) = 1.0f / _params.bcoef_y; + bcoef_inv(1) = 1.f / _params.bcoef_y; } if (using_bcoef_x && using_bcoef_y) { @@ -105,7 +104,12 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bcoef_inv(1) = bcoef_inv(0); } - Vector24f Kfusion; + _aid_src_drag.timestamp_sample = drag_sample.time_us; + _aid_src_drag.fused = false; + + bool fused[] {false, false}; + + VectorState Kfusion; // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { @@ -115,36 +119,51 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // Drag is modelled as an arbitrary combination of bluff body drag that proportional to // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. + const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; + + _aid_src_drag.observation[axis_index] = mea_acc; + _aid_src_drag.observation_variance[axis_index] = R_ACC; + _aid_src_drag.innovation[axis_index] = pred_acc - mea_acc; + _aid_src_drag.innovation_variance[axis_index] = NAN; // reset if (axis_index == 0) { + sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, + &_aid_src_drag.innovation_variance[axis_index], &Kfusion); + if (!using_bcoef_x && !using_mcoef) { continue; } - sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion); - } else if (axis_index == 1) { + sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, + &_aid_src_drag.innovation_variance[axis_index], &Kfusion); + if (!using_bcoef_y && !using_mcoef) { continue; } - - sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion); } - if (_drag_innov_var(axis_index) < R_ACC) { + if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) { // calculation is badly conditioned return; } - const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; - // Apply an innovation consistency check with a 5 Sigma threshold - _drag_innov(axis_index) = pred_acc - mea_acc; - _drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index)); + const float innov_gate = 5.f; + setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate); - // if the innovation consistency check fails then don't fuse the sample - if (_drag_test_ratio(axis_index) <= 1.0f) { - measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index)); + if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos + && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index]) + && (_aid_src_drag.test_ratio[axis_index] < 1.f) + ) { + if (measurementUpdate(Kfusion, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) { + fused[axis_index] = true; + } } } + + if (fused[0] && fused[1]) { + _aid_src_drag.fused = true; + _aid_src_drag.time_last_fuse = _time_delayed_us; + } } diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 2d172ae670..934ff06d5e 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -57,14 +57,20 @@ void Ekf::reset() { ECL_INFO("reset"); + _state.quat_nominal.setIdentity(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); _state.accel_bias.setZero(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) _state.mag_I.setZero(); _state.mag_B.setZero(); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) _state.wind_vel.setZero(); - _state.quat_nominal.setIdentity(); +#endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); @@ -86,7 +92,11 @@ void Ekf::reset() _prev_gyro_bias_var.zero(); _prev_accel_bias_var.zero(); +#if defined(CONFIG_EKF2_GNSS) resetGpsDriftCheckFilters(); + _gps_checks_passed = false; +#endif // CONFIG_EKF2_GNSS + _gps_alt_ref = NAN; _output_predictor.reset(); @@ -102,23 +112,26 @@ void Ekf::reset() _time_last_hor_vel_fuse = 0; _time_last_ver_vel_fuse = 0; _time_last_heading_fuse = 0; - _time_last_zero_velocity_fuse = 0; _last_known_pos.setZero(); _time_acc_bias_check = 0; - _gps_checks_passed = false; - _gps_alt_ref = NAN; - +#if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_MAGNETOMETER) _mag_counter = 0; +#endif // CONFIG_EKF2_MAGNETOMETER _time_bad_vert_accel = 0; _time_good_vert_accel = 0; _clip_counter = 0; +#if defined(CONFIG_EKF2_BAROMETER) resetEstimatorAidStatus(_aid_src_baro_hgt); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) resetEstimatorAidStatus(_aid_src_airspeed); #endif // CONFIG_EKF2_AIRSPEED @@ -136,16 +149,20 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_ev_yaw); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) resetEstimatorAidStatus(_aid_src_gnss_hgt); resetEstimatorAidStatus(_aid_src_gnss_pos); resetEstimatorAidStatus(_aid_src_gnss_vel); -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) resetEstimatorAidStatus(_aid_src_gnss_yaw); -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_MAGNETOMETER) resetEstimatorAidStatus(_aid_src_mag_heading); resetEstimatorAidStatus(_aid_src_mag); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AUXVEL) resetEstimatorAidStatus(_aid_src_aux_vel); @@ -159,6 +176,8 @@ void Ekf::reset() #if defined(CONFIG_EKF2_RANGE_FINDER) resetEstimatorAidStatus(_aid_src_rng_hgt); #endif // CONFIG_EKF2_RANGE_FINDER + + _zero_velocity_update.reset(); } bool Ekf::update() @@ -179,6 +198,14 @@ bool Ekf::update() // TODO: explicitly pop at desired time horizon const imuSample imu_sample_delayed = _imu_buffer.get_oldest(); + // calculate an average filter update time + // filter and limit input between -50% and +100% of nominal value + float input = 0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt); + float filter_update_s = 1e-6f * _params.filter_update_interval_us; + _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s); + + updateIMUBiasInhibit(imu_sample_delayed); + if (_filter_initialised) { // perform state and covariance prediction for the main filter predictCovariance(imu_sample_delayed); @@ -187,10 +214,10 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) // run a separate filter for terrain estimation runTerrainEstimator(imu_sample_delayed); -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN updated = true; } @@ -228,10 +255,10 @@ bool Ekf::initialiseFilter() // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) // Initialise the terrain estimator initHagl(); -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN return true; } @@ -288,13 +315,6 @@ void Ekf::predictState(const imuSample &imu_delayed) constrainStates(); - // calculate an average filter update time - float input = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt); - - // filter and limit input between -50% and +100% of nominal value - const float filter_update_s = 1e-6f * _params.filter_update_interval_us; - input = math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s); - _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; // some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication // protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate @@ -316,4 +336,8 @@ void Ekf::predictState(const imuSample &imu_delayed) // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic // Note fixed coefficients are used to save operations. The exact time constant is not important. _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt; + + // Calculate low pass filtered height rate + float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant + _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8498b6baa4..91f2ed3ab3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -45,28 +45,31 @@ #include "estimator_interface.h" -#include "EKFGSF_yaw.h" +#if defined(CONFIG_EKF2_GNSS) +# include "EKFGSF_yaw.h" +#endif // CONFIG_EKF2_GNSS + #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" #include "position_bias_estimator.hpp" +#include + #include #include #include +#include "aid_sources/ZeroGyroUpdate.hpp" +#include "aid_sources/ZeroVelocityUpdate.hpp" + enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface { public: - static constexpr uint8_t _k_num_states{24}; ///< number of EKF states - - typedef matrix::Vector Vector24f; - typedef matrix::SquareMatrix SquareMatrix24f; + typedef matrix::Vector VectorState; + typedef matrix::SquareMatrix SquareMatrixState; typedef matrix::SquareMatrix Matrix2f; - template - - using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; Ekf() { @@ -81,37 +84,16 @@ public: // should be called every time new data is pushed into the filter bool update(); - void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; + static uint8_t getNumberOfStates() { return State::size; } -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; -#endif // CONFIG_EKF2_EXTERNAL_VISION + const StateSample &state() const { return _state; } - void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; } - void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } - void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; } - -#if defined(CONFIG_EKF2_RANGE_FINDER) - // range height - const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } - const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } - - void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } - void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } - - void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } - void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } - void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } - - void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } - void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } - void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } +#if defined(CONFIG_EKF2_BAROMETER) + const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } + const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } +#endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_TERRAIN) // terrain estimate bool isTerrainEstimateValid() const; @@ -125,15 +107,30 @@ public: // get the terrain variance float get_terrain_var() const { return _terrain_var; } + +# if defined(CONFIG_EKF2_RANGE_FINDER) + const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; } +# endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } + const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } + + float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); } + float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); } + float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } - void getFlowInnov(float flow_innov[2]) const; - void getFlowInnovVar(float flow_innov_var[2]) const; - void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); } - const Vector2f &getFlowVelBody() const { return _flow_vel_body; } const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } @@ -142,160 +139,131 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } - - void getTerrainFlowInnov(float flow_innov[2]) const; - void getTerrainFlowInnovVar(float flow_innov_var[2]) const; - void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } - - const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } + const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; } + const Vector3f &getRefBodyRate() const { return _ref_body_rate; } + const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; } #endif // CONFIG_EKF2_OPTICAL_FLOW -#if defined(CONFIG_EKF2_AUXVEL) - void getAuxVelInnov(float aux_vel_innov[2]) const; - void getAuxVelInnovVar(float aux_vel_innov[2]) const; - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); } -#endif // CONFIG_EKF2_AUXVEL - - void getHeadingInnov(float &heading_innov) const + float getHeadingInnov() const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { - heading_innov = _aid_src_mag_heading.innovation; - return; - - } else if (_control_status.flags.mag_3D) { - heading_innov = Vector3f(_aid_src_mag.innovation).max(); - return; + return _aid_src_mag_heading.innovation; } + if (_control_status.flags.mag_3D) { + return Vector3f(_aid_src_mag.innovation).max(); + } +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - heading_innov = _aid_src_gnss_yaw.innovation; - return; + return _aid_src_gnss_yaw.innovation; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - heading_innov = _aid_src_ev_yaw.innovation; - return; + return _aid_src_ev_yaw.innovation; } #endif // CONFIG_EKF2_EXTERNAL_VISION + + return 0.f; } - void getHeadingInnovVar(float &heading_innov_var) const + float getHeadingInnovVar() const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { - heading_innov_var = _aid_src_mag_heading.innovation_variance; - return; - - } else if (_control_status.flags.mag_3D) { - heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max(); - return; + return _aid_src_mag_heading.innovation_variance; } + if (_control_status.flags.mag_3D) { + return Vector3f(_aid_src_mag.innovation_variance).max(); + } +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - heading_innov_var = _aid_src_gnss_yaw.innovation_variance; - return; + return _aid_src_gnss_yaw.innovation_variance; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - heading_innov_var = _aid_src_ev_yaw.innovation_variance; - return; + return _aid_src_ev_yaw.innovation_variance; } #endif // CONFIG_EKF2_EXTERNAL_VISION + + return 0.f; } - void getHeadingInnovRatio(float &heading_innov_ratio) const + float getHeadingInnovRatio() const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { - heading_innov_ratio = _aid_src_mag_heading.test_ratio; - return; - - } else if (_control_status.flags.mag_3D) { - heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); - return; + return _aid_src_mag_heading.test_ratio; } + if (_control_status.flags.mag_3D) { + return Vector3f(_aid_src_mag.test_ratio).max(); + } +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - heading_innov_ratio = _aid_src_gnss_yaw.test_ratio; - return; + return _aid_src_gnss_yaw.test_ratio; } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - heading_innov_ratio = _aid_src_ev_yaw.test_ratio; - return; + return _aid_src_ev_yaw.test_ratio; } #endif // CONFIG_EKF2_EXTERNAL_VISION - } - void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); } - void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); } - void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); } + return 0.f; + } #if defined(CONFIG_EKF2_DRAG_FUSION) - void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } - void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); } - void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); } + const auto &aid_src_drag() const { return _aid_src_drag; } #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_AIRSPEED) - void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; } - void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; } - void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; } -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_SIDESLIP) - void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; } - void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; } - void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; } -#endif // CONFIG_EKF2_SIDESLIP - - void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); } - void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); } - void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); } +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + const auto &aid_src_gravity() const { return _aid_src_gravity; } +#endif // CONFIG_EKF2_GRAVITY_FUSION // get the state vector at the delayed time horizon - matrix::Vector getStateAtFusionHorizonAsVector() const; + const matrix::Vector &getStateAtFusionHorizonAsVector() const { return _state.vector(); } +#if defined(CONFIG_EKF2_WIND) // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; + Vector2f getWindVelocityVariance() const { return getStateVariance(); } +#endif // CONFIG_EKF2_WIND - // get the wind velocity var - Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); } + template + matrix::VectorgetStateVariance() const { return P.slice(S.idx, S.idx).diag(); } // calling getStateCovariance().diag() uses more flash space + + template + matrix::SquareMatrixgetStateCovariance() const { return P.slice(S.idx, S.idx); } // get the full covariance matrix - const matrix::SquareMatrix &covariances() const { return P; } + const matrix::SquareMatrix &covariances() const { return P; } + float stateCovariance(unsigned r, unsigned c) const { return P(r, c); } // get the diagonal elements of the covariance matrix - matrix::Vector covariances_diagonal() const { return P.diag(); } + matrix::Vector covariances_diagonal() const { return P.diag(); } - // get the orientation (quaterion) covariances - matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } - - // get the linear velocity covariances - matrix::SquareMatrix velocity_covariances() const { return P.slice<3, 3>(4, 4); } - - // get the position covariances - matrix::SquareMatrix position_covariances() const { return P.slice<3, 3>(7, 7); } - - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - bool collect_gps(const gpsMessage &gps) override; + matrix::Vector getQuaternionVariance() const { return getStateVariance(); } + Vector3f getVelocityVariance() const { return getStateVariance(); }; + Vector3f getPositionVariance() const { return getStateVariance(); } // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); - float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } - bool setEkfGlobalOriginAltitude(const float altitude); - - // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -310,20 +278,12 @@ public: // Reset all IMU bias states and covariances to initial alignment values. void resetImuBias(); + void resetGyroBias(); + void resetGyroBiasCov(); + void resetAccelBias(); - - Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; - - Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); } - - // First argument returns GPS drift metrics in the following array locations - // 0 : Horizontal position drift rate (m/s) - // 1 : Vertical position drift rate (m/s) - // 2 : Filtered horizontal velocity (m/s) - // Second argument returns true when IMU movement is blocking the drift calculation - // Function returns true if the metrics have been updated and not returned previously by this function - bool get_gps_drift_metrics(float drift[3], bool *blocked); + void resetAccelBiasCov(); // return true if the global position estimate is valid // return true if the origin is set we are not doing unconstrained free inertial navigation @@ -351,37 +311,46 @@ public: bool isYawFinalAlignComplete() const { +#if defined(CONFIG_EKF2_MAGNETOMETER) const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); const bool is_mag_alignment_in_flight_complete = is_using_mag && _control_status.flags.mag_aligned_in_flight && ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6); return _control_status.flags.yaw_align && (is_mag_alignment_in_flight_complete || !is_using_mag); +#else + return _control_status.flags.yaw_align; +#endif } - // gyro bias (states 10, 11, 12) - const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s - Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)}; } // get the gyroscope bias variance in rad/s - float getGyroBiasLimit() const { return _params.gyro_bias_lim; } + // fuse single velocity and position measurement + bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); - // accel bias (states 13, 14, 15) + // gyro bias + const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s + Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s + float getGyroBiasLimit() const { return _params.gyro_bias_lim; } + float getGyroNoise() const { return _params.gyro_noise; } + + // accel bias const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 - Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2 + Vector3f getAccelBiasVariance() const { return getStateVariance(); } // get the accelerometer bias variance in m/s**2 float getAccelBiasLimit() const { return _params.acc_bias_lim; } +#if defined(CONFIG_EKF2_MAGNETOMETER) const Vector3f &getMagEarthField() const { return _state.mag_I; } - // mag bias (states 19, 20, 21) const Vector3f &getMagBias() const { return _state.mag_B; } Vector3f getMagBiasVariance() const { if (_control_status.flags.mag) { - return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; + return getStateVariance(); } return _saved_mag_bf_covmat.diag(); } float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss +#endif // CONFIG_EKF2_MAGNETOMETER bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; } @@ -443,31 +412,7 @@ public: Vector3f calcRotVecVariances() const; float getYawVar() const; - // set minimum continuous period without GPS fail required to mark a healthy GPS status - void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } - - const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; } - const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; } - - bool gps_checks_passed() const { return _gps_checks_passed; }; - - // get solution data from the EKF-GSF emergency yaw estimator - // returns false when data is not available - bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); - - // Returns true if the output of the yaw emergency estimator can be used for a reset - bool isYawEmergencyEstimateAvailable() const; - uint8_t getHeightSensorRef() const { return _height_sensor_ref; } - const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } - const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } - - const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } -#endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AIRSPEED) const auto &aid_src_airspeed() const { return _aid_src_airspeed; } @@ -477,8 +422,6 @@ public: const auto &aid_src_sideslip() const { return _aid_src_sideslip; } #endif // CONFIG_EKF2_SIDESLIP - const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } - const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; } const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } @@ -487,25 +430,83 @@ public: const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; } const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; } const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; } + + const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } + const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined + bool collect_gps(const gpsMessage &gps) override; + + // set minimum continuous period without GPS fail required to mark a healthy GPS status + void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } + + const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; } + const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; } + + bool gps_checks_passed() const { return _gps_checks_passed; }; + + const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } + const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; } const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; } const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; } -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; } -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW + // Returns true if the output of the yaw emergency estimator can be used for a reset + bool isYawEmergencyEstimateAvailable() const; + + // get solution data from the EKF-GSF emergency yaw estimator + // returns false when data is not available + bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_MAGNETOMETER) const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } const auto &aid_src_mag() const { return _aid_src_mag; } - - const auto &aid_src_gravity() const { return _aid_src_gravity; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AUXVEL) const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL + + bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) + { + clearInhibitedStateKalmanGains(K); + + const VectorState KS = K * innovation_variance; + SquareMatrixState KHP; + + for (unsigned row = 0; row < State::size; row++) { + for (unsigned col = 0; col < State::size; col++) { + // Instad of literally computing KHP, use an equvalent + // equation involving less mathematical operations + KHP(row, col) = KS(row) * K(col); + } + } + + const bool is_healthy = checkAndFixCovarianceUpdate(KHP); + + if (is_healthy) { + // apply the covariance corrections + P -= KHP; + + fixCovarianceErrors(true); + + // apply the state corrections + fuse(K, innovation); + } + + return is_healthy; + } + private: // set the internal states and status to their default value @@ -541,16 +542,10 @@ private: Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) - stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised - float _mag_heading_prev{}; ///< previous value of mag heading (rad) - float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad) - - // booleans true when fresh sensor data is available at the fusion time horizon - bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused - uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) uint64_t _time_last_v_pos_aiding{0}; uint64_t _time_last_v_vel_aiding{0}; @@ -560,7 +555,6 @@ private: uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_heading_fuse{0}; - uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -570,60 +564,52 @@ private: Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - // zero gyro update - Vector3f _zgup_delta_ang{}; - float _zgup_delta_ang_dt{0.f}; - - // used by magnetometer fusion mode selection Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) + float _height_rate_lpf{0.0f}; float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) - bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable - bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable - uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected - float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad) - bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. - uint8_t _nb_mag_heading_reset_available{0}; - uint8_t _nb_mag_3d_reset_available{0}; - uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time - SquareMatrix24f P{}; ///< state covariance matrix + SquareMatrixState P{}; ///< state covariance matrix #if defined(CONFIG_EKF2_DRAG_FUSION) - Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) - Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) + estimator_aid_source2d_s _aid_src_drag{}; #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_rng_hgt{}; - - HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; - - float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) - float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) - float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio - - uint64_t _time_last_healthy_rng_data{0}; - +#if defined(CONFIG_EKF2_TERRAIN) // Terrain height state estimation float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator - terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground + terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) + +# if defined(CONFIG_EKF2_RANGE_FINDER) + estimator_aid_source1d_s _aid_src_terrain_range_finder{}; + uint64_t _time_last_healthy_rng_data{0}; +# endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + estimator_aid_source1d_s _aid_src_rng_hgt{}; + HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) estimator_aid_source2d_s _aid_src_optical_flow{}; - estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) Vector2f _flow_vel_ne{}; ///< velocity from corrected flow measurement (local frame) (m/s) Vector3f _imu_del_ang_of{}; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) + Vector3f _ref_body_rate{}; + Vector3f _measured_body_rate{}; float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec) @@ -631,10 +617,8 @@ private: Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon - uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) #endif // CONFIG_EKF2_OPTICAL_FLOW - estimator_aid_source1d_s _aid_src_baro_hgt{}; #if defined(CONFIG_EKF2_AIRSPEED) estimator_aid_source1d_s _aid_src_airspeed{}; #endif // CONFIG_EKF2_AIRSPEED @@ -658,23 +642,9 @@ private: uint8_t _nb_ev_yaw_reset_available{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION - estimator_aid_source1d_s _aid_src_gnss_hgt{}; - estimator_aid_source2d_s _aid_src_gnss_pos{}; - estimator_aid_source3d_s _aid_src_gnss_vel{}; - -#if defined(CONFIG_EKF2_GNSS_YAW) - estimator_aid_source1d_s _aid_src_gnss_yaw{}; - uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source -#endif // CONFIG_EKF2_GNSS_YAW - - estimator_aid_source1d_s _aid_src_mag_heading{}; - estimator_aid_source3d_s _aid_src_mag{}; - - estimator_aid_source3d_s _aid_src_gravity{}; - -#if defined(CONFIG_EKF2_AUXVEL) - estimator_aid_source2d_s _aid_src_aux_vel{}; -#endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_GNSS) + // booleans true when fresh sensor data is available at the fusion time horizon + bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) @@ -687,19 +657,67 @@ private: uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed - // Variables used to publish the WGS-84 location of the EKF local NED origin - float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + gps_check_fail_status_u _gps_check_fail_status{}; + // height sensor status + bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent + + HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; + + estimator_aid_source1d_s _aid_src_gnss_hgt{}; + estimator_aid_source2d_s _aid_src_gnss_pos{}; + estimator_aid_source3d_s _aid_src_gnss_vel{}; + +# if defined(CONFIG_EKF2_GNSS_YAW) + estimator_aid_source1d_s _aid_src_gnss_yaw{}; + uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + estimator_aid_source3d_s _aid_src_gravity{}; +#endif // CONFIG_EKF2_GRAVITY_FUSION + +#if defined(CONFIG_EKF2_AUXVEL) + estimator_aid_source2d_s _aid_src_aux_vel{}; +#endif // CONFIG_EKF2_AUXVEL // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; - uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation - uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) +#if defined(CONFIG_EKF2_BAROMETER) + estimator_aid_source1d_s _aid_src_baro_hgt{}; + // Variables used to perform in flight resets and switch between height sources - AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) + uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation + + HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; + + bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_MAGNETOMETER) + float _mag_heading_prev{}; ///< previous value of mag heading (rad) + float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad) + + // used by magnetometer fusion mode selection + bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable + bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable + uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected + AlphaFilter _mag_heading_innov_lpf{0.1f}; + float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad) + bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. + uint8_t _nb_mag_heading_reset_available{0}; + uint8_t _nb_mag_3d_reset_available{0}; + uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time + + estimator_aid_source1d_s _aid_src_mag_heading{}; + estimator_aid_source3d_s _aid_src_mag{}; + + AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) + uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation // Variables used to control activation of post takeoff functionality uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) @@ -707,8 +725,7 @@ private: uint64_t _time_last_mag_check_failing{0}; Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2) - - gps_check_fail_status_u _gps_check_fail_status{}; +#endif // CONFIG_EKF2_MAGNETOMETER // variables used to inhibit accel bias learning bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis @@ -720,17 +737,11 @@ private: Vector3f _prev_gyro_bias_var{}; ///< saved gyro XYZ bias variances Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances - // height sensor status - bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags - bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent - // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) uint16_t _clip_counter{0}; ///< counter that increments when clipping ad decrements when not - float _height_rate_lpf{0.0f}; - // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); @@ -743,28 +754,23 @@ private: // predict ekf covariance void predictCovariance(const imuSample &imu_delayed); - // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); + template + void resetStateCovariance(const matrix::SquareMatrix &cov) + { + P.uncorrelateCovarianceSetVariance(S.idx, 0.0f); + P.slice(S.idx, S.idx) = cov; + } // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status); - bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW); - void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const; + bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); + void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; -#if defined(CONFIG_EKF2_GNSS_YAW) - void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); + void updateIMUBiasInhibit(const imuSample &imu_delayed); - // fuse the yaw angle obtained from a dual antenna GPS unit - void fuseGpsYaw(); - - // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit - // return true if the reset was successful - bool resetYawToGps(const float gnss_yaw); - - void updateGpsYaw(const gpsSample &gps_sample); - -#endif // CONFIG_EKF2_GNSS_YAW - void stopGpsYawFusion(); +#if defined(CONFIG_EKF2_MAGNETOMETER) + // ekf sequential fusion of magnetometer measurements + bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians @@ -772,6 +778,7 @@ private: // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) // control fusion of air data observations @@ -784,9 +791,6 @@ private: // Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip void resetWindUsingAirspeed(const airspeedSample &airspeed_sample); - - // perform a limited reset of the wind state covariances - void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) @@ -800,15 +804,12 @@ private: #if defined(CONFIG_EKF2_DRAG_FUSION) // control fusion of multi-rotor drag specific force observations - void controlDragFusion(); + void controlDragFusion(const imuSample &imu_delayed); // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(const dragSample &drag_sample); #endif // CONFIG_EKF2_DRAG_FUSION - // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); - void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var = Vector3f(NAN, NAN, NAN)); void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var); @@ -844,12 +845,7 @@ private: void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); -#if defined(CONFIG_EKF2_RANGE_FINDER) - // range height - void controlRangeHeightFusion(); - bool isConditionalRangeAidSuitable(); - void stopRngHgtFusion(); - +#if defined(CONFIG_EKF2_TERRAIN) // terrain vertical position estimator void initHagl(); void runTerrainEstimator(const imuSample &imu_delayed); @@ -857,16 +853,34 @@ private: float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } + void controlHaglFakeFusion(); + void terrainHandleVerticalPositionReset(float delta_z); + +# if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder void controlHaglRngFusion(); - void fuseHaglRng(); - void startHaglRngFusion(); - void resetHaglRngIfNeeded(); + void updateHaglRng(estimator_aid_source1d_s &aid_src) const; + void fuseHaglRng(estimator_aid_source1d_s &aid_src); void resetHaglRng(); void stopHaglRngFusion(); - float getRngVar(); + float getRngVar() const; +# endif // CONFIG_EKF2_RANGE_FINDER - void controlHaglFakeFusion(); +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + // update the terrain vertical position estimate using an optical flow measurement + void controlHaglFlowFusion(); + void resetHaglFlow(); + void stopHaglFlowFusion(); + void fuseFlowForTerrain(estimator_aid_source2d_s &flow); +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + void controlRangeHeightFusion(); + bool isConditionalRangeAidSuitable(); + void stopRngHgtFusion(); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -889,108 +903,74 @@ private: void fuseOptFlow(); float predictFlowRange(); Vector2f predictFlowVelBody(); - - // update the terrain vertical position estimate using an optical flow measurement - void controlHaglFlowFusion(); - void startHaglFlowFusion(); - void resetHaglFlow(); - void stopHaglFlowFusion(); - void fuseFlowForTerrain(estimator_aid_source2d_s &flow); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_MAGNETOMETER) // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); +#endif // CONFIG_EKF2_MAGNETOMETER - void clearInhibitedStateKalmanGains(Vector24f &K) const + void clearInhibitedStateKalmanGains(VectorState &K) const { - // gyro bias: states 10, 11, 12 - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < State::gyro_bias.dof; i++) { if (_gyro_bias_inhibit[i]) { - K(10 + i) = 0.f; + K(State::gyro_bias.idx + i) = 0.f; } } - // accel bias: states 13, 14, 15 - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < State::accel_bias.dof; i++) { if (_accel_bias_inhibit[i]) { - K(13 + i) = 0.f; + K(State::accel_bias.idx + i) = 0.f; } } - // mag I: states 16, 17, 18 +#if defined(CONFIG_EKF2_MAGNETOMETER) if (!_control_status.flags.mag) { - K(16) = 0.f; - K(17) = 0.f; - K(18) = 0.f; + for (unsigned i = 0; i < State::mag_I.dof; i++) { + K(State::mag_I.idx + i) = 0.f; + } } - // mag B: states 19, 20, 21 if (!_control_status.flags.mag) { - K(19) = 0.f; - K(20) = 0.f; - K(21) = 0.f; + for (unsigned i = 0; i < State::mag_B.dof; i++) { + K(State::mag_B.idx + i) = 0.f; + } } +#endif // CONFIG_EKF2_MAGNETOMETER - // wind: states 22, 23 +#if defined(CONFIG_EKF2_WIND) if (!_control_status.flags.wind) { - K(22) = 0.f; - K(23) = 0.f; - } - } - - bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation) - { - clearInhibitedStateKalmanGains(K); - - const Vector24f KS = K * innovation_variance; - SquareMatrix24f KHP; - - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned col = 0; col < _k_num_states; col++) { - // Instad of literally computing KHP, use an equvalent - // equation involving less mathematical operations - KHP(row, col) = KS(row) * K(col); + for (unsigned i = 0; i < State::wind_vel.dof; i++) { + K(State::wind_vel.idx + i) = 0.f; } } - - const bool is_healthy = checkAndFixCovarianceUpdate(KHP); - - if (is_healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(K, innovation); - } - - return is_healthy; +#endif // CONFIG_EKF2_WIND } // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected - bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP); + bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); // limit the diagonal of the covariance matrix // force symmetry when the argument is true void fixCovarianceErrors(bool force_symmetry); + void constrainStateVar(const IdxDof &state, float min, float max); + // constrain the ekf states void constrainStates(); // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value - void fuse(const Vector24f &K, float innovation); + void fuse(const VectorState &K, float innovation); +#if defined(CONFIG_EKF2_BARO_COMPENSATION) float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const; +#endif // CONFIG_EKF2_BARO_COMPENSATION // calculate the earth rotation vector from a given latitude Vector3f calcEarthRateNED(float lat_rad) const; - // return true id the GPS quality is good enough to set an origin and start aiding - bool gps_is_good(const gpsMessage &gps); - // Control the filter fusion modes void controlFusionModes(const imuSample &imu_delayed); @@ -1011,11 +991,49 @@ private: void stopEvYawFusion(); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) // control fusion of GPS observations void controlGpsFusion(const imuSample &imu_delayed); + void stopGpsFusion(); + bool shouldResetGpsFusion() const; + + // return true id the GPS quality is good enough to set an origin and start aiding + bool gps_is_good(const gpsMessage &gps); + + void controlGnssHeightFusion(const gpsSample &gps_sample); + void stopGpsHgtFusion(); + + void resetGpsDriftCheckFilters(); + +# if defined(CONFIG_EKF2_GNSS_YAW) + void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); + void stopGpsYawFusion(); + + // fuse the yaw angle obtained from a dual antenna GPS unit + void fuseGpsYaw(); + + // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit + // return true if the reset was successful + bool resetYawToGps(const float gnss_yaw); + + void updateGpsYaw(const gpsSample &gps_sample); + +# endif // CONFIG_EKF2_GNSS_YAW + + // Declarations used to control use of the EKF-GSF yaw estimator bool isYawFailure() const; + // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator + // Returns true if the reset was successful + bool resetYawToEKFGSF(); + + // yaw estimator instance + EKFGSF_yaw _yawEstimator{}; + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations void controlMagFusion(); void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src); @@ -1029,10 +1047,24 @@ private: void checkYawAngleObservability(); void checkMagBiasObservability(); + void checkMagHeadingConsistency(); bool checkMagField(const Vector3f &mag); static bool isMeasuredMatchingExpected(float measured, float expected, float gate); + void stopMagHdgFusion(); + void stopMagFusion(); + + // load and save mag field state covariance data for re-use + void loadMagCovData(); + void saveMagCovData(); + + // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter + // sensor measurement + float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); + +#endif // CONFIG_EKF2_MAGNETOMETER + // control fusion of fake position observations to constrain drift void controlFakePosFusion(); @@ -1041,10 +1073,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroVelocityUpdate(); - void controlZeroGyroUpdate(const imuSample &imu_delayed); - void fuseDeltaAngBias(float innov, float innov_var, int obs_index); - void controlZeroInnovationHeadingUpdate(); #if defined(CONFIG_EKF2_AUXVEL) @@ -1053,56 +1081,45 @@ private: void stopAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL + void checkVerticalAccelerationBias(const imuSample &imu_delayed); void checkVerticalAccelerationHealth(const imuSample &imu_delayed); Likelihood estimateInertialNavFallingLikelihood() const; // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(const imuSample &imu_delayed); void checkHeightSensorRefFallback(); + +#if defined(CONFIG_EKF2_BAROMETER) void controlBaroHeightFusion(); - void controlGnssHeightFusion(const gpsSample &gps_sample); - - void stopMagHdgFusion(); - void stopMagFusion(); - void stopBaroHgtFusion(); - void stopGpsHgtFusion(); void updateGroundEffect(); +#endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity fusion: heuristically enable / disable gravity fusion void controlGravityFusion(const imuSample &imu_delayed); +#endif // CONFIG_EKF2_GRAVITY_FUSION - // initialise the quaternion covariances using rotation vector variances - // do not call before quaternion states are initialised - void initialiseQuatCovariances(Vector3f &rot_vec_var); - - void resetQuatCov(); - void zeroQuatCov(); + void resetQuatCov(const float yaw_noise = NAN); + void resetQuatCov(const Vector3f &euler_noise_ned); +#if defined(CONFIG_EKF2_MAGNETOMETER) void resetMagCov(); +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) // perform a reset of the wind states and related covariances void resetWind(); + void resetWindCov(); void resetWindToZero(); - - // Increase the yaw error variance of the quaternions - // Argument is additional yaw variance in rad**2 - void increaseQuatYawErrVariance(float yaw_variance); - - // load and save mag field state covariance data for re-use - void loadMagCovData(); - void saveMagCovData(); +#endif // CONFIG_EKF2_WIND void resetGyroBiasZCov(); // uncorrelate quaternion states from other states void uncorrelateQuatFromOtherStates(); - // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter - // sensor measurement - float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); - bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us); @@ -1118,29 +1135,19 @@ private: return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us); } - void stopGpsFusion(); - void resetFakePosFusion(); void stopFakePosFusion(); - void setVelPosStatus(const int index, const bool healthy); + void setVelPosStatus(const int state_index, const bool healthy); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) // yaw_variance : yaw error variance (rad^2) void resetQuatStateYaw(float yaw, float yaw_variance); - // Declarations used to control use of the EKF-GSF yaw estimator - - // yaw estimator instance - EKFGSF_yaw _yawEstimator{}; - uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; uint8_t _position_sensor_ref{static_cast(PositionSensor::GNSS)}; - HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; - HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; - #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; @@ -1148,13 +1155,6 @@ private: bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION - // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator - // Resets the horizontal velocity and position to the default navigation sensor - // Returns true if the reset was successful - bool resetYawToEKFGSF(); - - void resetGpsDriftCheckFilters(); - void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const { // only bother resetting if timestamp_sample is set @@ -1170,7 +1170,6 @@ private: status.innovation_variance = 0; status.test_ratio = INFINITY; - status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; } @@ -1194,7 +1193,6 @@ private: status.test_ratio[i] = INFINITY; } - status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; } @@ -1240,6 +1238,9 @@ private: // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; } + + ZeroGyroUpdate _zero_gyro_update{}; + ZeroVelocityUpdate _zero_velocity_update{}; }; #endif // !EKF_EKF_H diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9b82183c9d..62c1582472 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -40,10 +40,11 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/quat_var_to_rot_var.h" -#include "python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h" +#include +#include #include +#include #include void Ekf::resetHorizontalVelocityToZero() @@ -66,11 +67,11 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f _state.vel.xy() = new_horz_vel; if (PX4_ISFINITE(new_horz_vel_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(4, math::max(sq(0.01f), new_horz_vel_var(0))); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0))); } if (PX4_ISFINITE(new_horz_vel_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1))); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); } _output_predictor.resetHorizontalVelocityTo(_time_delayed_us, new_horz_vel); @@ -96,7 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) _state.vel(2) = new_vert_vel; if (PX4_ISFINITE(new_vert_vel_var)) { - P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var)); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var)); } _output_predictor.resetVerticalVelocityTo(_time_delayed_us, new_vert_vel); @@ -132,11 +133,11 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f _state.pos.xy() = new_horz_pos; if (PX4_ISFINITE(new_horz_pos_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(7, math::max(sq(0.01f), new_horz_pos_var(0))); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); } if (PX4_ISFINITE(new_horz_pos_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1))); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } _output_predictor.resetHorizontalPositionTo(_time_delayed_us, new_horz_pos); @@ -179,7 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v if (PX4_ISFINITE(new_vert_pos_var)) { // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var)); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); } // apply the change in height / height rate to our newest height / height rate estimate @@ -197,15 +198,23 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _state_reset_status.reset_count.posD++; +#if defined(CONFIG_EKF2_BAROMETER) _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_TERRAIN) + terrainHandleVerticalPositionReset(delta_z); +#endif + // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; } @@ -230,14 +239,19 @@ void Ekf::constrainStates() const float accel_bias_limit = getAccelBiasLimit(); _state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit); +#if defined(CONFIG_EKF2_MAGNETOMETER) _state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f); _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) _state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f); +#endif // CONFIG_EKF2_WIND } +#if defined(CONFIG_EKF2_BARO_COMPENSATION) float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const { -#if defined(CONFIG_EKF2_BARO_COMPENSATION) if (_control_status.flags.wind && local_position_is_valid()) { // calculate static pressure error = Pmeas - Ptruth // model position error sensitivity as a body fixed ellipse with a different scale in the positive and @@ -265,11 +279,11 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) // correct baro measurement using pressure error estimate and assuming sea level gravity return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G); } -#endif // CONFIG_EKF2_BARO_COMPENSATION // otherwise return the uncorrected baro measurement return baro_alt_uncompensated; } +#endif // CONFIG_EKF2_BARO_COMPENSATION // calculate the earth rotation vector Vector3f Ekf::calcEarthRateNED(float lat_rad) const @@ -279,125 +293,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } -void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_gnss_vel.innovation[0]; - hvel[1] = _aid_src_gnss_vel.innovation[1]; - vvel = _aid_src_gnss_vel.innovation[2]; - - hpos[0] = _aid_src_gnss_pos.innovation[0]; - hpos[1] = _aid_src_gnss_pos.innovation[1]; - vpos = _aid_src_gnss_hgt.innovation; -} - -void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_gnss_vel.innovation_variance[0]; - hvel[1] = _aid_src_gnss_vel.innovation_variance[1]; - vvel = _aid_src_gnss_vel.innovation_variance[2]; - - hpos[0] = _aid_src_gnss_pos.innovation_variance[0]; - hpos[1] = _aid_src_gnss_pos.innovation_variance[1]; - vpos = _aid_src_gnss_hgt.innovation_variance; -} - -void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const -{ - hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]); - vvel = _aid_src_gnss_vel.test_ratio[2]; - - hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]); - vpos = _aid_src_gnss_hgt.test_ratio; -} - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) -void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_ev_vel.innovation[0]; - hvel[1] = _aid_src_ev_vel.innovation[1]; - vvel = _aid_src_ev_vel.innovation[2]; - - hpos[0] = _aid_src_ev_pos.innovation[0]; - hpos[1] = _aid_src_ev_pos.innovation[1]; - vpos = _aid_src_ev_hgt.innovation; -} - -void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const -{ - hvel[0] = _aid_src_ev_vel.innovation_variance[0]; - hvel[1] = _aid_src_ev_vel.innovation_variance[1]; - vvel = _aid_src_ev_vel.innovation_variance[2]; - - hpos[0] = _aid_src_ev_pos.innovation_variance[0]; - hpos[1] = _aid_src_ev_pos.innovation_variance[1]; - vpos = _aid_src_ev_hgt.innovation_variance; -} - -void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const -{ - hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]); - vvel = _aid_src_ev_vel.test_ratio[2]; - - hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]); - vpos = _aid_src_ev_hgt.test_ratio; -} -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_AUXVEL) -void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const -{ - aux_vel_innov[0] = _aid_src_aux_vel.innovation[0]; - aux_vel_innov[1] = _aid_src_aux_vel.innovation[1]; -} - -void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const -{ - aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0]; - aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1]; -} -#endif // CONFIG_EKF2_AUXVEL - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) -void Ekf::getFlowInnov(float flow_innov[2]) const -{ - flow_innov[0] = _aid_src_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_optical_flow.innovation[1]; -} - -void Ekf::getFlowInnovVar(float flow_innov_var[2]) const -{ - flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; -} - -void Ekf::getTerrainFlowInnov(float flow_innov[2]) const -{ - flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; -} - -void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const -{ - flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; -} -#endif // CONFIG_EKF2_OPTICAL_FLOW - -// get the state vector at the delayed time horizon -matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const -{ - matrix::Vector state; - state.slice<4, 1>(0, 0) = _state.quat_nominal; - state.slice<3, 1>(4, 0) = _state.vel; - state.slice<3, 1>(7, 0) = _state.pos; - state.slice<3, 1>(10, 0) = _state.gyro_bias; - state.slice<3, 1>(13, 0) = _state.accel_bias; - state.slice<3, 1>(16, 0) = _state.mag_I; - state.slice<3, 1>(19, 0) = _state.mag_B; - state.slice<2, 1>(22, 0) = _state.wind_vel; - return state; -} - bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { origin_time = _pos_ref.getProjectionReferenceTimestamp(); @@ -430,6 +325,26 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; +#if defined(CONFIG_EKF2_MAGNETOMETER) + const float mag_declination_gps = get_mag_declination_radians(latitude, longitude); + const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude); + const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); + + if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { + _mag_declination_gps = mag_declination_gps; + _mag_inclination_gps = mag_inclination_gps; + _mag_strength_gps = mag_strength_gps; + + _wmm_gps_time_last_set = _time_delayed_us; + } +#endif // CONFIG_EKF2_MAGNETOMETER + + // We don't know the uncertainty of the origin + _gpos_origin_eph = 0.f; + _gpos_origin_epv = 0.f; + + _NED_origin_initialised = true; + // minimum change in position or height that triggers a reset static constexpr float MIN_RESET_DIST_M = 0.01f; @@ -447,11 +362,15 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons // determine current z float current_alt = -_state.pos(2) + gps_alt_ref_prev; +#if defined(CONFIG_EKF2_GNSS) const float gps_hgt_bias = _gps_hgt_b_est.getBias(); +#endif // CONFIG_EKF2_GNSS resetVerticalPositionTo(_gps_alt_ref - current_alt); +#if defined(CONFIG_EKF2_GNSS) // preserve GPS height bias _gps_hgt_b_est.setBias(gps_hgt_bias); +#endif // CONFIG_EKF2_GNSS } return true; @@ -466,15 +385,17 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const // report absolute accuracy taking into account the uncertainty in location of the origin // If not aiding, return 0 for horizontal position estimate as no estimate is available // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gps_origin_eph)); + float hpos_err = sqrtf(P.trace<2>(State::pos.idx) + sq(_gpos_origin_eph)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_control_status.flags.inertial_dead_reckoning) { +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { @@ -484,22 +405,24 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9) + sq(_gps_origin_epv)); + *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv)); } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const { // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8)); + float hpos_err = sqrtf(P.trace<2>(State::pos.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_horizontal_deadreckon_time_exceeded) { +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { @@ -509,13 +432,13 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9)); + *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2)); } // get the 1-sigma horizontal and vertical velocity uncertainty void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const { - float hvel_err = sqrtf(P(4, 4) + P(5, 5)); + float hvel_err = sqrtf(P.trace<2>(State::vel.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -530,9 +453,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { @@ -548,7 +473,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const } *ekf_evh = hvel_err; - *ekf_evv = sqrtf(P(6, 6)); + *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } /* @@ -581,9 +506,8 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; } -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_OPTICAL_FLOW) +# if defined(CONFIG_EKF2_OPTICAL_FLOW) // Keep within flow AND range sensor limits when exclusively using optical flow const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); @@ -601,7 +525,9 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = flow_hagl_min; *hagl_max = flow_hagl_max; } -#endif // CONFIG_EKF2_OPTICAL_FLOW +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_RANGE_FINDER } void Ekf::resetImuBias() @@ -615,12 +541,17 @@ void Ekf::resetGyroBias() // Zero the gyro bias states _state.gyro_bias.zero(); + resetGyroBiasCov(); +} + +void Ekf::resetGyroBiasCov() +{ // Zero the corresponding covariances and set // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias)); + P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); // Set previous frame values - _prev_gyro_bias_var = P.slice<3, 3>(10, 10).diag(); + _prev_gyro_bias_var = getStateVariance(); } void Ekf::resetAccelBias() @@ -628,12 +559,17 @@ void Ekf::resetAccelBias() // Zero the accel bias states _state.accel_bias.zero(); + resetAccelBiasCov(); +} + +void Ekf::resetAccelBiasCov() +{ // Zero the corresponding covariances and set // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias)); + P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); // Set previous frame values - _prev_accel_bias_var = P.slice<3, 3>(13, 13).diag(); + _prev_accel_bias_var = getStateVariance(); } // get EKF innovation consistency check status information comprising of: @@ -650,6 +586,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f // return the largest magnetometer innovation test ratio mag = 0.f; +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio)); } @@ -657,6 +594,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f if (_control_status.flags.mag_3D) { mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max())); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -674,6 +612,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f vel = NAN; pos = NAN; +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max()); vel = math::max(gps_vel, FLT_MIN); @@ -681,6 +620,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max()); pos = math::max(gps_pos, FLT_MIN); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { @@ -705,15 +645,19 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f float hgt_sum = 0.f; int n_hgt_sources = 0; +#if defined(CONFIG_EKF2_BAROMETER) if (_control_status.flags.baro_hgt) { hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio); n_hgt_sources++; } +#endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps_hgt) { hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio); n_hgt_sources++; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { @@ -741,11 +685,23 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f tas = sqrtf(_aid_src_airspeed.test_ratio); #endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_RANGE_FINDER) - // return the terrain height innovation test ratio - hagl = sqrtf(_hagl_test_ratio); + hagl = NAN; +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + if (_hagl_sensor_status.flags.range_finder) { + // return the terrain height innovation test ratio + hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio); + } #endif // CONFIG_EKF2_RANGE_FINDER +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (_hagl_sensor_status.flags.flow) { + // return the terrain height innovation test ratio + hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1])); + } +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + #if defined(CONFIG_EKF2_SIDESLIP) // return the synthetic sideslip innovation test ratio beta = sqrtf(_aid_src_sideslip.test_ratio); @@ -763,13 +719,16 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; +#if defined(CONFIG_EKF2_TERRAIN) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); +#endif // CONFIG_EKF2_TERRAIN soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; bool mag_innov_good = true; +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { if (_aid_src_mag_heading.test_ratio < 1.f) { mag_innov_good = false; @@ -780,34 +739,45 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const mag_innov_good = false; } } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_GNSS) const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; +#else + (void)mag_innov_good; +#endif // CONFIG_EKF2_GNSS + soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical; *status = soln_status.value; } -void Ekf::fuse(const Vector24f &K, float innovation) +void Ekf::fuse(const VectorState &K, float innovation) { - _state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation; + _state.quat_nominal -= K.slice(State::quat_nominal.idx, 0) * innovation; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); - _state.vel -= K.slice<3, 1>(4, 0) * innovation; - _state.pos -= K.slice<3, 1>(7, 0) * innovation; - _state.gyro_bias -= K.slice<3, 1>(10, 0) * innovation; - _state.accel_bias -= K.slice<3, 1>(13, 0) * innovation; - _state.mag_I -= K.slice<3, 1>(16, 0) * innovation; - _state.mag_B -= K.slice<3, 1>(19, 0) * innovation; - _state.wind_vel -= K.slice<2, 1>(22, 0) * innovation; + _state.vel -= K.slice(State::vel.idx, 0) * innovation; + _state.pos -= K.slice(State::pos.idx, 0) * innovation; + _state.gyro_bias -= K.slice(State::gyro_bias.idx, 0) * innovation; + _state.accel_bias -= K.slice(State::accel_bias.idx, 0) * innovation; + +#if defined(CONFIG_EKF2_MAGNETOMETER) + _state.mag_I -= K.slice(State::mag_I.idx, 0) * innovation; + _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) + _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; +#endif // CONFIG_EKF2_WIND } void Ekf::uncorrelateQuatFromOtherStates() { - P.slice<_k_num_states - 4, 4>(4, 0) = 0.f; - P.slice<4, _k_num_states - 4>(0, 4) = 0.f; + P.uncorrelateCovarianceBlock(State::quat_nominal.idx); } void Ekf::updateDeadReckoningStatus() @@ -883,128 +853,32 @@ void Ekf::updateVerticalDeadReckoningStatus() Vector3f Ekf::calcRotVecVariances() const { Vector3f rot_var; - sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var); + sym::QuatVarToRotVar(_state.vector(), P, FLT_EPSILON, &rot_var); return rot_var; } float Ekf::getYawVar() const { - Vector24f H_YAW; + VectorState H_YAW; float yaw_var = 0.f; computeYawInnovVarAndH(0.f, yaw_var, H_YAW); return yaw_var; } -// initialise the quaternion covariances using rotation vector variances -// do not call before quaternion states are initialised -void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) -{ - // calculate an equivalent rotation vector from the quaternion - float q0,q1,q2,q3; - if (_state.quat_nominal(0) >= 0.0f) { - q0 = _state.quat_nominal(0); - q1 = _state.quat_nominal(1); - q2 = _state.quat_nominal(2); - q3 = _state.quat_nominal(3); - - } else { - q0 = -_state.quat_nominal(0); - q1 = -_state.quat_nominal(1); - q2 = -_state.quat_nominal(2); - q3 = -_state.quat_nominal(3); - } - float delta = 2.0f*acosf(q0); - float scaler = (delta/sinf(delta*0.5f)); - float rotX = scaler*q1; - float rotY = scaler*q2; - float rotZ = scaler*q3; - - // autocode generated using matlab symbolic toolbox - float t2 = rotX*rotX; - float t4 = rotY*rotY; - float t5 = rotZ*rotZ; - float t6 = t2+t4+t5; - if (t6 > 1e-9f) { - float t7 = sqrtf(t6); - float t8 = t7*0.5f; - float t3 = sinf(t8); - float t9 = t3*t3; - float t10 = 1.0f/t6; - float t11 = 1.0f/sqrtf(t6); - float t12 = cosf(t8); - float t13 = 1.0f/powf(t6,1.5f); - float t14 = t3*t11; - float t15 = rotX*rotY*t3*t13; - float t16 = rotX*rotZ*t3*t13; - float t17 = rotY*rotZ*t3*t13; - float t18 = t2*t10*t12*0.5f; - float t27 = t2*t3*t13; - float t19 = t14+t18-t27; - float t23 = rotX*rotY*t10*t12*0.5f; - float t28 = t15-t23; - float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f; - float t25 = rotX*rotZ*t10*t12*0.5f; - float t31 = t16-t25; - float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f; - float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f; - float t24 = t15-t23; - float t26 = t16-t25; - float t29 = t4*t10*t12*0.5f; - float t34 = t3*t4*t13; - float t30 = t14+t29-t34; - float t32 = t5*t10*t12*0.5f; - float t40 = t3*t5*t13; - float t33 = t14+t32-t40; - float t36 = rotY*rotZ*t10*t12*0.5f; - float t39 = t17-t36; - float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f; - float t37 = t15-t23; - float t38 = t17-t36; - float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25); - float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39; - float t43 = t16-t25; - float t44 = t17-t36; - - // zero all the quaternion covariances - P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); - - - // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox - P(0,0) = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f; - P(0,1) = t22; - P(0,2) = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f; - P(0,3) = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f; - P(1,0) = t22; - P(1,1) = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26); - P(1,2) = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; - P(1,3) = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; - P(2,0) = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f; - P(2,1) = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; - P(2,2) = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38); - P(2,3) = t42; - P(3,0) = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f; - P(3,1) = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; - P(3,2) = t42; - P(3,3) = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44); - - } else { - // the equations are badly conditioned so use a small angle approximation - P.uncorrelateCovarianceSetVariance<1>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(1, 0.25f * rot_vec_var); - } -} - +#if defined(CONFIG_EKF2_BAROMETER) void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { +#if defined(CONFIG_EKF2_TERRAIN) if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = _terrain_vpos - _state.pos(2); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); - } else if (_control_status.flags.gnd_effect) { + } else +#endif // CONFIG_EKF2_TERRAIN + if (_control_status.flags.gnd_effect) { // Turn off ground effect compensation if it times out if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; @@ -1015,40 +889,22 @@ void Ekf::updateGroundEffect() _control_status.flags.gnd_effect = false; } } - -void Ekf::increaseQuatYawErrVariance(float yaw_variance) -{ - matrix::SquareMatrix q_cov; - sym::YawVarToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), yaw_variance, &q_cov); - q_cov.copyLowerToUpperTriangle(); - P.slice<4, 4>(0, 0) += q_cov; -} - -void Ekf::saveMagCovData() -{ - // save the NED axis covariance sub-matrix - _saved_mag_ef_covmat = P.slice<3, 3>(16, 16); - - // save the XYZ body covariance sub-matrix - _saved_mag_bf_covmat = P.slice<3, 3>(19, 19); -} - -void Ekf::loadMagCovData() -{ - // re-instate the NED axis covariance sub-matrix - P.uncorrelateCovarianceSetVariance<3>(16, 0.f); - P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat; - - // re-instate the XYZ body axis covariance sub-matrix - P.uncorrelateCovarianceSetVariance<3>(19, 0.f); - P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat; -} +#endif // CONFIG_EKF2_BAROMETER void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) { // save a copy of the quaternion state for later use in calculating the amount of reset change const Quatf quat_before_reset = _state.quat_nominal; + // save a copy of covariance in NED frame to restore it after the quat reset + const matrix::SquareMatrix3f rot_cov = diag(calcRotVecVariances()); + Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag(); + + // update the yaw angle variance + if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) { + rot_var_ned_before_reset(2) = yaw_variance; + } + // update transformation matrix from body to world frame using the current estimate // update the rotation matrix using the new yaw value _R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal)); @@ -1061,10 +917,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _state.quat_nominal = quat_after_reset; uncorrelateQuatFromOtherStates(); - // update the yaw angle variance - if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) { - increaseQuatYawErrVariance(yaw_variance); - } + // restore covariance + resetQuatCov(rot_var_ned_before_reset); // add the reset amount to the output observer buffered data _output_predictor.resetQuaternion(_time_delayed_us, quat_after_reset); @@ -1106,58 +960,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) } } -bool Ekf::resetYawToEKFGSF() -{ - if (!isYawEmergencyEstimateAvailable()) { - return false; - } - - // don't allow reset if there's just been a yaw reset - const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); - const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); - - if (yaw_alignment_changed || quat_reset) { - return false; - } - - ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", - (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); - - resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); - - _control_status.flags.yaw_align = true; - _information_events.flags.yaw_aligned_to_imu_gps = true; - - return true; -} - -bool Ekf::isYawEmergencyEstimateAvailable() const -{ - // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity - // data and the yaw estimate has converged - if (!_yawEstimator.isActive()) { - return false; - } - - return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); -} - -bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) -{ - return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); -} - -void Ekf::resetGpsDriftCheckFilters() -{ - _gps_velNE_filt.setZero(); - _gps_pos_deriv_filt.setZero(); - - _gps_horizontal_position_drift_rate_m_s = NAN; - _gps_vertical_position_drift_rate_m_s = NAN; - _gps_filtered_horizontal_velocity_m_s = NAN; -} - +#if defined(CONFIG_EKF2_WIND) void Ekf::resetWind() { #if defined(CONFIG_EKF2_AIRSPEED) @@ -1177,6 +980,103 @@ void Ekf::resetWindToZero() // If we don't have an airspeed measurement, then assume the wind is zero _state.wind_vel.setZero(); - // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); + resetWindCov(); +} + +void Ekf::resetWindCov() +{ + // start with a small initial uncertainty to improve the initial estimate + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); +} +#endif // CONFIG_EKF2_WIND + +void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) +{ + // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected + // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector + { + const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f); + const float beta = 1.f - alpha; + _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); + } + + { + const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f); + const float beta = 1.f - alpha; + + _accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt); + _accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt; + } + + + const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim) + || (_accel_magnitude_filt > _params.acc_bias_learn_acc_lim); + + // gyro bias inhibit + const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); + + for (unsigned index = 0; index < State::gyro_bias.dof; index++) { + const unsigned stateIndex = State::gyro_bias.idx + index; + + bool is_bias_observable = true; + + // TODO: gyro bias conditions + + const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable; + + if (do_inhibit_axis) { + // store the bias state variances to be reinstated later + if (!_gyro_bias_inhibit[index]) { + _prev_gyro_bias_var(index) = P(stateIndex, stateIndex); + _gyro_bias_inhibit[index] = true; + } + + } else { + if (_gyro_bias_inhibit[index]) { + // reinstate the bias state variances + P(stateIndex, stateIndex) = _prev_gyro_bias_var(index); + _gyro_bias_inhibit[index] = false; + } + } + } + + // accel bias inhibit + const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) + || is_manoeuvre_level_high + || _fault_status.flags.bad_acc_vertical; + + for (unsigned index = 0; index < State::accel_bias.dof; index++) { + const unsigned stateIndex = State::accel_bias.idx + index; + + bool is_bias_observable = true; + + if (_control_status.flags.vehicle_at_rest) { + is_bias_observable = true; + + } else if (_control_status.flags.fake_hgt) { + is_bias_observable = false; + + } else if (_control_status.flags.fake_pos) { + // when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector + is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966 + } + + const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable; + + if (do_inhibit_axis) { + // store the bias state variances to be reinstated later + if (!_accel_bias_inhibit[index]) { + _prev_accel_bias_var(index) = P(stateIndex, stateIndex); + _accel_bias_inhibit[index] = true; + } + + } else { + if (_accel_bias_inhibit[index]) { + // reinstate the bias state variances + P(stateIndex, stateIndex) = _prev_accel_bias_var(index); + _accel_bias_inhibit[index] = false; + } + } + } + } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 8e780a4f33..850e409bdb 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -46,9 +46,15 @@ EstimatorInterface::~EstimatorInterface() { +#if defined(CONFIG_EKF2_GNSS) delete _gps_buffer; +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_MAGNETOMETER) delete _mag_buffer; +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_BAROMETER) delete _baro_buffer; +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) delete _range_buffer; #endif // CONFIG_EKF2_RANGE_FINDER @@ -102,6 +108,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) #endif // CONFIG_EKF2_DRAG_FUSION } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EstimatorInterface::setMagData(const magSample &mag_sample) { if (!_initialised) { @@ -137,7 +144,9 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_GNSS) void EstimatorInterface::setGpsData(const gpsMessage &gps) { if (!_initialised) { @@ -216,7 +225,9 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_BAROMETER) void EstimatorInterface::setBaroData(const baroSample &baro_sample) { if (!_initialised) { @@ -252,6 +263,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) @@ -494,6 +506,17 @@ void EstimatorInterface::setDragData(const imuSample &imu) } } + // don't use any accel samples that are clipping + if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) { + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + + return; + } + _drag_sample_count++; // note acceleration is accumulated as a delta velocity _drag_down_sampled.accelXY(0) += imu.delta_vel(0); @@ -540,10 +563,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) // using baro if (_params.baro_ctrl > 0) { max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) // using airspeed @@ -552,10 +577,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_MAGNETOMETER) // mag mode if (_params.mag_fusion_type != MagFuseType::NONE) { max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) // using range finder @@ -564,9 +591,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) if (_params.gnss_ctrl > 0) { max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_params.flow_ctrl > 0) { @@ -692,17 +721,23 @@ void EstimatorInterface::print_status() printf("minimum observation interval %d us\n", _min_obs_interval_us); +#if defined(CONFIG_EKF2_GNSS) if (_gps_buffer) { printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); } +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_mag_buffer) { printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_BAROMETER) if (_baro_buffer) { printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); } +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) if (_range_buffer) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f9c62967c6..5143a10c2d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -81,16 +81,27 @@ using namespace estimator; class EstimatorInterface { public: - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - virtual bool collect_gps(const gpsMessage &gps) = 0; - void setIMUData(const imuSample &imu_sample); - void setMagData(const magSample &mag_sample); - +#if defined(CONFIG_EKF2_GNSS) + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined + virtual bool collect_gps(const gpsMessage &gps) = 0; void setGpsData(const gpsMessage &gps); + const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } + + float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } + float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } + float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_MAGNETOMETER) + void setMagData(const magSample &mag_sample); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_BAROMETER) void setBaroData(const baroSample &baro_sample); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) void setAirspeedData(const airspeedSample &airspeed_sample); @@ -224,12 +235,14 @@ public: int getNumberOfActiveVerticalVelocityAidingSources() const; const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } + float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } Vector3f getVelocity() const { return _output_predictor.getVelocity(); } const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } Vector3f getPosition() const { return _output_predictor.getPosition(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } +#if defined(CONFIG_EKF2_MAGNETOMETER) // Get the value of magnetic declination in degrees to be saved for use at the next startup // Returns true when the declination can be saved // At the next startup, set param.mag_declination_deg to the value saved @@ -262,6 +275,7 @@ public: strength_gs = _mag_strength; strength_ref_gs = _mag_strength_gps; } +#endif // CONFIG_EKF2_MAGNETOMETER // get EKF mode status const filter_control_status_u &control_status() const { return _control_status; } @@ -292,17 +306,12 @@ public: const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } - const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } - const bool &global_origin_valid() const { return _NED_origin_initialised; } const MapProjection &global_origin() const { return _pos_ref; } + float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } void print_status(); - float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } - float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } - float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } - OutputPredictor &output_predictor() { return _output_predictor; }; protected: @@ -338,10 +347,6 @@ protected: OutputPredictor _output_predictor{}; - // measurement samples capturing measurements on the delayed time horizon - gpsSample _gps_sample_delayed{}; - - #if defined(CONFIG_EKF2_AIRSPEED) airspeedSample _airspeed_sample_delayed{}; #endif // CONFIG_EKF2_AIRSPEED @@ -374,23 +379,37 @@ protected: bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized + // Variables used to publish the WGS-84 location of the EKF local NED origin bool _NED_origin_initialised{false}; - float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin - float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin + float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin + float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin + +#if defined(CONFIG_EKF2_GNSS) + RingBuffer *_gps_buffer{nullptr}; + uint64_t _time_last_gps_buffer_push{0}; + + gpsSample _gps_sample_delayed{}; + + float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) + float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) + float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) + MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) -#if defined(CONFIG_EKF2_GNSS_YAW) + +# if defined(CONFIG_EKF2_GNSS_YAW) float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). // innovation consistency check monitoring ratios AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state uint64_t _time_last_gps_yaw_buffer_push{0}; -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_DRAG_FUSION) RingBuffer *_drag_buffer{nullptr}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) - Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio #endif // CONFIG_EKF2_DRAG_FUSION innovation_fault_status_u _innov_check_fail_status{}; @@ -399,10 +418,6 @@ protected: bool _vertical_position_deadreckon_time_exceeded{true}; bool _vertical_velocity_deadreckon_time_exceeded{true}; - float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) - float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) - float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) - uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec) uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) @@ -410,9 +425,10 @@ protected: static constexpr uint8_t kBufferLengthDefault = 12; RingBuffer _imu_buffer{kBufferLengthDefault}; - RingBuffer *_gps_buffer{nullptr}; +#if defined(CONFIG_EKF2_MAGNETOMETER) RingBuffer *_mag_buffer{nullptr}; - RingBuffer *_baro_buffer{nullptr}; + uint64_t _time_last_mag_buffer_push{0}; +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer{nullptr}; @@ -427,9 +443,10 @@ protected: #endif // CONFIG_EKF2_AUXVEL RingBuffer *_system_flag_buffer{nullptr}; - uint64_t _time_last_gps_buffer_push{0}; - uint64_t _time_last_mag_buffer_push{0}; +#if defined(CONFIG_EKF2_BAROMETER) + RingBuffer *_baro_buffer{nullptr}; uint64_t _time_last_baro_buffer_push{0}; +#endif // CONFIG_EKF2_BAROMETER uint64_t _time_last_gnd_effect_on{0}; @@ -440,12 +457,15 @@ protected: uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked uint64_t _wmm_gps_time_last_set{0}; // time WMM last set + +#if defined(CONFIG_EKF2_MAGNETOMETER) float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) float _mag_inclination{NAN}; float _mag_strength{NAN}; +#endif // CONFIG_EKF2_MAGNETOMETER // this is the current status of the filter control modes filter_control_status_u _control_status{}; diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 9dffdc8e38..0a4bc00c84 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -76,10 +76,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com const float measurement = pos(2) - pos_offset_earth(2); float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f)); +#if defined(CONFIG_EKF2_GNSS) // increase minimum variance if GPS active if (_control_status.flags.gps_hgt) { measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise)); } +#endif // CONFIG_EKF2_GNSS const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); @@ -94,7 +96,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com if (measurement_valid && quality_sufficient) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); - bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) @@ -104,10 +106,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com && continuing_conditions_passing; if (_control_status.flags.ev_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { - if (ev_reset) { if (quality_sufficient) { diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index fc4e2db633..3f6027400a 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -125,12 +125,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common break; } +#if defined(CONFIG_EKF2_GNSS) // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); } } +#endif // CONFIG_EKF2_GNSS const Vector2f measurement{pos(0), pos(1)}; @@ -164,7 +166,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common if (measurement_valid && quality_sufficient) { _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO - _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8))); + _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance())); } if (!measurement_valid) { @@ -175,7 +177,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common && continuing_conditions_passing; if (_control_status.flags.ev_pos) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive()); diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 0160ca9fc9..aff345ac11 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -106,12 +106,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common break; } +#if defined(CONFIG_EKF2_GNSS) // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); } } +#endif // CONFIG_EKF2_GNSS const Vector3f measurement{vel}; @@ -138,7 +140,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 4d0472802f..3afb3b78fc 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -76,8 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common && isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6); if (_control_status.flags.ev_yaw) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { if (ev_reset) { @@ -157,11 +155,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common _control_status.flags.ev_yaw = true; } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { - // turn on fusion of external vision yaw measurements and disable all other heading fusion - stopMagFusion(); - stopGpsYawFusion(); - stopGpsFusion(); - + // turn on fusion of external vision yaw measurements ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); // reset yaw to EV diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp index ef8c318081..8b7eba5664 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -63,9 +63,9 @@ void Ekf::controlFakeHgtFusion() if (continuing_conditions_passing) { // always protect against extreme values that could result in a NaN - aid_src.fusion_enabled = aid_src.test_ratio < sq(100.0f / innov_gate); - - fuseVerticalPosition(aid_src); + if (aid_src.test_ratio < sq(100.0f / innov_gate)) { + fuseVerticalPosition(aid_src); + } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index 4c876c4597..4962cd55fc 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion() Vector2f obs_var; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise)); + obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f)); } else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) { // Accelerate tilt fine alignment by fusing more @@ -76,10 +76,11 @@ void Ekf::controlFakePosFusion() if (continuing_conditions_passing) { // always protect against extreme values that could result in a NaN - aid_src.fusion_enabled = (aid_src.test_ratio[0] < sq(100.0f / innov_gate)) - && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)); - - fuseHorizontalPosition(aid_src); + if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate)) + && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) + ) { + fuseHorizontalPosition(aid_src); + } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 56b75a054b..100d4150f1 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) if (measurement_valid && gps_checks_passing && !gps_checks_failing) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding @@ -98,8 +98,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) && !gps_checks_failing; if (_control_status.flags.gps_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { fuseVerticalPosition(aid_src); diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index f42ad5599b..7396b31f3c 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -41,7 +41,10 @@ #include "ekf.h" -#include +#if defined(CONFIG_EKF2_MAGNETOMETER) +# include +#endif // CONFIG_EKF2_MAGNETOMETER + #include // GPS pre-flight check bit locations @@ -85,14 +88,14 @@ bool Ekf::collect_gps(const gpsMessage &gps) _NED_origin_initialised = true; // save the horizontal and vertical position uncertainty of the origin - _gps_origin_eph = gps.eph; - _gps_origin_epv = gps.epv; + _gpos_origin_eph = gps.eph; + _gpos_origin_epv = gps.epv; _information_events.flags.gps_checks_passed = true; ECL_INFO("GPS checks passed"); } - if (isTimedOut(_wmm_gps_time_last_checked, 1e6)) { + if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) { // a rough 2D fix is sufficient to lookup declination const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.eph < 1000); @@ -100,6 +103,8 @@ bool Ekf::collect_gps(const gpsMessage &gps) // If we have good GPS data set the origin's WGS-84 position to the last gps fix const double lat = gps.lat * 1.0e-7; + +#if defined(CONFIG_EKF2_MAGNETOMETER) const double lon = gps.lon * 1.0e-7; // set the magnetic field data returned by the geo library using the current GPS position @@ -113,9 +118,9 @@ bool Ekf::collect_gps(const gpsMessage &gps) const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); if ((_wmm_gps_time_last_set == 0) - || !PX4_ISFINITE(mag_declination_gps) - || !PX4_ISFINITE(mag_inclination_gps) - || !PX4_ISFINITE(mag_strength_gps) + || !PX4_ISFINITE(_mag_declination_gps) + || !PX4_ISFINITE(_mag_inclination_gps) + || !PX4_ISFINITE(_mag_strength_gps) || mag_declination_changed || mag_inclination_changed ) { @@ -126,6 +131,7 @@ bool Ekf::collect_gps(const gpsMessage &gps) _wmm_gps_time_last_set = _time_delayed_us; } } +#endif // CONFIG_EKF2_MAGNETOMETER _earth_rate_NED = calcEarthRateNED((float)math::radians(lat)); } @@ -265,3 +271,13 @@ bool Ekf::gps_is_good(const gpsMessage &gps) // continuous period without fail of x seconds required to return a healthy status return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us); } + +void Ekf::resetGpsDriftCheckFilters() +{ + _gps_velNE_filt.setZero(); + _gps_pos_deriv_filt.setZero(); + + _gps_horizontal_position_drift_rate_m_s = NAN; + _gps_vertical_position_drift_rate_m_s = NAN; + _gps_filtered_horizontal_velocity_m_s = NAN; +} diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index c5db7851da..a5671b111e 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -95,7 +95,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) vel_obs_var, // observation variance math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate _aid_src_gnss_vel); - _aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); + const bool gnss_vel_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); // GNSS position const Vector2f position{gps_sample.pos}; @@ -117,13 +117,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) pos_obs_var, // observation variance math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate _aid_src_gnss_pos); - _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); + const bool gnss_pos_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently bool mandatory_conditions_passing = false; - if (((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL)) + if ((gnss_pos_enabled || gnss_vel_enabled) && _control_status.flags.tilt_align && _NED_origin_initialised ) { @@ -148,8 +148,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (continuing_conditions_passing || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - fuseVelocity(_aid_src_gnss_vel); - fuseHorizontalPosition(_aid_src_gnss_pos); + if (gnss_vel_enabled) { + fuseVelocity(_aid_src_gnss_vel); + } + + if (gnss_pos_enabled) { + fuseHorizontalPosition(_aid_src_gnss_pos); + } bool do_vel_pos_reset = shouldResetGpsFusion(); @@ -196,15 +201,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (do_vel_pos_reset) { ECL_WARN("GPS fusion timeout, resetting velocity and position"); - // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + if (gnss_vel_enabled) { + // reset velocity + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + } - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + if (gnss_pos_enabled) { + // reset position + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + } } } else { @@ -228,15 +237,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) || !_control_status_prev.flags.yaw_align ) { // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + if (gnss_vel_enabled) { + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + } } - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + if (gnss_pos_enabled) { + // reset position + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + } _control_status.flags.gps = true; } @@ -289,18 +302,6 @@ bool Ekf::shouldResetGpsFusion() const return (is_reset_required || is_inflight_nav_failure); } -bool Ekf::isYawFailure() const -{ - if (!isYawEmergencyEstimateAvailable()) { - return false; - } - - const float euler_yaw = getEulerYaw(_R_to_earth); - const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); - - return fabsf(yaw_error) > math::radians(25.f); -} - #if defined(CONFIG_EKF2_GNSS_YAW) void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing) { @@ -387,12 +388,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi stopGpsYawFusion(); } } -#endif // CONFIG_EKF2_GNSS_YAW void Ekf::stopGpsYawFusion() { -#if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { _control_status.flags.gps_yaw = false; @@ -408,9 +406,8 @@ void Ekf::stopGpsYawFusion() ECL_INFO("stopping GPS yaw fusion"); } } - -#endif // CONFIG_EKF2_GNSS_YAW } +#endif // CONFIG_EKF2_GNSS_YAW void Ekf::stopGpsFusion() { @@ -423,5 +420,61 @@ void Ekf::stopGpsFusion() } stopGpsHgtFusion(); +#if defined(CONFIG_EKF2_GNSS_YAW) stopGpsYawFusion(); +#endif // CONFIG_EKF2_GNSS_YAW +} + +bool Ekf::isYawEmergencyEstimateAvailable() const +{ + // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity + // data and the yaw estimate has converged + if (!_yawEstimator.isActive()) { + return false; + } + + return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); +} + +bool Ekf::isYawFailure() const +{ + if (!isYawEmergencyEstimateAvailable()) { + return false; + } + + const float euler_yaw = getEulerYaw(_R_to_earth); + const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); + + return fabsf(yaw_error) > math::radians(25.f); +} + +bool Ekf::resetYawToEKFGSF() +{ + if (!isYawEmergencyEstimateAvailable()) { + return false; + } + + // don't allow reset if there's just been a yaw reset + const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); + + if (yaw_alignment_changed || quat_reset) { + return false; + } + + ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", + (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); + + resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); + + _control_status.flags.yaw_align = true; + _information_events.flags.yaw_aligned_to_imu_gps = true; + + return true; +} + +bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) +{ + return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 9deffe1e5f..d02c07f994 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2018-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -41,7 +41,7 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h" +#include #include #include @@ -64,8 +64,8 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) float heading_innov_var; { - Vector24f H; - sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); + VectorState H; + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } gnss_yaw.observation = measured_hdg; @@ -73,8 +73,6 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg); gnss_yaw.innovation_variance = heading_innov_var; - gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; - gnss_yaw.timestamp_sample = gps_sample.time_us; const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); @@ -91,7 +89,7 @@ void Ekf::fuseGpsYaw() return; } - Vector24f H; + VectorState H; { float heading_pred; @@ -99,11 +97,9 @@ void Ekf::fuseGpsYaw() // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H // making a separate function just for H uses more flash space without reducing CPU load significantly - sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } - const SparseVector24f<0,1,2,3> Hfusion(H); - // check if the innovation variance calculation is badly conditioned if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned @@ -131,7 +127,7 @@ void Ekf::fuseGpsYaw() // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance; + VectorState Kfusion = P * H / gnss_yaw.innovation_variance; const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 8a09b72f79..13ae1112c6 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -40,7 +40,7 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h" +#include #include @@ -60,9 +60,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) // calculate kalman gains and innovation variances Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2) Vector3f innovation_variance; - Vector24f Kx, Ky, Kz; // Kalman gain vectors + VectorState Kx, Ky, Kz; // Kalman gain vectors sym::ComputeGravityInnovVarAndKAndH( - getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON, + _state.vector(), P, measurement, measurement_var, FLT_EPSILON, &innovation, &innovation_variance, &Kx, &Ky, &Kz); // fill estimator aid source status @@ -80,9 +80,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) float innovation_gate = 1.f; setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); - _aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector; + const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; - if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) { + if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { // perform fusion for each axis _aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0)) && measurementUpdate(Ky, innovation_variance(1), innovation(1)) diff --git a/src/modules/ekf2/EKF/height_bias_estimator.hpp b/src/modules/ekf2/EKF/height_bias_estimator.hpp index 9082842b9e..1d1b9629f5 100644 --- a/src/modules/ekf2/EKF/height_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/height_bias_estimator.hpp @@ -35,7 +35,8 @@ * @file height_bias_estimator.hpp */ -#pragma once +#ifndef EKF_HEIGHT_BIAS_ESTIMATOR_HPP +#define EKF_HEIGHT_BIAS_ESTIMATOR_HPP #include "bias_estimator.hpp" @@ -72,3 +73,5 @@ private: bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool }; + +#endif // !EKF_HEIGHT_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index c9cfff5a3b..0d954663d0 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -39,12 +39,19 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) { + checkVerticalAccelerationBias(imu_delayed); checkVerticalAccelerationHealth(imu_delayed); +#if defined(CONFIG_EKF2_BAROMETER) updateGroundEffect(); controlBaroHeightFusion(); +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_GNSS) controlGnssHeightFusion(_gps_sample_delayed); +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_RANGE_FINDER) controlRangeHeightFusion(); #endif // CONFIG_EKF2_RANGE_FINDER @@ -113,6 +120,107 @@ void Ekf::checkHeightSensorRefFallback() } } +void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed) +{ + // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong + // calculate accel bias term aligned with the gravity vector + const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; + const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg; + const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); + + // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation + bool bad_acc_bias = false; + + if (fabsf(down_dvel_bias) > dVel_bias_lim) { + + bool bad_vz = false; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_hgt) { + if (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.f) { + bad_vz = true; + } + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps) { + if (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.f) { + bad_vz = true; + } + } + +#endif // CONFIG_EKF2_GNSS + + if (bad_vz) { +#if defined(CONFIG_EKF2_BAROMETER) + + if (_control_status.flags.baro_hgt) { + if (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_hgt) { + if (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps_hgt) { + if (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_control_status.flags.rng_hgt) { + if (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_RANGE_FINDER + } + } + + // record the pass/fail + if (!bad_acc_bias) { + _fault_status.flags.bad_acc_bias = false; + _time_acc_bias_check = _time_delayed_us; + + } else { + _fault_status.flags.bad_acc_bias = true; + } + + // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of + // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue + if (_fault_status.flags.bad_acc_bias && isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { + + resetAccelBiasCov(); + + _time_acc_bias_check = imu_delayed.time_us; + + _fault_status.flags.bad_acc_bias = false; + _warning_events.flags.invalid_accel_bias_cov_reset = true; + ECL_WARN("invalid accel bias - covariance reset"); + } +} + void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) { // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical @@ -175,10 +283,13 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const bool failed_lim{false}; } checks[6] {}; +#if defined(CONFIG_EKF2_BAROMETER) if (_control_status.flags.baro_hgt) { checks[0] = {ReferenceType::PRESSURE, _aid_src_baro_hgt.innovation, _aid_src_baro_hgt.innovation_variance}; } +#endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps_hgt) { checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance}; } @@ -186,6 +297,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.gps) { checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]}; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { diff --git a/src/modules/ekf2/EKF/imu_down_sampler.hpp b/src/modules/ekf2/EKF/imu_down_sampler.hpp index ec0d8a1008..29732bd584 100644 --- a/src/modules/ekf2/EKF/imu_down_sampler.hpp +++ b/src/modules/ekf2/EKF/imu_down_sampler.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2019-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 541e3454e3..738441cf27 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -63,6 +63,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star _control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO) && _control_status.flags.mag && _control_status.flags.mag_aligned_in_flight + && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps) && !_control_status.flags.mag_fault && isRecent(aid_src.time_last_fuse, 500'000) && getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f)) @@ -88,7 +89,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star if (_control_status.flags.mag) { aid_src.timestamp_sample = mag_sample.time_us; - aid_src.fusion_enabled = true; if (continuing_conditions_passing && _control_status.flags.yaw_align) { @@ -166,8 +166,8 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star || wmm_updated || !_mag_decl_cov_reset || !_state.mag_I.longerThan(0.f) - || (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I - || (P.slice<3, 3>(19, 19).diag().min() < sq(0.0001f)) // mag_B + || (getStateVariance().min() < sq(0.0001f)) + || (getStateVariance().min() < sq(0.0001f)) ) { ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME); @@ -189,8 +189,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star _nb_mag_3d_reset_available = 2; } } - - aid_src.fusion_enabled = _control_status.flags.mag; } void Ekf::stopMagFusion() @@ -216,3 +214,20 @@ void Ekf::stopMagFusion() } } +void Ekf::saveMagCovData() +{ + // save the NED axis covariance sub-matrix + _saved_mag_ef_covmat = getStateCovariance(); + + // save the XYZ body covariance sub-matrix + _saved_mag_bf_covmat = getStateCovariance(); +} + +void Ekf::loadMagCovData() +{ + // re-instate the NED axis covariance sub-matrix + resetStateCovariance(_saved_mag_ef_covmat); + + // re-instate the XYZ body axis covariance sub-matrix + resetStateCovariance(_saved_mag_bf_covmat); +} diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 38599c359e..cd714e91ec 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2019-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -50,6 +50,7 @@ void Ekf::controlMagFusion() // check mag state observability checkYawAngleObservability(); checkMagBiasObservability(); + checkMagHeadingConsistency(); if (_mag_bias_observable || _yaw_angle_observable) { _time_last_mov_3d_mag_suitable = _time_delayed_us; @@ -122,17 +123,17 @@ void Ekf::controlMagFusion() bool Ekf::checkHaglYawResetReq() const { +#if defined(CONFIG_EKF2_TERRAIN) // We need to reset the yaw angle after climbing away from the ground to enable // recovery from ground level magnetic interference. if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. -#if defined(CONFIG_EKF2_RANGE_FINDER) static constexpr float mag_anomalies_max_hagl = 1.5f; const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; -#endif // CONFIG_EKF2_RANGE_FINDER } +#endif // CONFIG_EKF2_TERRAIN return false; } @@ -154,7 +155,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) * Vector3f(_mag_strength_gps, 0, 0); // mag_B: reset +#if defined(CONFIG_EKF2_GNSS) if (isYawEmergencyEstimateAvailable()) { + const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth); const Dcmf R_to_body = R_to_earth.transpose(); @@ -164,6 +167,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) ECL_INFO("resetMagStates using yaw estimator"); } else if (!reset_heading && _control_status.flags.yaw_align) { +#else + if (!reset_heading && _control_status.flags.yaw_align) { +#endif // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); _state.mag_B = mag - (R_to_body * mag_earth_pred); @@ -257,6 +263,19 @@ void Ekf::checkMagBiasObservability() _time_yaw_started = _time_delayed_us; } +void Ekf::checkMagHeadingConsistency() +{ + if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { + if (_yaw_angle_observable) { + // yaw angle must be observable to consider consistency + _control_status.flags.mag_heading_consistent = true; + } + + } else { + _control_status.flags.mag_heading_consistent = false; + } +} + bool Ekf::checkMagField(const Vector3f &mag_sample) { _control_status.flags.mag_field_disturbed = false; @@ -357,6 +376,9 @@ void Ekf::resetMagHeading(const Vector3f &mag) _mag_heading_last_declination = declination; _time_last_heading_fuse = _time_delayed_us; + + _mag_heading_innov_lpf.reset(0.f); + _control_status.flags.mag_heading_consistent = true; } float Ekf::getMagDeclination() diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f04a2df8e8..03898b36d0 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -42,12 +42,12 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h" -#include "python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h" + +#include +#include +#include + +#include #include @@ -57,70 +57,13 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains - const char *numerical_error_covariance_reset_string = "numerical error - covariance reset"; Vector3f mag_innov; Vector3f innov_var; // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; - Vector24f H; - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + VectorState H; + const VectorState state_vector = _state.vector(); sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); - Hfusion = H; - - innov_var.copyTo(aid_src_mag.innovation_variance); - - if (aid_src_mag.innovation_variance[0] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_x = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(); - } - - resetMagCov(); - - ECL_ERR("magX %s", numerical_error_covariance_reset_string); - return false; - } - - _fault_status.flags.bad_mag_x = false; - - // check innovation variances for being badly conditioned - if (aid_src_mag.innovation_variance[1] < R_MAG) { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_y = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(); - } - - resetMagCov(); - - ECL_ERR("magY %s", numerical_error_covariance_reset_string); - return false; - } - - _fault_status.flags.bad_mag_y = false; - - if (aid_src_mag.innovation_variance[2] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_z = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(); - } - - resetMagCov(); - - ECL_ERR("magZ %s", numerical_error_covariance_reset_string); - return false; - } - - _fault_status.flags.bad_mag_z = false; // do not use the synthesized measurement for the magnetomter Z component for 3D fusion if (_control_status.flags.synthetic_mag_z) { @@ -131,23 +74,44 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo aid_src_mag.observation[i] = mag(i) - _state.mag_B(i); aid_src_mag.observation_variance[i] = R_MAG; aid_src_mag.innovation[i] = mag_innov(i); - } - - aid_src_mag.fusion_enabled = _control_status.flags.mag; - - // do not use the synthesized measurement for the magnetomter Z component for 3D fusion - if (_control_status.flags.synthetic_mag_z) { - aid_src_mag.innovation[2] = 0.0f; + aid_src_mag.innovation_variance[i] = innov_var(i); } const float innov_gate = math::max(_params.mag_innov_gate, 1.f); setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate); + if (update_all_states) { + _fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]); + _fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]); + _fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]); + + } else { + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; + } + // Perform an innovation consistency check and report the result _innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f); _innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f); _innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f); + const char *numerical_error_covariance_reset_string = "numerical error - covariance reset"; + + // check innovation variances for being badly conditioned + if (innov_var.min() < R_MAG) { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + // we need to re-initialise covariances and abort this fusion step + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + + ECL_ERR("mag %s", numerical_error_covariance_reset_string); + return false; + } + // if any axis fails, abort the mag fusion if (aid_src_mag.innovation_rejected) { return false; @@ -164,7 +128,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } else if (index == 1) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); - Hfusion = H; // recalculate innovation using the updated state aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); @@ -175,7 +138,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); @@ -193,7 +156,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); - Hfusion = H; // recalculate innovation using the updated state aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); @@ -204,7 +166,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); @@ -214,16 +176,19 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } } - Vector24f Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index]; + VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index]; if (!update_all_states) { - for (unsigned row = 0; row <= 15; row++) { - Kfusion(row) = 0.f; - } + // zero non-mag Kalman gains if not updating all states - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = 0.f; - } + // copy mag_I and mag_B Kalman gains + const Vector3f K_mag_I = Kfusion.slice(State::mag_I.idx, 0); + const Vector3f K_mag_B = Kfusion.slice(State::mag_B.idx, 0); + + // zero all Kalman gains, then restore mag + Kfusion.setZero(); + Kfusion.slice(State::mag_I.idx, 0) = K_mag_I; + Kfusion.slice(State::mag_B.idx, 0) = K_mag_B; } if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) { @@ -235,11 +200,13 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } } - _fault_status.flags.bad_mag_x = !fused[0]; - _fault_status.flags.bad_mag_y = !fused[1]; - _fault_status.flags.bad_mag_z = !fused[2]; + if (update_all_states) { + _fault_status.flags.bad_mag_x = !fused[0]; + _fault_status.flags.bad_mag_y = !fused[1]; + _fault_status.flags.bad_mag_z = !fused[2]; + } - if (fused[0] && fused[1] && (fused[2] || _control_status.flags.synthetic_mag_z)) { + if (fused[0] && fused[1] && fused[2]) { aid_src_mag.fused = true; aid_src_mag.time_last_fuse = _time_delayed_us; @@ -254,114 +221,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo return false; } -// update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) -{ - Vector24f H_YAW; - computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); - - return fuseYaw(aid_src_status, H_YAW); -} - -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW) -{ - // define the innovation gate size - float gate_sigma = math::max(_params.heading_innov_gate, 1.f); - - // innovation test ratio - setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); - - if (aid_src_status.fusion_enabled) { - - // check if the innovation variance calculation is badly conditioned - if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.flags.bad_hdg = false; - - } else { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_hdg = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - ECL_ERR("yaw fusion numerical error - covariance reset"); - - return false; - } - - // calculate the Kalman gains - // only calculate gains for states we are using - Vector24f Kfusion; - const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; - - for (uint8_t row = 0; row < _k_num_states; row++) { - for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * H_YAW(col); - } - - Kfusion(row) *= heading_innov_var_inv; - } - - // set the magnetometer unhealthy if the test fails - if (aid_src_status.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; - - // if we are in air we don't want to fuse the measurement - // we allow to use it when on the ground because the large innovation could be caused - // by interference or a large initial gyro bias - if (!_control_status.flags.in_air - && isTimedOut(_time_last_in_air, (uint64_t)5e6) - && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) - ) { - // constrain the innovation to the maximum set by the gate - // we need to delay this forced fusion to avoid starting it - // immediately after touchdown, when the drone is still armed - float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); - aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); - - // also reset the yaw gyro variance to converge faster and avoid - // being stuck on a previous bad estimate - resetGyroBiasZCov(); - - } else { - return false; - } - - } else { - _innov_check_fail_status.flags.reject_yaw = false; - } - - if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { - - _time_last_heading_fuse = _time_delayed_us; - - aid_src_status.time_last_fuse = _time_delayed_us; - aid_src_status.fused = true; - - _fault_status.flags.bad_hdg = false; - - return true; - - } else { - _fault_status.flags.bad_hdg = true; - } - } - - // otherwise - aid_src_status.fused = false; - return false; -} - -void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const -{ - if (shouldUse321RotationSequence(_R_to_earth)) { - sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - - } else { - sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - } -} - bool Ekf::fuseDeclination(float decl_sigma) { if (!_control_status.flags.mag) { @@ -371,12 +230,12 @@ bool Ekf::fuseDeclination(float decl_sigma) // observation variance (rad**2) const float R_DECL = sq(decl_sigma); - Vector24f H; + VectorState H; float decl_pred; float innovation_variance; // TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter? - sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); const float innovation = wrap_pi(decl_pred - getMagDeclination()); @@ -385,10 +244,8 @@ bool Ekf::fuseDeclination(float decl_sigma) return false; } - SparseVector24f<16,17> Hfusion(H); - // Calculate the Kalman gains - Vector24f Kfusion = P * Hfusion / innovation_variance; + VectorState Kfusion = P * H / innovation_variance; const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation); diff --git a/src/modules/ekf2/EKF/mag_heading_control.cpp b/src/modules/ekf2/EKF/mag_heading_control.cpp index 9c00d01312..612474937e 100644 --- a/src/modules/ekf2/EKF/mag_heading_control.cpp +++ b/src/modules/ekf2/EKF/mag_heading_control.cpp @@ -68,18 +68,23 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common if (_control_status.flags.yaw_align) { // mag heading aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation); + _mag_heading_innov_lpf.update(aid_src.innovation); } else { // mag heading delta (logging only) aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev) - wrap_pi(measured_hdg - _mag_heading_prev)); + _mag_heading_innov_lpf.reset(0.f); } // determine if we should use mag heading aiding + const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps; + bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING) || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) && _control_status.flags.tilt_align - && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) + && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss) + || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed && !_control_status.flags.ev_yaw @@ -92,13 +97,13 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common && isTimedOut(aid_src.time_last_fuse, 3e6); if (_control_status.flags.mag_hdg) { - aid_src.fusion_enabled = true; aid_src.timestamp_sample = mag_sample.time_us; if (continuing_conditions_passing && _control_status.flags.yaw_align) { const bool declination_changed = _control_status_prev.flags.mag_hdg && (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f)); + bool skip_timeout_check = false; if (mag_sample.reset || declination_changed) { if (declination_changed) { @@ -113,10 +118,21 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common aid_src.time_last_fuse = _time_delayed_us; } else { - fuseYaw(aid_src); + VectorState H_YAW; + computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW); + + if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) { + // Only fuse mag to constrain the yaw variance to a safe value + fuseYaw(aid_src, H_YAW); + + } else { + // Yaw variance is low enough, stay in mag heading mode but don't fuse the data and skip the fusion timeout check + skip_timeout_check = true; + aid_src.test_ratio = 0.f; // required for preflight checks to pass + } } - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max) && !skip_timeout_check; if (is_fusion_failing) { if (_nb_mag_heading_reset_available > 0) { @@ -168,8 +184,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common } } - aid_src.fusion_enabled = _control_status.flags.mag_hdg; - // record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only) _mag_heading_prev = measured_hdg; _mag_heading_pred_prev = getEulerYaw(_state.quat_nominal); diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 243edabfe7..bcef15721e 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -45,8 +45,8 @@ #include #include -#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h" -#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h" +#include +#include void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) { @@ -77,11 +77,9 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) aid_src.observation_variance[0] = R_LOS; aid_src.observation_variance[1] = R_LOS; - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); - Vector2f innov_var; - Vector24f H; - sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); + VectorState H; + sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(aid_src.innovation_variance); // run the innovation consistency check and record result @@ -90,18 +88,16 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) void Ekf::fuseOptFlow() { - _aid_src_optical_flow.fusion_enabled = true; - const float R_LOS = _aid_src_optical_flow.observation_variance[0]; // calculate the height above the ground of the optical flow camera. Since earth frame is NED // a positive offset in earth frame leads to a smaller height above the ground. float range = predictFlowRange(); - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + const VectorState state_vector = _state.vector(); Vector2f innov_var; - Vector24f H; + VectorState H; sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(_aid_src_optical_flow.innovation_variance); @@ -148,8 +144,7 @@ void Ekf::fuseOptFlow() } } - SparseVector24f<0,1,2,3,4,5,6> Hfusion(H); - Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index]; + VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index]; if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) { fused[index] = true; @@ -188,7 +183,7 @@ Vector2f Ekf::predictFlowVelBody() // calculate the velocity of the sensor relative to the imu in body frame // Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body; + const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt - _flow_gyro_bias)) % pos_offset_body; // calculate the velocity of the sensor in the earth frame const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; @@ -203,40 +198,29 @@ Vector2f Ekf::predictFlowVelBody() bool Ekf::calcOptFlowBodyRateComp() { bool is_body_rate_comp_available = false; - const bool use_flow_sensor_gyro = _flow_sample_delayed.gyro_xyz.isAllFinite(); - if (use_flow_sensor_gyro) { + if ((_delta_time_of > FLT_EPSILON) + && (_flow_sample_delayed.dt > FLT_EPSILON)) { + const Vector3f reference_body_rate = -_imu_del_ang_of / _delta_time_of; // flow gyro has opposite sign convention + _ref_body_rate = reference_body_rate; - // if accumulation time differences are not excessive and accumulation time is adequate - // compare the optical flow and and navigation rate data and calculate a bias error - if ((_delta_time_of > FLT_EPSILON) - && (_flow_sample_delayed.dt > FLT_EPSILON) - && (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) { + if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1))) { + _flow_sample_delayed.gyro_xyz = reference_body_rate * _flow_sample_delayed.dt; - const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of)); + } else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) { + // Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro + _flow_sample_delayed.gyro_xyz(2) = reference_body_rate(2) * _flow_sample_delayed.dt; + } - const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt)); + const Vector3f measured_body_rate = _flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt; + _measured_body_rate = measured_body_rate; + if (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) { // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f; - - // apply gyro bias - _flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt); - - is_body_rate_comp_available = true; } - } else { - // Use the EKF gyro data if optical flow sensor gyro data is not available - // for clarification of the sign see definition of flowSample and imuSample in common.h - if ((_delta_time_of > FLT_EPSILON) - && (_flow_sample_delayed.dt > FLT_EPSILON)) { - - _flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt; - _flow_gyro_bias.zero(); - - is_body_rate_comp_available = true; - } + is_body_rate_comp_available = true; } // reset the accumulators diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 8b99d2e5c7..d4b2dc1466 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -142,15 +142,25 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon if (_flow_data_ready) { - // Inhibit flow use if motion is un-suitable or we have good quality GPS - // Apply hysteresis to prevent rapid mode switching - const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f; - // Check if we are in-air and require optical flow to control position drift - const bool is_flow_required = _control_status.flags.in_air + bool is_flow_required = _control_status.flags.in_air && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow) - || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad + || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)); + +#if defined(CONFIG_EKF2_GNSS) + // check if using GPS, but GPS is bad + if (_control_status.flags.gps) { + if (_control_status.flags.in_air && !is_flow_required) { + // Inhibit flow use if motion is un-suitable or we have good quality GPS + // Apply hysteresis to prevent rapid mode switching + const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f; + + if (_gps_error_norm > gps_err_norm_lim) { + is_flow_required = true; + } + } + } +#endif // CONFIG_EKF2_GNSS // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation const bool preflight_motion_not_ok = !_control_status.flags.in_air @@ -209,7 +219,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data - if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { + if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6)) { fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index 839b00e64d..2be978c8f4 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -102,16 +102,6 @@ void OutputPredictor::resetQuaternion(const uint64_t time_delayed_us, const Quat void OutputPredictor::resetHorizontalVelocityTo(const uint64_t time_delayed_us, const Vector2f &new_horz_vel) { - // TODO: review time_us - - if (_output_buffer.get_oldest().time_us != time_delayed_us) { - - - - - } - - const outputSample &output_delayed = _output_buffer.get_oldest(); // horizontal velocity @@ -299,6 +289,11 @@ bool OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; } + // update auxiliary yaw estimate + const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled; + const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); + _unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D); + return true; } diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor.h index 477e6716f2..8501b1cf0e 100644 --- a/src/modules/ekf2/EKF/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor.h @@ -92,6 +92,9 @@ public: const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } + // get a yaw value solely based on bias-removed gyro integration + float getUnaidedYaw() const { return _unaided_yaw; } + // get the velocity of the body frame origin in local NED earth frame matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } @@ -167,6 +170,8 @@ private: matrix::Vector3f _imu_pos_body{}; ///< xyz position of IMU in body frame (m) + float _unaided_yaw{}; + // output complementary filter tuning float _vel_tau{0.25f}; ///< velocity state correction time constant (1/sec) float _pos_tau{0.25f}; ///< position state correction time constant (1/sec) diff --git a/src/modules/ekf2/EKF/position_bias_estimator.hpp b/src/modules/ekf2/EKF/position_bias_estimator.hpp index ddcbe144ce..0e997e2d28 100644 --- a/src/modules/ekf2/EKF/position_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/position_bias_estimator.hpp @@ -35,7 +35,8 @@ * @file position_bias_estimator.hpp */ -#pragma once +#ifndef EKF_POSITION_BIAS_ESTIMATOR_HPP +#define EKF_POSITION_BIAS_ESTIMATOR_HPP #include "bias_estimator.hpp" @@ -111,3 +112,5 @@ private: bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool }; + +#endif // !EKF_POSITION_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 83b20495db..4ab40828b8 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -1,7 +1,7 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ - Copyright (c) 2022 PX4 Development Team + Copyright (c) 2022-2023 PX4 Development Team Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -33,85 +33,114 @@ File: derivation.py Description: """ +import argparse + import symforce symforce.set_epsilon_to_symbol() import symforce.symbolic as sf +from symforce import typing as T +from symforce import ops +from symforce.values import Values from derivation_utils import * -class State: - qw = 0 - qx = 1 - qy = 2 - qz = 3 - vx = 4 - vy = 5 - vz = 6 - px = 7 - py = 8 - pz = 9 - gyro_bx = 10 - gyro_by = 11 - gyro_bz = 12 - accel_bx = 13 - accel_by = 14 - accel_bz = 15 - ix = 16 - iy = 17 - iz = 18 - ibx = 19 - iby = 20 - ibz = 21 - wx = 22 - wy = 23 - n_states = 24 +# Initialize parser +parser = argparse.ArgumentParser() + +parser.add_argument("--disable_mag", action='store_true', help="disable mag") +parser.add_argument("--disable_wind", action='store_true', help="disable wind") + +# Read arguments from command line +args = parser.parse_args() + +# The state vector is organized in an ordered dictionary +State = Values( + quat_nominal = sf.V4(), + vel = sf.V3(), + pos = sf.V3(), + gyro_bias = sf.V3(), + accel_bias = sf.V3(), + mag_I = sf.V3(), + mag_B = sf.V3(), + wind_vel = sf.V2() +) + +if args.disable_mag: + del State["mag_I"] + del State["mag_B"] + +if args.disable_wind: + del State["wind_vel"] + +class IdxDof(): + def __init__(self, idx, dof): + self.idx = idx + self.dof = dof + +def BuildTangentStateIndex(): + # Build a dictionary that can be used to access elements of vectors + # and matrices defined in the state tangent space (e.g.: P, K and H) + tangent_state_index = {} + idx = 0 + for key in State.keys_recursive(): + dof = State[key].tangent_dim() + tangent_state_index[key] = IdxDof(idx, dof) + idx += dof + return tangent_state_index + +tangent_idx = BuildTangentStateIndex() class VState(sf.Matrix): - SHAPE = (State.n_states, 1) + SHAPE = (State.storage_dim(), 1) -class MState(sf.Matrix): - SHAPE = (State.n_states, State.n_states) +class VTangent(sf.Matrix): + SHAPE = (State.tangent_dim(), 1) -def state_to_quat(state: VState) -> sf.Quaternion: - return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw]) +class MTangent(sf.Matrix): + SHAPE = (State.tangent_dim(), State.tangent_dim()) -def state_to_rot3(state: VState) -> sf.Rot3: - return sf.Rot3(state_to_quat(state)) +def state_to_rot3(state: Values): + q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0]) + return sf.Rot3(q) def predict_covariance( - state: VState, - P: MState, - d_vel: sf.V3, - d_vel_var: sf.V3, - d_ang: sf.V3, - d_ang_var: sf.Scalar, - dt: sf.Scalar -): + state: VState, + P: MTangent, + d_vel: sf.V3, + d_vel_dt: sf.Scalar, + d_vel_var: sf.V3, + d_ang: sf.V3, + d_ang_dt: sf.Scalar, + d_ang_var: sf.Scalar +) -> MTangent: + + state = State.from_storage(state) g = sf.Symbol("g") # does not appear in the jacobians - accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz]) - d_vel_b = accel_b * dt + d_vel_b = state["accel_bias"] * d_vel_dt d_vel_true = d_vel - d_vel_b - gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz]) - d_ang_b = gyro_b * dt + d_ang_b = state["gyro_bias"] * d_ang_dt d_ang_true = d_ang - d_ang_b - q = state_to_quat(state) - R_to_earth = sf.Rot3(q) - v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) - p = sf.V3(state[State.px], state[State.py], state[State.pz]) + q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0]) + R_to_earth = state_to_rot3(state) + v = state["vel"] + p = state["pos"] q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1) - v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * dt - p_new = p + v * dt + v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * d_vel_dt + p_new = p + v * d_vel_dt # Predicted state vector at time t + dt - state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.gyro_bx:State.n_states])]]) + state_new = state.copy() + state_new["quat_nominal"] = sf.V4(q_new.w, q_new.x, q_new.y, q_new.z), # convert to Hamiltonian form + state_new["vel"] = v_new, + state_new["pos"] = p_new, # State propagation jacobian - A = state_new.jacobian(state) - G = state_new.jacobian(sf.V6.block_matrix([[d_vel], [d_ang]])) + A = VState(state_new.to_storage()).jacobian(state, tangent_space = False) + G = VState(state_new.to_storage()).jacobian(sf.V6.block_matrix([[d_vel], [d_ang]]), tangent_space = False) # Covariance propagation var_u = sf.Matrix.diag([d_vel_var[0], d_vel_var[1], d_vel_var[2], d_ang_var, d_ang_var, d_ang_var]) @@ -120,8 +149,8 @@ def predict_covariance( # Generate the equations for the upper triangular matrix and the diagonal only # Since the matrix is symmetric, the lower triangle does not need to be derived # and can simply be copied in the implementation - for index in range(State.n_states): - for j in range(State.n_states): + for index in range(state.storage_dim()): + for j in range(state.storage_dim()): if index > j: P_new[index,j] = 0 @@ -129,43 +158,77 @@ def predict_covariance( def compute_airspeed_innov_and_innov_var( state: VState, - P: MState, + P: MTangent, airspeed: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.Scalar, sf.Scalar): - vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + state = State.from_storage(state) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind airspeed_pred = vel_rel.norm(epsilon=epsilon) innov = airspeed_pred - airspeed - H = sf.V1(airspeed_pred).jacobian(state) + H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) def compute_airspeed_h_and_k( state: VState, - P: MState, + P: MTangent, innov_var: sf.Scalar, epsilon: sf.Scalar -) -> (VState, VState): +) -> (VTangent, VTangent): - vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + state = State.from_storage(state) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind airspeed_pred = vel_rel.norm(epsilon=epsilon) - H = sf.V1(airspeed_pred).jacobian(state) + H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False) K = P * H.T / sf.Max(innov_var, epsilon) return (H.T, K) +def compute_wind_init_and_cov_from_airspeed( + v_local: sf.V3, + heading: sf.Scalar, + airspeed: sf.Scalar, + v_var: sf.V3, + heading_var: sf.Scalar, + sideslip_var: sf.Scalar, + airspeed_var: sf.Scalar, +) -> (sf.V2, sf.M22): + + # Initialise wind states assuming horizontal flight + sideslip = sf.Symbol("beta") + wind = sf.V2(v_local[0] - airspeed * sf.cos(heading + sideslip), v_local[1] - airspeed * sf.sin(heading + sideslip)) + J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed]) + + R = sf.M55() + R[0,0] = v_var[0] + R[1,1] = v_var[1] + R[2,2] = heading_var + R[3,3] = sideslip_var + R[4,4] = airspeed_var + + P = J * R * J.T + + # Assume zero sideslip + P = P.subs({sideslip: 0.0}) + wind = wind.subs({sideslip: 0.0}) + return (wind, P) + def predict_sideslip( - state: VState, + state: State, epsilon: sf.Scalar ) -> (sf.Scalar): - vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind relative_wind_body = state_to_rot3(state).inverse() * vel_rel # Small angle approximation of side slip model @@ -176,166 +239,176 @@ def predict_sideslip( def compute_sideslip_innov_and_innov_var( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.Scalar, sf.Scalar, sf.Scalar): + state = State.from_storage(state) sideslip_pred = predict_sideslip(state, epsilon); innov = sideslip_pred - 0.0 - H = sf.V1(sideslip_pred).jacobian(state) + H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) def compute_sideslip_h_and_k( state: VState, - P: MState, + P: MTangent, innov_var: sf.Scalar, epsilon: sf.Scalar -) -> (VState, VState): +) -> (VTangent, VTangent): + state = State.from_storage(state) sideslip_pred = predict_sideslip(state, epsilon); - H = sf.V1(sideslip_pred).jacobian(state) + H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False) K = P * H.T / sf.Max(innov_var, epsilon) return (H.T, K) def predict_mag_body(state) -> sf.V3: - mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz]) - mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz]) + mag_field_earth = state["mag_I"] + mag_bias_body = state["mag_B"] mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body return mag_body def compute_mag_innov_innov_var_and_hx( state: VState, - P: MState, + P: MTangent, meas: sf.V3, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VState): +) -> (sf.V3, sf.V3, VTangent): + state = State.from_storage(state) meas_pred = predict_mag_body(state); innov = meas_pred - meas innov_var = sf.V3() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var[1] = (Hy * P * Hy.T + R)[0,0] - Hz = sf.V1(meas_pred[2]).jacobian(state) + Hz = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False) innov_var[2] = (Hz * P * Hz.T + R)[0,0] return (innov, innov_var, Hx.T) def compute_mag_y_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[1]).jacobian(state) + H = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_mag_z_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[2]).jacobian(state) + H = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_321_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Fix the singularity at pi/2 by inserting epsilon meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_321_innov_var_and_h_alternate( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form that has a singularity at yaw 0 instead of pi/2 meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_312_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form to be used when close to pitch +-pi/2 meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_312_innov_var_and_h_alternate( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form to be used when close to pitch +-pi/2 meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_mag_declination_pred_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, sf.Scalar, VState): +) -> (sf.Scalar, sf.Scalar, VTangent): - meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon) + state = State.from_storage(state) + meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) @@ -344,8 +417,7 @@ def predict_opt_flow(state, distance, epsilon): R_to_body = state_to_rot3(state).inverse() # Calculate earth relative velocity in a non-rotating sensor frame - v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) - rel_vel_sensor = R_to_body * v + rel_vel_sensor = R_to_body * state["vel"] # Divide by range to get predicted angular LOS rates relative to X and Y # axes. Note these are rates in a non-rotating sensor frame @@ -359,43 +431,46 @@ def predict_opt_flow(state, distance, epsilon): def compute_flow_xy_innov_var_and_hx( state: VState, - P: MState, + P: MTangent, distance: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V2, VState): +) -> (sf.V2, VTangent): + state = State.from_storage(state) meas_pred = predict_opt_flow(state, distance, epsilon); innov_var = sf.V2() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var[1] = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hx.T) def compute_flow_y_innov_var_and_h( state: VState, - P: MState, + P: MTangent, distance: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_opt_flow(state, distance, epsilon); - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hy.T) def compute_gnss_yaw_pred_innov_var_and_h( state: VState, - P: MState, + P: MTangent, antenna_yaw_offset: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, sf.Scalar, VState): +) -> (sf.Scalar, sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state) # define antenna vector in body frame @@ -407,13 +482,13 @@ def compute_gnss_yaw_pred_innov_var_and_h( # Calculate the yaw angle from the projection meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) def predict_drag( - state: VState, + state: State, rho: sf.Scalar, cd: sf.Scalar, cm: sf.Scalar, @@ -421,9 +496,8 @@ def predict_drag( ) -> (sf.Scalar): R_to_body = state_to_rot3(state).inverse() - vel_rel = sf.V3(state[State.vx] - state[State.wx], - state[State.vy] - state[State.wy], - state[State.vz]) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind vel_rel_body = R_to_body * vel_rel vel_rel_body_xy = sf.V2(vel_rel_body[0], vel_rel_body[1]) @@ -435,52 +509,53 @@ def predict_drag( def compute_drag_x_innov_var_and_k( state: VState, - P: MState, + P: MTangent, rho: sf.Scalar, cd: sf.Scalar, cm: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False) innov_var = (Hx * P * Hx.T + R)[0,0] Ktotal = P * Hx.T / sf.Max(innov_var, epsilon) - K = VState() - K[State.wx] = Ktotal[State.wx] - K[State.wy] = Ktotal[State.wy] + K = VTangent() + K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] return (innov_var, K) def compute_drag_y_innov_var_and_k( state: VState, - P: MState, + P: MTangent, rho: sf.Scalar, cd: sf.Scalar, cm: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var = (Hy * P * Hy.T + R)[0,0] Ktotal = P * Hy.T / sf.Max(innov_var, epsilon) - K = VState() - K[State.wx] = Ktotal[State.wx] - K[State.wy] = Ktotal[State.wy] + K = VTangent() + K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] return (innov_var, K) def compute_gravity_innov_var_and_k_and_h( state: VState, - P: MState, + P: MTangent, meas: sf.V3, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VState, VState, VState): +) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent): + state = State.from_storage(state) # get transform from earth to body frame R_to_body = state_to_rot3(state).inverse() @@ -496,7 +571,7 @@ def compute_gravity_innov_var_and_k_and_h( # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) # for each axis for i in range(3): - H = sf.V1(meas_pred[i]).jacobian(state) + H = sf.V1(meas_pred[i]).jacobian(state, tangent_space=False) innov_var[i] = (H * P * H.T + R)[0,0] K[i] = P * H.T / innov_var[i] @@ -504,52 +579,66 @@ def compute_gravity_innov_var_and_k_and_h( def quat_var_to_rot_var( state: VState, - P: MState, + P: MTangent, epsilon: sf.Scalar -): - J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state) +) -> sf.V3: + state = State.from_storage(state) + J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state, tangent_space=False) rot_cov = J * P * J.T return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2]) -def yaw_var_to_lower_triangular_quat_cov( +def rot_var_ned_to_lower_triangular_quat_cov( state: VState, - yaw_var: sf.Scalar -): - q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + rot_var_ned: sf.V3 +) -> sf.M44: + # This function converts an attitude variance defined by a 3D vector in NED frame + # into a 4x4 covariance matrix representing the uncertainty on each of the 4 quaternion parameters + # Note: the resulting quaternion uncertainty is defined as a perturbation + # at the tip of the quaternion (i.e.:body-frame uncertainty) + state = State.from_storage(state) + q = state["quat_nominal"] attitude = state_to_rot3(state) J = q.jacobian(attitude) - # Convert yaw uncertainty from NED to body frame - yaw_cov_ned = sf.M33.diag([0, 0, yaw_var]) + # Convert uncertainties from NED to body frame + rot_cov_ned = sf.M33.diag(rot_var_ned) adjoint = attitude.to_rotation_matrix() # the adjoint of SO(3) is simply the rotation matrix itself - yaw_cov_body = adjoint.T * yaw_cov_ned * adjoint + rot_cov_body = adjoint.T * rot_cov_ned * adjoint # Convert yaw (body) to quaternion parameter uncertainty - q_var = J * yaw_cov_body * J.T + q_var = J * rot_cov_body * J.T # Generate lower trangle only and copy it to the upper part in implementation (produces less code) return q_var.lower_triangle() print("Derive EKF2 equations...") -generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) -generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) +generate_px4_function(predict_covariance, output_names=None) + +if not args.disable_mag: + generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"]) + generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) + generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) + generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) + +if not args.disable_wind: + generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) + generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) + generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) + generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) + generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) -generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) -generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) -generate_px4_function(predict_covariance, output_names=["P_new"]) -generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) -generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"]) generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"]) -generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"]) +generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"]) +generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) -generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) -generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) + generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"]) -generate_px4_function(yaw_var_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) +generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) + +generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py index 29f2504133..2d321eb6ed 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py @@ -33,6 +33,9 @@ File: derivation_terrain_estimator.py Description: """ +import symforce +symforce.set_epsilon_to_symbol() + import symforce.symbolic as sf from derivation_utils import * @@ -43,7 +46,7 @@ def predict_opt_flow( pos_z: sf.Scalar, epsilon : sf.Scalar ): - R_to_earth = quat_to_rot(q_att) + R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix() flow_pred = sf.V2() dist = - (pos_z - terrain_vpos) dist = add_epsilon_sign(dist, dist, epsilon) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index ca2987e00f..bd91c3ba61 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- """ - Copyright (c) 2022 PX4 Development Team + Copyright (c) 2022-2023 PX4 Development Team Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -87,3 +87,80 @@ def generate_python_function(function_name, output_names): metadata = codegen.generate_function( output_dir="generated", skip_directory_nesting=True) + +def build_state_struct(state, T="float"): + out = "struct StateSample {\n" + + def TypeFromLength(len): + if len == 1: + return f"{T}" + elif len == 2: + return f"matrix::Vector2<{T}>" + elif len == 3: + return f"matrix::Vector3<{T}>" + elif len == 4: + return f"matrix::Quaternion<{T}>" + else: + raise NotImplementedError + + for key, val in state.items(): + out += f"\t{TypeFromLength(len(val))} {key}{{}};\n" + + state_size = state.storage_dim() + out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \ + + f"\t\tmatrix::Vector<{T}, {state_size}> state;\n" + + index = state.index() + for key in index: + out += f"\t\tstate.slice<{index[key].storage_dim}, 1>({index[key].offset}, 0) = {key};\n" + + out += "\t\treturn state;\n" + out += "\t};\n" # Data + + # const ref vector access + first_field = next(iter(state)) + + out += f"\n\tconst matrix::Vector<{T}, {state_size}>& vector() const {{\n" \ + + f"\t\treturn *reinterpret_cast*>(const_cast(reinterpret_cast(&{first_field})));\n" \ + + f"\t}};\n\n" + + out += "};\n" # StateSample + + out += f"static_assert(sizeof(matrix::Vector<{T}, {state_size}>) == sizeof(StateSample), \"state vector doesn't match StateSample size\");\n" + + return out + +def build_tangent_state_struct(state, tangent_state_index): + out = "struct IdxDof { unsigned idx; unsigned dof; };\n" + + out += "namespace State {\n" + + start_index = 0 + for key in tangent_state_index.keys(): + out += f"\tstatic constexpr IdxDof {key}{{{tangent_state_index[key].idx}, {tangent_state_index[key].dof}}};\n" + + out += f"\tstatic constexpr uint8_t size{{{state.tangent_dim()}}};\n" + out += "};\n" # namespace State + return out + +def generate_px4_state(state, tangent_state_index): + print("Generate EKF tangent state definition") + filename = "state.h" + f = open(f"./generated/{filename}", "w") + header = ["// --------------------------------------------------\n", + "// This file was autogenerated, do NOT modify by hand\n", + "// --------------------------------------------------\n", + "\n#ifndef EKF_STATE_H", + "\n#define EKF_STATE_H\n\n", + "#include \n\n", + "namespace estimator\n{\n"] + f.writelines(header) + + f.write(build_state_struct(state)) + f.write("\n") + f.write(build_tangent_state_struct(state, tangent_state_index)) + + f.write("}\n") # namespace estimator + f.write("#endif // !EKF_STATE_H\n") + f.close() + print(f" |- {filename}") diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py index 1b0bdb9076..b950b54d09 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py @@ -33,6 +33,9 @@ File: derivation_yaw_estimator.py Description: """ +import symforce +symforce.set_epsilon_to_symbol() + import symforce.symbolic as sf from derivation_utils import * diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h new file mode 100644 index 0000000000..95144609e6 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h @@ -0,0 +1,73 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_wind_init_and_cov_from_airspeed + * + * Args: + * v_local: Matrix31 + * heading: Scalar + * airspeed: Scalar + * v_var: Matrix31 + * heading_var: Scalar + * sideslip_var: Scalar + * airspeed_var: Scalar + * + * Outputs: + * wind: Matrix21 + * P_wind: Matrix22 + */ +template +void ComputeWindInitAndCovFromAirspeed(const matrix::Matrix& v_local, + const Scalar heading, const Scalar airspeed, + const matrix::Matrix& v_var, + const Scalar heading_var, const Scalar sideslip_var, + const Scalar airspeed_var, + matrix::Matrix* const wind = nullptr, + matrix::Matrix* const P_wind = nullptr) { + // Total ops: 29 + + // Input arrays + + // Intermediate terms (9) + const Scalar _tmp0 = std::cos(heading); + const Scalar _tmp1 = std::sin(heading); + const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)); + const Scalar _tmp3 = std::pow(airspeed, Scalar(2)); + const Scalar _tmp4 = _tmp3 * sideslip_var; + const Scalar _tmp5 = _tmp3 * heading_var; + const Scalar _tmp6 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp7 = _tmp0 * _tmp1; + const Scalar _tmp8 = -_tmp4 * _tmp7 - _tmp5 * _tmp7 + _tmp7 * airspeed_var; + + // Output terms (2) + if (wind != nullptr) { + matrix::Matrix& _wind = (*wind); + + _wind(0, 0) = -_tmp0 * airspeed + v_local(0, 0); + _wind(1, 0) = -_tmp1 * airspeed + v_local(1, 0); + } + + if (P_wind != nullptr) { + matrix::Matrix& _p_wind = (*P_wind); + + _p_wind(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var(0, 0); + _p_wind(1, 0) = _tmp8; + _p_wind(0, 1) = _tmp8; + _p_wind(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var(1, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h index a24f06b065..3637c654a3 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h @@ -19,1058 +19,1052 @@ namespace sym { * state: Matrix24_1 * P: Matrix24_24 * d_vel: Matrix31 + * d_vel_dt: Scalar * d_vel_var: Matrix31 * d_ang: Matrix31 + * d_ang_dt: Scalar * d_ang_var: Scalar - * dt: Scalar * * Outputs: - * P_new: Matrix24_24 + * res: Matrix24_24 */ template -void PredictCovariance(const matrix::Matrix& state, - const matrix::Matrix& P, - const matrix::Matrix& d_vel, - const matrix::Matrix& d_vel_var, - const matrix::Matrix& d_ang, const Scalar d_ang_var, - const Scalar dt, matrix::Matrix* const P_new = nullptr) { +matrix::Matrix PredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& d_vel, + const Scalar d_vel_dt, + const matrix::Matrix& d_vel_var, + const matrix::Matrix& d_ang, + const Scalar d_ang_dt, const Scalar d_ang_var) { // Total ops: 2882 // Input arrays - // Intermediate terms (174) - const Scalar _tmp0 = Scalar(0.5) * d_ang(0, 0); - const Scalar _tmp1 = Scalar(0.5) * dt; - const Scalar _tmp2 = _tmp1 * state(10, 0); + // Intermediate terms (173) + const Scalar _tmp0 = Scalar(0.5) * d_ang(1, 0); + const Scalar _tmp1 = Scalar(0.5) * d_ang_dt; + const Scalar _tmp2 = _tmp1 * state(11, 0); const Scalar _tmp3 = -_tmp0 + _tmp2; const Scalar _tmp4 = Scalar(0.5) * d_ang(2, 0); const Scalar _tmp5 = _tmp1 * state(12, 0); const Scalar _tmp6 = -_tmp4 + _tmp5; - const Scalar _tmp7 = Scalar(0.5) * d_ang(1, 0); - const Scalar _tmp8 = _tmp1 * state(11, 0); + const Scalar _tmp7 = Scalar(0.5) * d_ang(0, 0); + const Scalar _tmp8 = _tmp1 * state(10, 0); const Scalar _tmp9 = -_tmp7 + _tmp8; - const Scalar _tmp10 = _tmp1 * state(2, 0); + const Scalar _tmp10 = _tmp1 * state(1, 0); const Scalar _tmp11 = _tmp1 * state(3, 0); - const Scalar _tmp12 = _tmp1 * state(1, 0); - const Scalar _tmp13 = P(0, 12) + P(1, 12) * _tmp3 + P(10, 12) * _tmp12 + P(11, 12) * _tmp10 + - P(12, 12) * _tmp11 + P(2, 12) * _tmp9 + P(3, 12) * _tmp6; - const Scalar _tmp14 = P(0, 11) + P(1, 11) * _tmp3 + P(10, 11) * _tmp12 + P(11, 11) * _tmp10 + - P(12, 11) * _tmp11 + P(2, 11) * _tmp9 + P(3, 11) * _tmp6; - const Scalar _tmp15 = P(0, 10) + P(1, 10) * _tmp3 + P(10, 10) * _tmp12 + P(11, 10) * _tmp10 + - P(12, 10) * _tmp11 + P(2, 10) * _tmp9 + P(3, 10) * _tmp6; - const Scalar _tmp16 = P(0, 2) + P(1, 2) * _tmp3 + P(10, 2) * _tmp12 + P(11, 2) * _tmp10 + - P(12, 2) * _tmp11 + P(2, 2) * _tmp9 + P(3, 2) * _tmp6; - const Scalar _tmp17 = P(0, 1) + P(1, 1) * _tmp3 + P(10, 1) * _tmp12 + P(11, 1) * _tmp10 + - P(12, 1) * _tmp11 + P(2, 1) * _tmp9 + P(3, 1) * _tmp6; - const Scalar _tmp18 = P(0, 3) + P(1, 3) * _tmp3 + P(10, 3) * _tmp12 + P(11, 3) * _tmp10 + - P(12, 3) * _tmp11 + P(2, 3) * _tmp9 + P(3, 3) * _tmp6; - const Scalar _tmp19 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp20 = Scalar(0.25) * d_ang_var; - const Scalar _tmp21 = _tmp19 * _tmp20; - const Scalar _tmp22 = P(0, 0) + P(1, 0) * _tmp3 + P(10, 0) * _tmp12 + P(11, 0) * _tmp10 + - P(12, 0) * _tmp11 + P(2, 0) * _tmp9 + P(3, 0) * _tmp6; - const Scalar _tmp23 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp24 = _tmp20 * _tmp23; - const Scalar _tmp25 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp26 = _tmp20 * _tmp25; - const Scalar _tmp27 = _tmp24 + _tmp26; - const Scalar _tmp28 = _tmp20 * state(1, 0); + const Scalar _tmp12 = _tmp1 * state(2, 0); + const Scalar _tmp13 = P(0, 10) + P(1, 10) * _tmp9 + P(10, 10) * _tmp10 + P(11, 10) * _tmp12 + + P(12, 10) * _tmp11 + P(2, 10) * _tmp3 + P(3, 10) * _tmp6; + const Scalar _tmp14 = P(0, 11) + P(1, 11) * _tmp9 + P(10, 11) * _tmp10 + P(11, 11) * _tmp12 + + P(12, 11) * _tmp11 + P(2, 11) * _tmp3 + P(3, 11) * _tmp6; + const Scalar _tmp15 = P(0, 12) + P(1, 12) * _tmp9 + P(10, 12) * _tmp10 + P(11, 12) * _tmp12 + + P(12, 12) * _tmp11 + P(2, 12) * _tmp3 + P(3, 12) * _tmp6; + const Scalar _tmp16 = P(0, 2) + P(1, 2) * _tmp9 + P(10, 2) * _tmp10 + P(11, 2) * _tmp12 + + P(12, 2) * _tmp11 + P(2, 2) * _tmp3 + P(3, 2) * _tmp6; + const Scalar _tmp17 = P(0, 3) + P(1, 3) * _tmp9 + P(10, 3) * _tmp10 + P(11, 3) * _tmp12 + + P(12, 3) * _tmp11 + P(2, 3) * _tmp3 + P(3, 3) * _tmp6; + const Scalar _tmp18 = P(11, 1) * _tmp1; + const Scalar _tmp19 = P(0, 1) + P(1, 1) * _tmp9 + P(10, 1) * _tmp10 + P(12, 1) * _tmp11 + + P(2, 1) * _tmp3 + P(3, 1) * _tmp6 + _tmp18 * state(2, 0); + const Scalar _tmp20 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp21 = Scalar(0.25) * d_ang_var; + const Scalar _tmp22 = _tmp20 * _tmp21; + const Scalar _tmp23 = P(0, 0) + P(1, 0) * _tmp9 + P(10, 0) * _tmp10 + P(11, 0) * _tmp12 + + P(12, 0) * _tmp11 + P(2, 0) * _tmp3 + P(3, 0) * _tmp6; + const Scalar _tmp24 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp25 = _tmp21 * _tmp24; + const Scalar _tmp26 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp27 = _tmp21 * _tmp26; + const Scalar _tmp28 = _tmp25 + _tmp27; const Scalar _tmp29 = _tmp1 * state(0, 0); - const Scalar _tmp30 = _tmp0 - _tmp2; - const Scalar _tmp31 = _tmp4 - _tmp5; - const Scalar _tmp32 = P(0, 10) * _tmp30 + P(1, 10) - P(10, 10) * _tmp29 + P(11, 10) * _tmp11 - - P(12, 10) * _tmp10 + P(2, 10) * _tmp31 + P(3, 10) * _tmp9; - const Scalar _tmp33 = P(0, 11) * _tmp30 + P(1, 11) - P(10, 11) * _tmp29 + P(11, 11) * _tmp11 - - P(12, 11) * _tmp10 + P(2, 11) * _tmp31 + P(3, 11) * _tmp9; - const Scalar _tmp34 = P(0, 12) * _tmp30 + P(1, 12) - P(10, 12) * _tmp29 + P(11, 12) * _tmp11 - - P(12, 12) * _tmp10 + P(2, 12) * _tmp31 + P(3, 12) * _tmp9; - const Scalar _tmp35 = P(0, 0) * _tmp30 + P(1, 0) - P(10, 0) * _tmp29 + P(11, 0) * _tmp11 - - P(12, 0) * _tmp10 + P(2, 0) * _tmp31 + P(3, 0) * _tmp9; - const Scalar _tmp36 = P(0, 3) * _tmp30 + P(1, 3) - P(10, 3) * _tmp29 + P(11, 3) * _tmp11 - - P(12, 3) * _tmp10 + P(2, 3) * _tmp31 + P(3, 3) * _tmp9; - const Scalar _tmp37 = P(0, 2) * _tmp30 + P(1, 2) - P(10, 2) * _tmp29 + P(11, 2) * _tmp11 - - P(12, 2) * _tmp10 + P(2, 2) * _tmp31 + P(3, 2) * _tmp9; - const Scalar _tmp38 = _tmp20 * std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp39 = P(0, 1) * _tmp30 + P(1, 1) - P(10, 1) * _tmp29 + P(11, 1) * _tmp11 - - P(12, 1) * _tmp10 + P(2, 1) * _tmp31 + P(3, 1) * _tmp9; - const Scalar _tmp40 = _tmp20 * state(0, 0); - const Scalar _tmp41 = _tmp7 - _tmp8; - const Scalar _tmp42 = P(0, 11) * _tmp41 + P(1, 11) * _tmp6 - P(10, 11) * _tmp11 - - P(11, 11) * _tmp29 + P(12, 11) * _tmp12 + P(2, 11) + P(3, 11) * _tmp30; - const Scalar _tmp43 = P(0, 10) * _tmp41 + P(1, 10) * _tmp6 - P(10, 10) * _tmp11 - - P(11, 10) * _tmp29 + P(12, 10) * _tmp12 + P(2, 10) + P(3, 10) * _tmp30; - const Scalar _tmp44 = P(0, 12) * _tmp41 + P(1, 12) * _tmp6 - P(10, 12) * _tmp11 - - P(11, 12) * _tmp29 + P(12, 12) * _tmp12 + P(2, 12) + P(3, 12) * _tmp30; - const Scalar _tmp45 = P(0, 3) * _tmp41 + P(1, 3) * _tmp6 - P(10, 3) * _tmp11 - P(11, 3) * _tmp29 + - P(12, 3) * _tmp12 + P(2, 3) + P(3, 3) * _tmp30; - const Scalar _tmp46 = P(0, 1) * _tmp41 + P(1, 1) * _tmp6 - P(10, 1) * _tmp11 - P(11, 1) * _tmp29 + - P(12, 1) * _tmp12 + P(2, 1) + P(3, 1) * _tmp30; - const Scalar _tmp47 = P(0, 0) * _tmp41 + P(1, 0) * _tmp6 - P(10, 0) * _tmp11 - P(11, 0) * _tmp29 + - P(12, 0) * _tmp12 + P(2, 0) + P(3, 0) * _tmp30; - const Scalar _tmp48 = P(0, 2) * _tmp41 + P(1, 2) * _tmp6 - P(10, 2) * _tmp11 - P(11, 2) * _tmp29 + - P(12, 2) * _tmp12 + P(2, 2) + P(3, 2) * _tmp30; - const Scalar _tmp49 = _tmp21 + _tmp38; - const Scalar _tmp50 = P(0, 10) * _tmp31 + P(1, 10) * _tmp41 + P(10, 10) * _tmp10 - - P(11, 10) * _tmp12 - P(12, 10) * _tmp29 + P(2, 10) * _tmp3 + P(3, 10); - const Scalar _tmp51 = P(0, 11) * _tmp31 + P(1, 11) * _tmp41 + P(10, 11) * _tmp10 - - P(11, 11) * _tmp12 - P(12, 11) * _tmp29 + P(2, 11) * _tmp3 + P(3, 11); - const Scalar _tmp52 = P(0, 12) * _tmp31 + P(1, 12) * _tmp41 + P(10, 12) * _tmp10 - - P(11, 12) * _tmp12 - P(12, 12) * _tmp29 + P(2, 12) * _tmp3 + P(3, 12); - const Scalar _tmp53 = P(0, 2) * _tmp31 + P(1, 2) * _tmp41 + P(10, 2) * _tmp10 - - P(11, 2) * _tmp12 - P(12, 2) * _tmp29 + P(2, 2) * _tmp3 + P(3, 2); - const Scalar _tmp54 = P(0, 1) * _tmp31 + P(1, 1) * _tmp41 + P(10, 1) * _tmp10 - - P(11, 1) * _tmp12 - P(12, 1) * _tmp29 + P(2, 1) * _tmp3 + P(3, 1); - const Scalar _tmp55 = P(0, 0) * _tmp31 + P(1, 0) * _tmp41 + P(10, 0) * _tmp10 - - P(11, 0) * _tmp12 - P(12, 0) * _tmp29 + P(2, 0) * _tmp3 + P(3, 0); - const Scalar _tmp56 = P(0, 3) * _tmp31 + P(1, 3) * _tmp41 + P(10, 3) * _tmp10 - - P(11, 3) * _tmp12 - P(12, 3) * _tmp29 + P(2, 3) * _tmp3 + P(3, 3); - const Scalar _tmp57 = P(0, 13) + P(1, 13) * _tmp3 + P(10, 13) * _tmp12 + P(11, 13) * _tmp10 + - P(12, 13) * _tmp11 + P(2, 13) * _tmp9 + P(3, 13) * _tmp6; - const Scalar _tmp58 = -2 * _tmp23; - const Scalar _tmp59 = 1 - 2 * _tmp25; - const Scalar _tmp60 = _tmp58 + _tmp59; - const Scalar _tmp61 = _tmp60 * dt; - const Scalar _tmp62 = P(0, 15) + P(1, 15) * _tmp3 + P(10, 15) * _tmp12 + P(11, 15) * _tmp10 + - P(12, 15) * _tmp11 + P(2, 15) * _tmp9 + P(3, 15) * _tmp6; - const Scalar _tmp63 = 2 * state(0, 0); - const Scalar _tmp64 = _tmp63 * state(2, 0); - const Scalar _tmp65 = 2 * state(1, 0); - const Scalar _tmp66 = _tmp65 * state(3, 0); - const Scalar _tmp67 = _tmp64 + _tmp66; - const Scalar _tmp68 = _tmp67 * dt; - const Scalar _tmp69 = P(0, 14) + P(1, 14) * _tmp3 + P(10, 14) * _tmp12 + P(11, 14) * _tmp10 + - P(12, 14) * _tmp11 + P(2, 14) * _tmp9 + P(3, 14) * _tmp6; - const Scalar _tmp70 = _tmp63 * state(3, 0); - const Scalar _tmp71 = _tmp65 * state(2, 0); - const Scalar _tmp72 = -_tmp70 + _tmp71; - const Scalar _tmp73 = _tmp72 * dt; - const Scalar _tmp74 = d_vel(1, 0) - dt * state(14, 0); - const Scalar _tmp75 = 2 * state(3, 0); - const Scalar _tmp76 = _tmp74 * _tmp75; - const Scalar _tmp77 = d_vel(2, 0) - dt * state(15, 0); - const Scalar _tmp78 = 2 * state(2, 0); - const Scalar _tmp79 = _tmp77 * _tmp78; - const Scalar _tmp80 = -_tmp76 + _tmp79; - const Scalar _tmp81 = _tmp74 * _tmp78; - const Scalar _tmp82 = _tmp75 * _tmp77; - const Scalar _tmp83 = _tmp81 + _tmp82; - const Scalar _tmp84 = _tmp63 * _tmp74; - const Scalar _tmp85 = d_vel(0, 0) - dt * state(13, 0); - const Scalar _tmp86 = 4 * _tmp85; - const Scalar _tmp87 = _tmp65 * _tmp77; - const Scalar _tmp88 = -_tmp84 - _tmp86 * state(3, 0) + _tmp87; - const Scalar _tmp89 = _tmp65 * _tmp74; - const Scalar _tmp90 = _tmp63 * _tmp77; - const Scalar _tmp91 = -_tmp86 * state(2, 0) + _tmp89 + _tmp90; - const Scalar _tmp92 = P(0, 4) + P(1, 4) * _tmp3 + P(10, 4) * _tmp12 + P(11, 4) * _tmp10 + - P(12, 4) * _tmp11 + P(2, 4) * _tmp9 + P(3, 4) * _tmp6; - const Scalar _tmp93 = P(0, 13) * _tmp30 + P(1, 13) - P(10, 13) * _tmp29 + P(11, 13) * _tmp11 - - P(12, 13) * _tmp10 + P(2, 13) * _tmp31 + P(3, 13) * _tmp9; - const Scalar _tmp94 = P(0, 15) * _tmp30 + P(1, 15) - P(10, 15) * _tmp29 + P(11, 15) * _tmp11 - - P(12, 15) * _tmp10 + P(2, 15) * _tmp31 + P(3, 15) * _tmp9; - const Scalar _tmp95 = P(0, 14) * _tmp30 + P(1, 14) - P(10, 14) * _tmp29 + P(11, 14) * _tmp11 - - P(12, 14) * _tmp10 + P(2, 14) * _tmp31 + P(3, 14) * _tmp9; - const Scalar _tmp96 = P(0, 4) * _tmp30 + P(1, 4) - P(10, 4) * _tmp29 + P(11, 4) * _tmp11 - - P(12, 4) * _tmp10 + P(2, 4) * _tmp31 + P(3, 4) * _tmp9; - const Scalar _tmp97 = P(0, 15) * _tmp41 + P(1, 15) * _tmp6 - P(10, 15) * _tmp11 - - P(11, 15) * _tmp29 + P(12, 15) * _tmp12 + P(2, 15) + P(3, 15) * _tmp30; - const Scalar _tmp98 = P(0, 14) * _tmp41 + P(1, 14) * _tmp6 - P(10, 14) * _tmp11 - - P(11, 14) * _tmp29 + P(12, 14) * _tmp12 + P(2, 14) + P(3, 14) * _tmp30; - const Scalar _tmp99 = _tmp98 * dt; - const Scalar _tmp100 = P(0, 13) * _tmp41 + P(1, 13) * _tmp6 - P(10, 13) * _tmp11 - - P(11, 13) * _tmp29 + P(12, 13) * _tmp12 + P(2, 13) + P(3, 13) * _tmp30; - const Scalar _tmp101 = _tmp100 * dt; - const Scalar _tmp102 = P(0, 4) * _tmp41 + P(1, 4) * _tmp6 - P(10, 4) * _tmp11 - - P(11, 4) * _tmp29 + P(12, 4) * _tmp12 + P(2, 4) + P(3, 4) * _tmp30; - const Scalar _tmp103 = P(0, 15) * _tmp31 + P(1, 15) * _tmp41 + P(10, 15) * _tmp10 - - P(11, 15) * _tmp12 - P(12, 15) * _tmp29 + P(2, 15) * _tmp3 + P(3, 15); - const Scalar _tmp104 = P(0, 14) * _tmp31 + P(1, 14) * _tmp41 + P(10, 14) * _tmp10 - - P(11, 14) * _tmp12 - P(12, 14) * _tmp29 + P(2, 14) * _tmp3 + P(3, 14); - const Scalar _tmp105 = P(0, 13) * _tmp31 + P(1, 13) * _tmp41 + P(10, 13) * _tmp10 - - P(11, 13) * _tmp12 - P(12, 13) * _tmp29 + P(2, 13) * _tmp3 + P(3, 13); - const Scalar _tmp106 = P(0, 4) * _tmp31 + P(1, 4) * _tmp41 + P(10, 4) * _tmp10 - - P(11, 4) * _tmp12 - P(12, 4) * _tmp29 + P(2, 4) * _tmp3 + P(3, 4); - const Scalar _tmp107 = P(0, 1) * _tmp80 + P(1, 1) * _tmp83 - P(13, 1) * _tmp61 - - P(14, 1) * _tmp73 - P(15, 1) * _tmp68 + P(2, 1) * _tmp91 + - P(3, 1) * _tmp88 + P(4, 1); - const Scalar _tmp108 = P(0, 0) * _tmp80 + P(1, 0) * _tmp83 - P(13, 0) * _tmp61 - - P(14, 0) * _tmp73 - P(15, 0) * _tmp68 + P(2, 0) * _tmp91 + - P(3, 0) * _tmp88 + P(4, 0); - const Scalar _tmp109 = P(0, 13) * _tmp80 + P(1, 13) * _tmp83 - P(13, 13) * _tmp61 - - P(14, 13) * _tmp73 - P(15, 13) * _tmp68 + P(2, 13) * _tmp91 + - P(3, 13) * _tmp88 + P(4, 13); - const Scalar _tmp110 = P(0, 15) * _tmp80 + P(1, 15) * _tmp83 - P(13, 15) * _tmp61 - - P(14, 15) * _tmp73 - P(15, 15) * _tmp68 + P(2, 15) * _tmp91 + - P(3, 15) * _tmp88 + P(4, 15); - const Scalar _tmp111 = P(0, 14) * _tmp80 + P(1, 14) * _tmp83 - P(13, 14) * _tmp61 - - P(14, 14) * _tmp73 - P(15, 14) * _tmp68 + P(2, 14) * _tmp91 + - P(3, 14) * _tmp88 + P(4, 14); - const Scalar _tmp112 = P(0, 3) * _tmp80 + P(1, 3) * _tmp83 - P(13, 3) * _tmp61 - - P(14, 3) * _tmp73 - P(15, 3) * _tmp68 + P(2, 3) * _tmp91 + - P(3, 3) * _tmp88 + P(4, 3); - const Scalar _tmp113 = P(0, 2) * _tmp80 + P(1, 2) * _tmp83 - P(13, 2) * _tmp61 - - P(14, 2) * _tmp73 - P(15, 2) * _tmp68 + P(2, 2) * _tmp91 + - P(3, 2) * _tmp88 + P(4, 2); - const Scalar _tmp114 = P(0, 4) * _tmp80 + P(1, 4) * _tmp83 - P(13, 4) * _tmp61 - - P(14, 4) * _tmp73 - P(15, 4) * _tmp68 + P(2, 4) * _tmp91 + - P(3, 4) * _tmp88 + P(4, 4); - const Scalar _tmp115 = -2 * _tmp19; - const Scalar _tmp116 = _tmp115 + _tmp58 + 1; - const Scalar _tmp117 = _tmp116 * dt; - const Scalar _tmp118 = _tmp70 + _tmp71; - const Scalar _tmp119 = _tmp118 * dt; - const Scalar _tmp120 = _tmp65 * _tmp85; - const Scalar _tmp121 = _tmp120 + _tmp82; - const Scalar _tmp122 = _tmp75 * _tmp85; - const Scalar _tmp123 = _tmp122 - _tmp87; - const Scalar _tmp124 = 4 * _tmp74; - const Scalar _tmp125 = _tmp78 * _tmp85; - const Scalar _tmp126 = -_tmp124 * state(1, 0) + _tmp125 - _tmp90; - const Scalar _tmp127 = _tmp63 * _tmp85; - const Scalar _tmp128 = -_tmp124 * state(3, 0) + _tmp127 + _tmp79; - const Scalar _tmp129 = _tmp75 * state(2, 0); - const Scalar _tmp130 = _tmp63 * state(1, 0); - const Scalar _tmp131 = _tmp129 - _tmp130; - const Scalar _tmp132 = _tmp131 * dt; - const Scalar _tmp133 = P(0, 5) + P(1, 5) * _tmp3 + P(10, 5) * _tmp12 + P(11, 5) * _tmp10 + - P(12, 5) * _tmp11 + P(2, 5) * _tmp9 + P(3, 5) * _tmp6; - const Scalar _tmp134 = P(0, 5) * _tmp30 + P(1, 5) - P(10, 5) * _tmp29 + P(11, 5) * _tmp11 - - P(12, 5) * _tmp10 + P(2, 5) * _tmp31 + P(3, 5) * _tmp9; - const Scalar _tmp135 = P(0, 5) * _tmp41 + P(1, 5) * _tmp6 - P(10, 5) * _tmp11 - - P(11, 5) * _tmp29 + P(12, 5) * _tmp12 + P(2, 5) + P(3, 5) * _tmp30; - const Scalar _tmp136 = P(0, 5) * _tmp31 + P(1, 5) * _tmp41 + P(10, 5) * _tmp10 - - P(11, 5) * _tmp12 - P(12, 5) * _tmp29 + P(2, 5) * _tmp3 + P(3, 5); - const Scalar _tmp137 = _tmp118 * d_vel_var(0, 0); - const Scalar _tmp138 = _tmp72 * d_vel_var(1, 0); - const Scalar _tmp139 = _tmp67 * d_vel_var(2, 0); - const Scalar _tmp140 = P(0, 5) * _tmp80 + P(1, 5) * _tmp83 - P(13, 5) * _tmp61 - - P(14, 5) * _tmp73 - P(15, 5) * _tmp68 + P(2, 5) * _tmp91 + - P(3, 5) * _tmp88 + P(4, 5); - const Scalar _tmp141 = P(0, 0) * _tmp123 + P(1, 0) * _tmp126 - P(13, 0) * _tmp119 - - P(14, 0) * _tmp117 - P(15, 0) * _tmp132 + P(2, 0) * _tmp121 + - P(3, 0) * _tmp128 + P(5, 0); - const Scalar _tmp142 = P(0, 2) * _tmp123 + P(1, 2) * _tmp126 - P(13, 2) * _tmp119 - - P(14, 2) * _tmp117 - P(15, 2) * _tmp132 + P(2, 2) * _tmp121 + - P(3, 2) * _tmp128 + P(5, 2); - const Scalar _tmp143 = P(0, 14) * _tmp123 + P(1, 14) * _tmp126 - P(13, 14) * _tmp119 - - P(14, 14) * _tmp117 - P(15, 14) * _tmp132 + P(2, 14) * _tmp121 + - P(3, 14) * _tmp128 + P(5, 14); - const Scalar _tmp144 = P(0, 15) * _tmp123 + P(1, 15) * _tmp126 - P(13, 15) * _tmp119 - - P(14, 15) * _tmp117 - P(15, 15) * _tmp132 + P(2, 15) * _tmp121 + - P(3, 15) * _tmp128 + P(5, 15); - const Scalar _tmp145 = P(0, 3) * _tmp123 + P(1, 3) * _tmp126 - P(13, 3) * _tmp119 - - P(14, 3) * _tmp117 - P(15, 3) * _tmp132 + P(2, 3) * _tmp121 + - P(3, 3) * _tmp128 + P(5, 3); - const Scalar _tmp146 = P(0, 1) * _tmp123 + P(1, 1) * _tmp126 - P(13, 1) * _tmp119 - - P(14, 1) * _tmp117 - P(15, 1) * _tmp132 + P(2, 1) * _tmp121 + - P(3, 1) * _tmp128 + P(5, 1); - const Scalar _tmp147 = P(0, 13) * _tmp123 + P(1, 13) * _tmp126 - P(13, 13) * _tmp119 - - P(14, 13) * _tmp117 - P(15, 13) * _tmp132 + P(2, 13) * _tmp121 + - P(3, 13) * _tmp128 + P(5, 13); - const Scalar _tmp148 = _tmp147 * dt; - const Scalar _tmp149 = P(0, 5) * _tmp123 + P(1, 5) * _tmp126 - P(13, 5) * _tmp119 - - P(14, 5) * _tmp117 - P(15, 5) * _tmp132 + P(2, 5) * _tmp121 + - P(3, 5) * _tmp128 + P(5, 5); - const Scalar _tmp150 = _tmp115 + _tmp59; - const Scalar _tmp151 = _tmp150 * dt; - const Scalar _tmp152 = -_tmp64 + _tmp66; - const Scalar _tmp153 = _tmp152 * dt; - const Scalar _tmp154 = -_tmp125 + _tmp89; - const Scalar _tmp155 = _tmp120 + _tmp81; - const Scalar _tmp156 = 4 * _tmp77; - const Scalar _tmp157 = _tmp122 - _tmp156 * state(1, 0) + _tmp84; - const Scalar _tmp158 = -_tmp127 - _tmp156 * state(2, 0) + _tmp76; - const Scalar _tmp159 = _tmp129 + _tmp130; - const Scalar _tmp160 = _tmp159 * dt; - const Scalar _tmp161 = P(11, 6) * _tmp1; - const Scalar _tmp162 = P(0, 6) + P(1, 6) * _tmp3 + P(10, 6) * _tmp12 + P(12, 6) * _tmp11 + - P(2, 6) * _tmp9 + P(3, 6) * _tmp6 + _tmp161 * state(2, 0); - const Scalar _tmp163 = P(0, 6) * _tmp30 + P(1, 6) - P(10, 6) * _tmp29 - P(12, 6) * _tmp10 + - P(2, 6) * _tmp31 + P(3, 6) * _tmp9 + _tmp161 * state(3, 0); - const Scalar _tmp164 = P(0, 6) * _tmp41 + P(1, 6) * _tmp6 - P(10, 6) * _tmp11 + - P(12, 6) * _tmp12 + P(2, 6) + P(3, 6) * _tmp30 - _tmp161 * state(0, 0); - const Scalar _tmp165 = P(0, 6) * _tmp31 + P(1, 6) * _tmp41 + P(10, 6) * _tmp10 - - P(11, 6) * _tmp12 - P(12, 6) * _tmp29 + P(2, 6) * _tmp3 + P(3, 6); - const Scalar _tmp166 = P(0, 6) * _tmp80 + P(1, 6) * _tmp83 - P(13, 6) * _tmp61 - - P(14, 6) * _tmp73 - P(15, 6) * _tmp68 + P(2, 6) * _tmp91 + - P(3, 6) * _tmp88 + P(4, 6); - const Scalar _tmp167 = P(0, 6) * _tmp123 + P(1, 6) * _tmp126 - P(13, 6) * _tmp119 - - P(14, 6) * _tmp117 - P(15, 6) * _tmp132 + P(2, 6) * _tmp121 + - P(3, 6) * _tmp128 + P(5, 6); - const Scalar _tmp168 = P(0, 13) * _tmp154 + P(1, 13) * _tmp157 - P(13, 13) * _tmp153 - - P(14, 13) * _tmp160 - P(15, 13) * _tmp151 + P(2, 13) * _tmp158 + - P(3, 13) * _tmp155 + P(6, 13); - const Scalar _tmp169 = P(0, 15) * _tmp154 + P(1, 15) * _tmp157 - P(13, 15) * _tmp153 - - P(14, 15) * _tmp160 - P(15, 15) * _tmp151 + P(2, 15) * _tmp158 + - P(3, 15) * _tmp155 + P(6, 15); - const Scalar _tmp170 = P(0, 14) * _tmp154 + P(1, 14) * _tmp157 - P(13, 14) * _tmp153 - - P(14, 14) * _tmp160 - P(15, 14) * _tmp151 + P(2, 14) * _tmp158 + - P(3, 14) * _tmp155 + P(6, 14); - const Scalar _tmp171 = P(0, 6) * _tmp154 + P(1, 6) * _tmp157 - P(13, 6) * _tmp153 - - P(14, 6) * _tmp160 - P(15, 6) * _tmp151 + P(2, 6) * _tmp158 + - P(3, 6) * _tmp155 + P(6, 6); + const Scalar _tmp30 = _tmp21 * state(1, 0); + const Scalar _tmp31 = _tmp7 - _tmp8; + const Scalar _tmp32 = _tmp4 - _tmp5; + const Scalar _tmp33 = P(0, 12) * _tmp31 + P(1, 12) - P(10, 12) * _tmp29 + P(11, 12) * _tmp11 - + P(12, 12) * _tmp12 + P(2, 12) * _tmp32 + P(3, 12) * _tmp3; + const Scalar _tmp34 = P(0, 10) * _tmp31 + P(1, 10) - P(10, 10) * _tmp29 + P(11, 10) * _tmp11 - + P(12, 10) * _tmp12 + P(2, 10) * _tmp32 + P(3, 10) * _tmp3; + const Scalar _tmp35 = P(0, 11) * _tmp31 + P(1, 11) - P(10, 11) * _tmp29 + P(11, 11) * _tmp11 - + P(12, 11) * _tmp12 + P(2, 11) * _tmp32 + P(3, 11) * _tmp3; + const Scalar _tmp36 = P(0, 2) * _tmp31 + P(1, 2) - P(10, 2) * _tmp29 + P(11, 2) * _tmp11 - + P(12, 2) * _tmp12 + P(2, 2) * _tmp32 + P(3, 2) * _tmp3; + const Scalar _tmp37 = P(0, 3) * _tmp31 + P(1, 3) - P(10, 3) * _tmp29 + P(11, 3) * _tmp11 - + P(12, 3) * _tmp12 + P(2, 3) * _tmp32 + P(3, 3) * _tmp3; + const Scalar _tmp38 = P(0, 0) * _tmp31 + P(1, 0) - P(10, 0) * _tmp29 + P(11, 0) * _tmp11 - + P(12, 0) * _tmp12 + P(2, 0) * _tmp32 + P(3, 0) * _tmp3; + const Scalar _tmp39 = P(0, 1) * _tmp31 + P(1, 1) - P(10, 1) * _tmp29 - P(12, 1) * _tmp12 + + P(2, 1) * _tmp32 + P(3, 1) * _tmp3 + _tmp18 * state(3, 0); + const Scalar _tmp40 = _tmp21 * std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp41 = _tmp22 + _tmp40; + const Scalar _tmp42 = _tmp21 * state(0, 0); + const Scalar _tmp43 = _tmp0 - _tmp2; + const Scalar _tmp44 = P(0, 0) * _tmp43 + P(1, 0) * _tmp6 - P(10, 0) * _tmp11 - P(11, 0) * _tmp29 + + P(12, 0) * _tmp10 + P(2, 0) + P(3, 0) * _tmp31; + const Scalar _tmp45 = P(0, 1) * _tmp43 + P(1, 1) * _tmp6 - P(10, 1) * _tmp11 + P(12, 1) * _tmp10 + + P(2, 1) + P(3, 1) * _tmp31 - _tmp18 * state(0, 0); + const Scalar _tmp46 = P(0, 3) * _tmp43 + P(1, 3) * _tmp6 - P(10, 3) * _tmp11 - P(11, 3) * _tmp29 + + P(12, 3) * _tmp10 + P(2, 3) + P(3, 3) * _tmp31; + const Scalar _tmp47 = P(0, 11) * _tmp43 + P(1, 11) * _tmp6 - P(10, 11) * _tmp11 - + P(11, 11) * _tmp29 + P(12, 11) * _tmp10 + P(2, 11) + P(3, 11) * _tmp31; + const Scalar _tmp48 = P(0, 10) * _tmp43 + P(1, 10) * _tmp6 - P(10, 10) * _tmp11 - + P(11, 10) * _tmp29 + P(12, 10) * _tmp10 + P(2, 10) + P(3, 10) * _tmp31; + const Scalar _tmp49 = P(0, 12) * _tmp43 + P(1, 12) * _tmp6 - P(10, 12) * _tmp11 - + P(11, 12) * _tmp29 + P(12, 12) * _tmp10 + P(2, 12) + P(3, 12) * _tmp31; + const Scalar _tmp50 = P(0, 2) * _tmp43 + P(1, 2) * _tmp6 - P(10, 2) * _tmp11 - P(11, 2) * _tmp29 + + P(12, 2) * _tmp10 + P(2, 2) + P(3, 2) * _tmp31; + const Scalar _tmp51 = P(0, 2) * _tmp32 + P(1, 2) * _tmp43 + P(10, 2) * _tmp12 - + P(11, 2) * _tmp10 - P(12, 2) * _tmp29 + P(2, 2) * _tmp9 + P(3, 2); + const Scalar _tmp52 = P(0, 1) * _tmp32 + P(1, 1) * _tmp43 + P(10, 1) * _tmp12 - + P(11, 1) * _tmp10 - P(12, 1) * _tmp29 + P(2, 1) * _tmp9 + P(3, 1); + const Scalar _tmp53 = P(0, 0) * _tmp32 + P(1, 0) * _tmp43 + P(10, 0) * _tmp12 - + P(11, 0) * _tmp10 - P(12, 0) * _tmp29 + P(2, 0) * _tmp9 + P(3, 0); + const Scalar _tmp54 = P(0, 11) * _tmp32 + P(1, 11) * _tmp43 + P(10, 11) * _tmp12 - + P(11, 11) * _tmp10 - P(12, 11) * _tmp29 + P(2, 11) * _tmp9 + P(3, 11); + const Scalar _tmp55 = P(0, 12) * _tmp32 + P(1, 12) * _tmp43 + P(10, 12) * _tmp12 - + P(11, 12) * _tmp10 - P(12, 12) * _tmp29 + P(2, 12) * _tmp9 + P(3, 12); + const Scalar _tmp56 = P(0, 10) * _tmp32 + P(1, 10) * _tmp43 + P(10, 10) * _tmp12 - + P(11, 10) * _tmp10 - P(12, 10) * _tmp29 + P(2, 10) * _tmp9 + P(3, 10); + const Scalar _tmp57 = P(0, 3) * _tmp32 + P(1, 3) * _tmp43 + P(10, 3) * _tmp12 - + P(11, 3) * _tmp10 - P(12, 3) * _tmp29 + P(2, 3) * _tmp9 + P(3, 3); + const Scalar _tmp58 = d_vel(2, 0) - d_vel_dt * state(15, 0); + const Scalar _tmp59 = 2 * state(2, 0); + const Scalar _tmp60 = _tmp58 * _tmp59; + const Scalar _tmp61 = d_vel(1, 0) - d_vel_dt * state(14, 0); + const Scalar _tmp62 = 2 * _tmp61; + const Scalar _tmp63 = _tmp62 * state(3, 0); + const Scalar _tmp64 = _tmp60 - _tmp63; + const Scalar _tmp65 = P(0, 13) + P(1, 13) * _tmp9 + P(10, 13) * _tmp10 + P(11, 13) * _tmp12 + + P(12, 13) * _tmp11 + P(2, 13) * _tmp3 + P(3, 13) * _tmp6; + const Scalar _tmp66 = -2 * _tmp20; + const Scalar _tmp67 = 1 - 2 * _tmp24; + const Scalar _tmp68 = _tmp66 + _tmp67; + const Scalar _tmp69 = _tmp68 * d_vel_dt; + const Scalar _tmp70 = P(0, 15) + P(1, 15) * _tmp9 + P(10, 15) * _tmp10 + P(11, 15) * _tmp12 + + P(12, 15) * _tmp11 + P(2, 15) * _tmp3 + P(3, 15) * _tmp6; + const Scalar _tmp71 = _tmp59 * state(0, 0); + const Scalar _tmp72 = 2 * state(1, 0); + const Scalar _tmp73 = _tmp72 * state(3, 0); + const Scalar _tmp74 = _tmp71 + _tmp73; + const Scalar _tmp75 = _tmp74 * d_vel_dt; + const Scalar _tmp76 = P(0, 14) + P(1, 14) * _tmp9 + P(10, 14) * _tmp10 + P(11, 14) * _tmp12 + + P(12, 14) * _tmp11 + P(2, 14) * _tmp3 + P(3, 14) * _tmp6; + const Scalar _tmp77 = 2 * state(0, 0); + const Scalar _tmp78 = _tmp77 * state(3, 0); + const Scalar _tmp79 = _tmp59 * state(1, 0); + const Scalar _tmp80 = -_tmp78 + _tmp79; + const Scalar _tmp81 = _tmp80 * d_vel_dt; + const Scalar _tmp82 = 2 * _tmp58; + const Scalar _tmp83 = _tmp82 * state(3, 0); + const Scalar _tmp84 = _tmp59 * _tmp61; + const Scalar _tmp85 = _tmp83 + _tmp84; + const Scalar _tmp86 = d_vel(0, 0) - d_vel_dt * state(13, 0); + const Scalar _tmp87 = 4 * _tmp86; + const Scalar _tmp88 = _tmp82 * state(0, 0); + const Scalar _tmp89 = _tmp62 * state(1, 0); + const Scalar _tmp90 = -_tmp87 * state(2, 0) + _tmp88 + _tmp89; + const Scalar _tmp91 = _tmp82 * state(1, 0); + const Scalar _tmp92 = _tmp62 * state(0, 0); + const Scalar _tmp93 = -_tmp87 * state(3, 0) + _tmp91 - _tmp92; + const Scalar _tmp94 = P(11, 4) * _tmp1; + const Scalar _tmp95 = P(0, 4) + P(1, 4) * _tmp9 + P(10, 4) * _tmp10 + P(12, 4) * _tmp11 + + P(2, 4) * _tmp3 + P(3, 4) * _tmp6 + _tmp94 * state(2, 0); + const Scalar _tmp96 = P(0, 13) * _tmp31 + P(1, 13) - P(10, 13) * _tmp29 + P(11, 13) * _tmp11 - + P(12, 13) * _tmp12 + P(2, 13) * _tmp32 + P(3, 13) * _tmp3; + const Scalar _tmp97 = P(0, 14) * _tmp31 + P(1, 14) - P(10, 14) * _tmp29 + P(11, 14) * _tmp11 - + P(12, 14) * _tmp12 + P(2, 14) * _tmp32 + P(3, 14) * _tmp3; + const Scalar _tmp98 = P(0, 15) * _tmp31 + P(1, 15) - P(10, 15) * _tmp29 + P(11, 15) * _tmp11 - + P(12, 15) * _tmp12 + P(2, 15) * _tmp32 + P(3, 15) * _tmp3; + const Scalar _tmp99 = P(0, 4) * _tmp31 + P(1, 4) - P(10, 4) * _tmp29 - P(12, 4) * _tmp12 + + P(2, 4) * _tmp32 + P(3, 4) * _tmp3 + _tmp94 * state(3, 0); + const Scalar _tmp100 = P(0, 13) * _tmp43 + P(1, 13) * _tmp6 - P(10, 13) * _tmp11 - + P(11, 13) * _tmp29 + P(12, 13) * _tmp10 + P(2, 13) + P(3, 13) * _tmp31; + const Scalar _tmp101 = P(0, 14) * _tmp43 + P(1, 14) * _tmp6 - P(10, 14) * _tmp11 - + P(11, 14) * _tmp29 + P(12, 14) * _tmp10 + P(2, 14) + P(3, 14) * _tmp31; + const Scalar _tmp102 = P(0, 15) * _tmp43 + P(1, 15) * _tmp6 - P(10, 15) * _tmp11 - + P(11, 15) * _tmp29 + P(12, 15) * _tmp10 + P(2, 15) + P(3, 15) * _tmp31; + const Scalar _tmp103 = P(0, 4) * _tmp43 + P(1, 4) * _tmp6 - P(10, 4) * _tmp11 + + P(12, 4) * _tmp10 + P(2, 4) + P(3, 4) * _tmp31 - _tmp94 * state(0, 0); + const Scalar _tmp104 = P(0, 13) * _tmp32 + P(1, 13) * _tmp43 + P(10, 13) * _tmp12 - + P(11, 13) * _tmp10 - P(12, 13) * _tmp29 + P(2, 13) * _tmp9 + P(3, 13); + const Scalar _tmp105 = P(0, 14) * _tmp32 + P(1, 14) * _tmp43 + P(10, 14) * _tmp12 - + P(11, 14) * _tmp10 - P(12, 14) * _tmp29 + P(2, 14) * _tmp9 + P(3, 14); + const Scalar _tmp106 = P(0, 15) * _tmp32 + P(1, 15) * _tmp43 + P(10, 15) * _tmp12 - + P(11, 15) * _tmp10 - P(12, 15) * _tmp29 + P(2, 15) * _tmp9 + P(3, 15); + const Scalar _tmp107 = P(0, 4) * _tmp32 + P(1, 4) * _tmp43 + P(10, 4) * _tmp12 - + P(12, 4) * _tmp29 + P(2, 4) * _tmp9 + P(3, 4) - _tmp94 * state(1, 0); + const Scalar _tmp108 = P(0, 13) * _tmp64 + P(1, 13) * _tmp85 - P(13, 13) * _tmp69 - + P(14, 13) * _tmp81 - P(15, 13) * _tmp75 + P(2, 13) * _tmp90 + + P(3, 13) * _tmp93 + P(4, 13); + const Scalar _tmp109 = P(0, 14) * _tmp64 + P(1, 14) * _tmp85 - P(13, 14) * _tmp69 - + P(14, 14) * _tmp81 - P(15, 14) * _tmp75 + P(2, 14) * _tmp90 + + P(3, 14) * _tmp93 + P(4, 14); + const Scalar _tmp110 = P(0, 15) * _tmp64 + P(1, 15) * _tmp85 - P(13, 15) * _tmp69 - + P(14, 15) * _tmp81 - P(15, 15) * _tmp75 + P(2, 15) * _tmp90 + + P(3, 15) * _tmp93 + P(4, 15); + const Scalar _tmp111 = P(0, 1) * _tmp64 + P(1, 1) * _tmp85 - P(13, 1) * _tmp69 - + P(14, 1) * _tmp81 - P(15, 1) * _tmp75 + P(2, 1) * _tmp90 + + P(3, 1) * _tmp93 + P(4, 1); + const Scalar _tmp112 = P(0, 0) * _tmp64 + P(1, 0) * _tmp85 - P(13, 0) * _tmp69 - + P(14, 0) * _tmp81 - P(15, 0) * _tmp75 + P(2, 0) * _tmp90 + + P(3, 0) * _tmp93 + P(4, 0); + const Scalar _tmp113 = P(0, 3) * _tmp64 + P(1, 3) * _tmp85 - P(13, 3) * _tmp69 - + P(14, 3) * _tmp81 - P(15, 3) * _tmp75 + P(2, 3) * _tmp90 + + P(3, 3) * _tmp93 + P(4, 3); + const Scalar _tmp114 = P(0, 2) * _tmp64 + P(1, 2) * _tmp85 - P(13, 2) * _tmp69 - + P(14, 2) * _tmp81 - P(15, 2) * _tmp75 + P(2, 2) * _tmp90 + + P(3, 2) * _tmp93 + P(4, 2); + const Scalar _tmp115 = P(0, 4) * _tmp64 + P(1, 4) * _tmp85 - P(13, 4) * _tmp69 - + P(14, 4) * _tmp81 - P(15, 4) * _tmp75 + P(2, 4) * _tmp90 + + P(3, 4) * _tmp93 + P(4, 4); + const Scalar _tmp116 = _tmp59 * state(3, 0); + const Scalar _tmp117 = _tmp72 * state(0, 0); + const Scalar _tmp118 = _tmp116 - _tmp117; + const Scalar _tmp119 = _tmp118 * d_vel_dt; + const Scalar _tmp120 = -2 * _tmp26; + const Scalar _tmp121 = _tmp120 + _tmp67; + const Scalar _tmp122 = _tmp121 * d_vel_dt; + const Scalar _tmp123 = _tmp78 + _tmp79; + const Scalar _tmp124 = _tmp123 * d_vel_dt; + const Scalar _tmp125 = _tmp59 * _tmp86; + const Scalar _tmp126 = 4 * _tmp61; + const Scalar _tmp127 = _tmp125 - _tmp126 * state(1, 0) - _tmp88; + const Scalar _tmp128 = _tmp77 * _tmp86; + const Scalar _tmp129 = -_tmp126 * state(3, 0) + _tmp128 + _tmp60; + const Scalar _tmp130 = 2 * _tmp86 * state(3, 0); + const Scalar _tmp131 = _tmp130 - _tmp91; + const Scalar _tmp132 = _tmp72 * _tmp86; + const Scalar _tmp133 = _tmp132 + _tmp83; + const Scalar _tmp134 = P(0, 5) + P(1, 5) * _tmp9 + P(10, 5) * _tmp10 + P(11, 5) * _tmp12 + + P(12, 5) * _tmp11 + P(2, 5) * _tmp3 + P(3, 5) * _tmp6; + const Scalar _tmp135 = P(0, 5) * _tmp31 + P(1, 5) - P(10, 5) * _tmp29 + P(11, 5) * _tmp11 - + P(12, 5) * _tmp12 + P(2, 5) * _tmp32 + P(3, 5) * _tmp3; + const Scalar _tmp136 = P(0, 5) * _tmp43 + P(1, 5) * _tmp6 - P(10, 5) * _tmp11 - + P(11, 5) * _tmp29 + P(12, 5) * _tmp10 + P(2, 5) + P(3, 5) * _tmp31; + const Scalar _tmp137 = P(0, 5) * _tmp32 + P(1, 5) * _tmp43 + P(10, 5) * _tmp12 - + P(11, 5) * _tmp10 - P(12, 5) * _tmp29 + P(2, 5) * _tmp9 + P(3, 5); + const Scalar _tmp138 = _tmp123 * d_vel_var(0, 0); + const Scalar _tmp139 = _tmp80 * d_vel_var(1, 0); + const Scalar _tmp140 = P(0, 5) * _tmp64 + P(1, 5) * _tmp85 - P(13, 5) * _tmp69 - + P(14, 5) * _tmp81 - P(15, 5) * _tmp75 + P(2, 5) * _tmp90 + + P(3, 5) * _tmp93 + P(4, 5); + const Scalar _tmp141 = P(0, 13) * _tmp131 + P(1, 13) * _tmp127 - P(13, 13) * _tmp124 - + P(14, 13) * _tmp122 - P(15, 13) * _tmp119 + P(2, 13) * _tmp133 + + P(3, 13) * _tmp129 + P(5, 13); + const Scalar _tmp142 = P(0, 14) * _tmp131 + P(1, 14) * _tmp127 - P(13, 14) * _tmp124 - + P(14, 14) * _tmp122 - P(15, 14) * _tmp119 + P(2, 14) * _tmp133 + + P(3, 14) * _tmp129 + P(5, 14); + const Scalar _tmp143 = P(0, 15) * _tmp131 + P(1, 15) * _tmp127 - P(13, 15) * _tmp124 - + P(14, 15) * _tmp122 - P(15, 15) * _tmp119 + P(2, 15) * _tmp133 + + P(3, 15) * _tmp129 + P(5, 15); + const Scalar _tmp144 = P(0, 2) * _tmp131 + P(1, 2) * _tmp127 - P(13, 2) * _tmp124 - + P(14, 2) * _tmp122 - P(15, 2) * _tmp119 + P(2, 2) * _tmp133 + + P(3, 2) * _tmp129 + P(5, 2); + const Scalar _tmp145 = P(0, 0) * _tmp131 + P(1, 0) * _tmp127 - P(13, 0) * _tmp124 - + P(14, 0) * _tmp122 - P(15, 0) * _tmp119 + P(2, 0) * _tmp133 + + P(3, 0) * _tmp129 + P(5, 0); + const Scalar _tmp146 = P(0, 3) * _tmp131 + P(1, 3) * _tmp127 - P(13, 3) * _tmp124 - + P(14, 3) * _tmp122 - P(15, 3) * _tmp119 + P(2, 3) * _tmp133 + + P(3, 3) * _tmp129 + P(5, 3); + const Scalar _tmp147 = P(0, 1) * _tmp131 + P(1, 1) * _tmp127 - P(13, 1) * _tmp124 - + P(14, 1) * _tmp122 - P(15, 1) * _tmp119 + P(2, 1) * _tmp133 + + P(3, 1) * _tmp129 + P(5, 1); + const Scalar _tmp148 = P(0, 5) * _tmp131 + P(1, 5) * _tmp127 - P(13, 5) * _tmp124 - + P(14, 5) * _tmp122 - P(15, 5) * _tmp119 + P(2, 5) * _tmp133 + + P(3, 5) * _tmp129 + P(5, 5); + const Scalar _tmp149 = _tmp116 + _tmp117; + const Scalar _tmp150 = _tmp149 * d_vel_dt; + const Scalar _tmp151 = _tmp120 + _tmp66 + 1; + const Scalar _tmp152 = _tmp151 * d_vel_dt; + const Scalar _tmp153 = -_tmp71 + _tmp73; + const Scalar _tmp154 = _tmp153 * d_vel_dt; + const Scalar _tmp155 = 4 * _tmp58; + const Scalar _tmp156 = _tmp130 - _tmp155 * state(1, 0) + _tmp92; + const Scalar _tmp157 = -_tmp128 - _tmp155 * state(2, 0) + _tmp63; + const Scalar _tmp158 = -_tmp125 + _tmp89; + const Scalar _tmp159 = _tmp132 + _tmp84; + const Scalar _tmp160 = P(11, 6) * _tmp1; + const Scalar _tmp161 = P(0, 6) + P(1, 6) * _tmp9 + P(10, 6) * _tmp10 + P(12, 6) * _tmp11 + + P(2, 6) * _tmp3 + P(3, 6) * _tmp6 + _tmp160 * state(2, 0); + const Scalar _tmp162 = P(0, 6) * _tmp31 + P(1, 6) - P(10, 6) * _tmp29 - P(12, 6) * _tmp12 + + P(2, 6) * _tmp32 + P(3, 6) * _tmp3 + _tmp160 * state(3, 0); + const Scalar _tmp163 = P(0, 6) * _tmp43 + P(1, 6) * _tmp6 - P(10, 6) * _tmp11 + + P(12, 6) * _tmp10 + P(2, 6) + P(3, 6) * _tmp31 - _tmp160 * state(0, 0); + const Scalar _tmp164 = P(0, 6) * _tmp32 + P(1, 6) * _tmp43 + P(10, 6) * _tmp12 - + P(11, 6) * _tmp10 - P(12, 6) * _tmp29 + P(2, 6) * _tmp9 + P(3, 6); + const Scalar _tmp165 = _tmp151 * d_vel_var(2, 0); + const Scalar _tmp166 = P(0, 6) * _tmp64 + P(1, 6) * _tmp85 - P(13, 6) * _tmp69 - + P(14, 6) * _tmp81 - P(15, 6) * _tmp75 + P(2, 6) * _tmp90 + + P(3, 6) * _tmp93 + P(4, 6); + const Scalar _tmp167 = P(0, 6) * _tmp131 + P(1, 6) * _tmp127 - P(13, 6) * _tmp124 - + P(14, 6) * _tmp122 - P(15, 6) * _tmp119 + P(2, 6) * _tmp133 + + P(3, 6) * _tmp129 + P(5, 6); + const Scalar _tmp168 = P(0, 13) * _tmp158 + P(1, 13) * _tmp156 - P(13, 13) * _tmp154 - + P(14, 13) * _tmp150 - P(15, 13) * _tmp152 + P(2, 13) * _tmp157 + + P(3, 13) * _tmp159 + P(6, 13); + const Scalar _tmp169 = P(0, 14) * _tmp158 + P(1, 14) * _tmp156 - P(13, 14) * _tmp154 - + P(14, 14) * _tmp150 - P(15, 14) * _tmp152 + P(2, 14) * _tmp157 + + P(3, 14) * _tmp159 + P(6, 14); + const Scalar _tmp170 = P(0, 15) * _tmp158 + P(1, 15) * _tmp156 - P(13, 15) * _tmp154 - + P(14, 15) * _tmp150 - P(15, 15) * _tmp152 + P(2, 15) * _tmp157 + + P(3, 15) * _tmp159 + P(6, 15); + const Scalar _tmp171 = P(0, 6) * _tmp158 + P(1, 6) * _tmp156 - P(13, 6) * _tmp154 - + P(14, 6) * _tmp150 - P(15, 6) * _tmp152 + P(2, 6) * _tmp157 + + P(3, 6) * _tmp159 + P(6, 6); const Scalar _tmp172 = P(11, 8) * _tmp1; - const Scalar _tmp173 = P(11, 9) * _tmp1; // Output terms (1) - if (P_new != nullptr) { - matrix::Matrix& _p_new = (*P_new); + matrix::Matrix _res; - _p_new(0, 0) = _tmp10 * _tmp14 + _tmp11 * _tmp13 + _tmp12 * _tmp15 + _tmp16 * _tmp9 + - _tmp17 * _tmp3 + _tmp18 * _tmp6 + _tmp21 + _tmp22 + _tmp27; - _p_new(1, 0) = 0; - _p_new(2, 0) = 0; - _p_new(3, 0) = 0; - _p_new(4, 0) = 0; - _p_new(5, 0) = 0; - _p_new(6, 0) = 0; - _p_new(7, 0) = 0; - _p_new(8, 0) = 0; - _p_new(9, 0) = 0; - _p_new(10, 0) = 0; - _p_new(11, 0) = 0; - _p_new(12, 0) = 0; - _p_new(13, 0) = 0; - _p_new(14, 0) = 0; - _p_new(15, 0) = 0; - _p_new(16, 0) = 0; - _p_new(17, 0) = 0; - _p_new(18, 0) = 0; - _p_new(19, 0) = 0; - _p_new(20, 0) = 0; - _p_new(21, 0) = 0; - _p_new(22, 0) = 0; - _p_new(23, 0) = 0; - _p_new(0, 1) = -_tmp10 * _tmp13 + _tmp11 * _tmp14 - _tmp15 * _tmp29 + _tmp16 * _tmp31 + _tmp17 + - _tmp18 * _tmp9 + _tmp22 * _tmp30 - _tmp28 * state(0, 0); - _p_new(1, 1) = -_tmp10 * _tmp34 + _tmp11 * _tmp33 + _tmp27 - _tmp29 * _tmp32 + _tmp30 * _tmp35 + - _tmp31 * _tmp37 + _tmp36 * _tmp9 + _tmp38 + _tmp39; - _p_new(2, 1) = 0; - _p_new(3, 1) = 0; - _p_new(4, 1) = 0; - _p_new(5, 1) = 0; - _p_new(6, 1) = 0; - _p_new(7, 1) = 0; - _p_new(8, 1) = 0; - _p_new(9, 1) = 0; - _p_new(10, 1) = 0; - _p_new(11, 1) = 0; - _p_new(12, 1) = 0; - _p_new(13, 1) = 0; - _p_new(14, 1) = 0; - _p_new(15, 1) = 0; - _p_new(16, 1) = 0; - _p_new(17, 1) = 0; - _p_new(18, 1) = 0; - _p_new(19, 1) = 0; - _p_new(20, 1) = 0; - _p_new(21, 1) = 0; - _p_new(22, 1) = 0; - _p_new(23, 1) = 0; - _p_new(0, 2) = -_tmp11 * _tmp15 + _tmp12 * _tmp13 - _tmp14 * _tmp29 + _tmp16 + _tmp17 * _tmp6 + - _tmp18 * _tmp30 + _tmp22 * _tmp41 - _tmp40 * state(2, 0); - _p_new(1, 2) = -_tmp11 * _tmp32 + _tmp12 * _tmp34 - _tmp28 * state(2, 0) - _tmp29 * _tmp33 + - _tmp30 * _tmp36 + _tmp35 * _tmp41 + _tmp37 + _tmp39 * _tmp6; - _p_new(2, 2) = -_tmp11 * _tmp43 + _tmp12 * _tmp44 + _tmp24 - _tmp29 * _tmp42 + _tmp30 * _tmp45 + - _tmp41 * _tmp47 + _tmp46 * _tmp6 + _tmp48 + _tmp49; - _p_new(3, 2) = 0; - _p_new(4, 2) = 0; - _p_new(5, 2) = 0; - _p_new(6, 2) = 0; - _p_new(7, 2) = 0; - _p_new(8, 2) = 0; - _p_new(9, 2) = 0; - _p_new(10, 2) = 0; - _p_new(11, 2) = 0; - _p_new(12, 2) = 0; - _p_new(13, 2) = 0; - _p_new(14, 2) = 0; - _p_new(15, 2) = 0; - _p_new(16, 2) = 0; - _p_new(17, 2) = 0; - _p_new(18, 2) = 0; - _p_new(19, 2) = 0; - _p_new(20, 2) = 0; - _p_new(21, 2) = 0; - _p_new(22, 2) = 0; - _p_new(23, 2) = 0; - _p_new(0, 3) = _tmp10 * _tmp15 - _tmp12 * _tmp14 - _tmp13 * _tmp29 + _tmp16 * _tmp3 + - _tmp17 * _tmp41 + _tmp18 + _tmp22 * _tmp31 - _tmp40 * state(3, 0); - _p_new(1, 3) = _tmp10 * _tmp32 - _tmp12 * _tmp33 - _tmp28 * state(3, 0) - _tmp29 * _tmp34 + - _tmp3 * _tmp37 + _tmp31 * _tmp35 + _tmp36 + _tmp39 * _tmp41; - _p_new(2, 3) = _tmp10 * _tmp43 - _tmp12 * _tmp42 - _tmp20 * state(2, 0) * state(3, 0) - - _tmp29 * _tmp44 + _tmp3 * _tmp48 + _tmp31 * _tmp47 + _tmp41 * _tmp46 + _tmp45; - _p_new(3, 3) = _tmp10 * _tmp50 - _tmp12 * _tmp51 + _tmp26 - _tmp29 * _tmp52 + _tmp3 * _tmp53 + - _tmp31 * _tmp55 + _tmp41 * _tmp54 + _tmp49 + _tmp56; - _p_new(4, 3) = 0; - _p_new(5, 3) = 0; - _p_new(6, 3) = 0; - _p_new(7, 3) = 0; - _p_new(8, 3) = 0; - _p_new(9, 3) = 0; - _p_new(10, 3) = 0; - _p_new(11, 3) = 0; - _p_new(12, 3) = 0; - _p_new(13, 3) = 0; - _p_new(14, 3) = 0; - _p_new(15, 3) = 0; - _p_new(16, 3) = 0; - _p_new(17, 3) = 0; - _p_new(18, 3) = 0; - _p_new(19, 3) = 0; - _p_new(20, 3) = 0; - _p_new(21, 3) = 0; - _p_new(22, 3) = 0; - _p_new(23, 3) = 0; - _p_new(0, 4) = _tmp16 * _tmp91 + _tmp17 * _tmp83 + _tmp18 * _tmp88 + _tmp22 * _tmp80 - - _tmp57 * _tmp61 - _tmp62 * _tmp68 - _tmp69 * _tmp73 + _tmp92; - _p_new(1, 4) = _tmp35 * _tmp80 + _tmp36 * _tmp88 + _tmp37 * _tmp91 + _tmp39 * _tmp83 - - _tmp61 * _tmp93 - _tmp68 * _tmp94 - _tmp73 * _tmp95 + _tmp96; - _p_new(2, 4) = -_tmp101 * _tmp60 + _tmp102 + _tmp45 * _tmp88 + _tmp46 * _tmp83 + - _tmp47 * _tmp80 + _tmp48 * _tmp91 - _tmp68 * _tmp97 - _tmp72 * _tmp99; - _p_new(3, 4) = -_tmp103 * _tmp68 - _tmp104 * _tmp73 - _tmp105 * _tmp61 + _tmp106 + - _tmp53 * _tmp91 + _tmp54 * _tmp83 + _tmp55 * _tmp80 + _tmp56 * _tmp88; - _p_new(4, 4) = _tmp107 * _tmp83 + _tmp108 * _tmp80 - _tmp109 * _tmp61 - _tmp110 * _tmp68 - - _tmp111 * _tmp73 + _tmp112 * _tmp88 + _tmp113 * _tmp91 + _tmp114 + - std::pow(_tmp60, Scalar(2)) * d_vel_var(0, 0) + - std::pow(_tmp67, Scalar(2)) * d_vel_var(2, 0) + - std::pow(_tmp72, Scalar(2)) * d_vel_var(1, 0); - _p_new(5, 4) = 0; - _p_new(6, 4) = 0; - _p_new(7, 4) = 0; - _p_new(8, 4) = 0; - _p_new(9, 4) = 0; - _p_new(10, 4) = 0; - _p_new(11, 4) = 0; - _p_new(12, 4) = 0; - _p_new(13, 4) = 0; - _p_new(14, 4) = 0; - _p_new(15, 4) = 0; - _p_new(16, 4) = 0; - _p_new(17, 4) = 0; - _p_new(18, 4) = 0; - _p_new(19, 4) = 0; - _p_new(20, 4) = 0; - _p_new(21, 4) = 0; - _p_new(22, 4) = 0; - _p_new(23, 4) = 0; - _p_new(0, 5) = -_tmp117 * _tmp69 - _tmp119 * _tmp57 + _tmp121 * _tmp16 + _tmp123 * _tmp22 + - _tmp126 * _tmp17 + _tmp128 * _tmp18 - _tmp132 * _tmp62 + _tmp133; - _p_new(1, 5) = -_tmp117 * _tmp95 - _tmp119 * _tmp93 + _tmp121 * _tmp37 + _tmp123 * _tmp35 + - _tmp126 * _tmp39 + _tmp128 * _tmp36 - _tmp132 * _tmp94 + _tmp134; - _p_new(2, 5) = -_tmp101 * _tmp118 - _tmp116 * _tmp99 + _tmp121 * _tmp48 + _tmp123 * _tmp47 + - _tmp126 * _tmp46 + _tmp128 * _tmp45 - _tmp132 * _tmp97 + _tmp135; - _p_new(3, 5) = -_tmp103 * _tmp132 - _tmp104 * _tmp117 - _tmp105 * _tmp119 + _tmp121 * _tmp53 + - _tmp123 * _tmp55 + _tmp126 * _tmp54 + _tmp128 * _tmp56 + _tmp136; - _p_new(4, 5) = _tmp107 * _tmp126 + _tmp108 * _tmp123 - _tmp109 * _tmp119 - _tmp110 * _tmp132 - - _tmp111 * _tmp117 + _tmp112 * _tmp128 + _tmp113 * _tmp121 + _tmp116 * _tmp138 + - _tmp131 * _tmp139 + _tmp137 * _tmp60 + _tmp140; - _p_new(5, 5) = std::pow(_tmp116, Scalar(2)) * d_vel_var(1, 0) - _tmp117 * _tmp143 + - std::pow(_tmp118, Scalar(2)) * d_vel_var(0, 0) - _tmp118 * _tmp148 + - _tmp121 * _tmp142 + _tmp123 * _tmp141 + _tmp126 * _tmp146 + _tmp128 * _tmp145 + - std::pow(_tmp131, Scalar(2)) * d_vel_var(2, 0) - _tmp132 * _tmp144 + _tmp149; - _p_new(6, 5) = 0; - _p_new(7, 5) = 0; - _p_new(8, 5) = 0; - _p_new(9, 5) = 0; - _p_new(10, 5) = 0; - _p_new(11, 5) = 0; - _p_new(12, 5) = 0; - _p_new(13, 5) = 0; - _p_new(14, 5) = 0; - _p_new(15, 5) = 0; - _p_new(16, 5) = 0; - _p_new(17, 5) = 0; - _p_new(18, 5) = 0; - _p_new(19, 5) = 0; - _p_new(20, 5) = 0; - _p_new(21, 5) = 0; - _p_new(22, 5) = 0; - _p_new(23, 5) = 0; - _p_new(0, 6) = -_tmp151 * _tmp62 - _tmp153 * _tmp57 + _tmp154 * _tmp22 + _tmp155 * _tmp18 + - _tmp157 * _tmp17 + _tmp158 * _tmp16 - _tmp160 * _tmp69 + _tmp162; - _p_new(1, 6) = -_tmp151 * _tmp94 - _tmp153 * _tmp93 + _tmp154 * _tmp35 + _tmp155 * _tmp36 + - _tmp157 * _tmp39 + _tmp158 * _tmp37 - _tmp160 * _tmp95 + _tmp163; - _p_new(2, 6) = -_tmp101 * _tmp152 - _tmp151 * _tmp97 + _tmp154 * _tmp47 + _tmp155 * _tmp45 + - _tmp157 * _tmp46 + _tmp158 * _tmp48 - _tmp159 * _tmp99 + _tmp164; - _p_new(3, 6) = -_tmp103 * _tmp151 - _tmp104 * _tmp160 - _tmp105 * _tmp153 + _tmp154 * _tmp55 + - _tmp155 * _tmp56 + _tmp157 * _tmp54 + _tmp158 * _tmp53 + _tmp165; - _p_new(4, 6) = _tmp107 * _tmp157 + _tmp108 * _tmp154 - _tmp109 * _tmp153 - _tmp110 * _tmp151 - - _tmp111 * _tmp160 + _tmp112 * _tmp155 + _tmp113 * _tmp158 + _tmp138 * _tmp159 + - _tmp139 * _tmp150 + _tmp152 * _tmp60 * d_vel_var(0, 0) + _tmp166; - _p_new(5, 6) = _tmp116 * _tmp159 * d_vel_var(1, 0) + _tmp131 * _tmp150 * d_vel_var(2, 0) + - _tmp137 * _tmp152 + _tmp141 * _tmp154 + _tmp142 * _tmp158 - _tmp143 * _tmp160 - - _tmp144 * _tmp151 + _tmp145 * _tmp155 + _tmp146 * _tmp157 - _tmp148 * _tmp152 + - _tmp167; - _p_new(6, 6) = - std::pow(_tmp150, Scalar(2)) * d_vel_var(2, 0) - _tmp151 * _tmp169 + - std::pow(_tmp152, Scalar(2)) * d_vel_var(0, 0) - _tmp153 * _tmp168 + - _tmp154 * (P(0, 0) * _tmp154 + P(1, 0) * _tmp157 - P(13, 0) * _tmp153 - P(14, 0) * _tmp160 - - P(15, 0) * _tmp151 + P(2, 0) * _tmp158 + P(3, 0) * _tmp155 + P(6, 0)) + - _tmp155 * (P(0, 3) * _tmp154 + P(1, 3) * _tmp157 - P(13, 3) * _tmp153 - P(14, 3) * _tmp160 - - P(15, 3) * _tmp151 + P(2, 3) * _tmp158 + P(3, 3) * _tmp155 + P(6, 3)) + - _tmp157 * (P(0, 1) * _tmp154 + P(1, 1) * _tmp157 - P(13, 1) * _tmp153 - P(14, 1) * _tmp160 - - P(15, 1) * _tmp151 + P(2, 1) * _tmp158 + P(3, 1) * _tmp155 + P(6, 1)) + - _tmp158 * (P(0, 2) * _tmp154 + P(1, 2) * _tmp157 - P(13, 2) * _tmp153 - P(14, 2) * _tmp160 - - P(15, 2) * _tmp151 + P(2, 2) * _tmp158 + P(3, 2) * _tmp155 + P(6, 2)) + - std::pow(_tmp159, Scalar(2)) * d_vel_var(1, 0) - _tmp160 * _tmp170 + _tmp171; - _p_new(7, 6) = 0; - _p_new(8, 6) = 0; - _p_new(9, 6) = 0; - _p_new(10, 6) = 0; - _p_new(11, 6) = 0; - _p_new(12, 6) = 0; - _p_new(13, 6) = 0; - _p_new(14, 6) = 0; - _p_new(15, 6) = 0; - _p_new(16, 6) = 0; - _p_new(17, 6) = 0; - _p_new(18, 6) = 0; - _p_new(19, 6) = 0; - _p_new(20, 6) = 0; - _p_new(21, 6) = 0; - _p_new(22, 6) = 0; - _p_new(23, 6) = 0; - _p_new(0, 7) = P(0, 7) + P(1, 7) * _tmp3 + P(10, 7) * _tmp12 + P(11, 7) * _tmp10 + - P(12, 7) * _tmp11 + P(2, 7) * _tmp9 + P(3, 7) * _tmp6 + _tmp92 * dt; - _p_new(1, 7) = P(0, 7) * _tmp30 + P(1, 7) - P(10, 7) * _tmp29 + P(11, 7) * _tmp11 - - P(12, 7) * _tmp10 + P(2, 7) * _tmp31 + P(3, 7) * _tmp9 + _tmp96 * dt; - _p_new(2, 7) = P(0, 7) * _tmp41 + P(1, 7) * _tmp6 - P(10, 7) * _tmp11 - P(11, 7) * _tmp29 + - P(12, 7) * _tmp12 + P(2, 7) + P(3, 7) * _tmp30 + _tmp102 * dt; - _p_new(3, 7) = P(0, 7) * _tmp31 + P(1, 7) * _tmp41 + P(10, 7) * _tmp10 - P(11, 7) * _tmp12 - - P(12, 7) * _tmp29 + P(2, 7) * _tmp3 + P(3, 7) + _tmp106 * dt; - _p_new(4, 7) = P(0, 7) * _tmp80 + P(1, 7) * _tmp83 - P(13, 7) * _tmp61 - P(14, 7) * _tmp73 - - P(15, 7) * _tmp68 + P(2, 7) * _tmp91 + P(3, 7) * _tmp88 + P(4, 7) + _tmp114 * dt; - _p_new(5, 7) = - P(0, 7) * _tmp123 + P(1, 7) * _tmp126 - P(13, 7) * _tmp119 - P(14, 7) * _tmp117 - - P(15, 7) * _tmp132 + P(2, 7) * _tmp121 + P(3, 7) * _tmp128 + P(5, 7) + - dt * (P(0, 4) * _tmp123 + P(1, 4) * _tmp126 - P(13, 4) * _tmp119 - P(14, 4) * _tmp117 - - P(15, 4) * _tmp132 + P(2, 4) * _tmp121 + P(3, 4) * _tmp128 + P(5, 4)); - _p_new(6, 7) = - P(0, 7) * _tmp154 + P(1, 7) * _tmp157 - P(13, 7) * _tmp153 - P(14, 7) * _tmp160 - - P(15, 7) * _tmp151 + P(2, 7) * _tmp158 + P(3, 7) * _tmp155 + P(6, 7) + - dt * (P(0, 4) * _tmp154 + P(1, 4) * _tmp157 - P(13, 4) * _tmp153 - P(14, 4) * _tmp160 - - P(15, 4) * _tmp151 + P(2, 4) * _tmp158 + P(3, 4) * _tmp155 + P(6, 4)); - _p_new(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4)); - _p_new(8, 7) = 0; - _p_new(9, 7) = 0; - _p_new(10, 7) = 0; - _p_new(11, 7) = 0; - _p_new(12, 7) = 0; - _p_new(13, 7) = 0; - _p_new(14, 7) = 0; - _p_new(15, 7) = 0; - _p_new(16, 7) = 0; - _p_new(17, 7) = 0; - _p_new(18, 7) = 0; - _p_new(19, 7) = 0; - _p_new(20, 7) = 0; - _p_new(21, 7) = 0; - _p_new(22, 7) = 0; - _p_new(23, 7) = 0; - _p_new(0, 8) = P(0, 8) + P(1, 8) * _tmp3 + P(10, 8) * _tmp12 + P(12, 8) * _tmp11 + - P(2, 8) * _tmp9 + P(3, 8) * _tmp6 + _tmp133 * dt + _tmp172 * state(2, 0); - _p_new(1, 8) = P(0, 8) * _tmp30 + P(1, 8) - P(10, 8) * _tmp29 - P(12, 8) * _tmp10 + - P(2, 8) * _tmp31 + P(3, 8) * _tmp9 + _tmp134 * dt + _tmp172 * state(3, 0); - _p_new(2, 8) = P(0, 8) * _tmp41 + P(1, 8) * _tmp6 - P(10, 8) * _tmp11 + P(12, 8) * _tmp12 + - P(2, 8) + P(3, 8) * _tmp30 + _tmp135 * dt - _tmp172 * state(0, 0); - _p_new(3, 8) = P(0, 8) * _tmp31 + P(1, 8) * _tmp41 + P(10, 8) * _tmp10 - P(11, 8) * _tmp12 - - P(12, 8) * _tmp29 + P(2, 8) * _tmp3 + P(3, 8) + _tmp136 * dt; - _p_new(4, 8) = P(0, 8) * _tmp80 + P(1, 8) * _tmp83 - P(13, 8) * _tmp61 - P(14, 8) * _tmp73 - - P(15, 8) * _tmp68 + P(2, 8) * _tmp91 + P(3, 8) * _tmp88 + P(4, 8) + _tmp140 * dt; - _p_new(5, 8) = P(0, 8) * _tmp123 + P(1, 8) * _tmp126 - P(13, 8) * _tmp119 - P(14, 8) * _tmp117 - - P(15, 8) * _tmp132 + P(2, 8) * _tmp121 + P(3, 8) * _tmp128 + P(5, 8) + - _tmp149 * dt; - _p_new(6, 8) = - P(0, 8) * _tmp154 + P(1, 8) * _tmp157 - P(13, 8) * _tmp153 - P(14, 8) * _tmp160 - - P(15, 8) * _tmp151 + P(2, 8) * _tmp158 + P(3, 8) * _tmp155 + P(6, 8) + - dt * (P(0, 5) * _tmp154 + P(1, 5) * _tmp157 - P(13, 5) * _tmp153 - P(14, 5) * _tmp160 - - P(15, 5) * _tmp151 + P(2, 5) * _tmp158 + P(3, 5) * _tmp155 + P(6, 5)); - _p_new(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5)); - _p_new(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5)); - _p_new(9, 8) = 0; - _p_new(10, 8) = 0; - _p_new(11, 8) = 0; - _p_new(12, 8) = 0; - _p_new(13, 8) = 0; - _p_new(14, 8) = 0; - _p_new(15, 8) = 0; - _p_new(16, 8) = 0; - _p_new(17, 8) = 0; - _p_new(18, 8) = 0; - _p_new(19, 8) = 0; - _p_new(20, 8) = 0; - _p_new(21, 8) = 0; - _p_new(22, 8) = 0; - _p_new(23, 8) = 0; - _p_new(0, 9) = P(0, 9) + P(1, 9) * _tmp3 + P(10, 9) * _tmp12 + P(12, 9) * _tmp11 + - P(2, 9) * _tmp9 + P(3, 9) * _tmp6 + _tmp162 * dt + _tmp173 * state(2, 0); - _p_new(1, 9) = P(0, 9) * _tmp30 + P(1, 9) - P(10, 9) * _tmp29 - P(12, 9) * _tmp10 + - P(2, 9) * _tmp31 + P(3, 9) * _tmp9 + _tmp163 * dt + _tmp173 * state(3, 0); - _p_new(2, 9) = P(0, 9) * _tmp41 + P(1, 9) * _tmp6 - P(10, 9) * _tmp11 + P(12, 9) * _tmp12 + - P(2, 9) + P(3, 9) * _tmp30 + _tmp164 * dt - _tmp173 * state(0, 0); - _p_new(3, 9) = P(0, 9) * _tmp31 + P(1, 9) * _tmp41 + P(10, 9) * _tmp10 - P(11, 9) * _tmp12 - - P(12, 9) * _tmp29 + P(2, 9) * _tmp3 + P(3, 9) + _tmp165 * dt; - _p_new(4, 9) = P(0, 9) * _tmp80 + P(1, 9) * _tmp83 - P(13, 9) * _tmp61 - P(14, 9) * _tmp73 - - P(15, 9) * _tmp68 + P(2, 9) * _tmp91 + P(3, 9) * _tmp88 + P(4, 9) + _tmp166 * dt; - _p_new(5, 9) = P(0, 9) * _tmp123 + P(1, 9) * _tmp126 - P(13, 9) * _tmp119 - P(14, 9) * _tmp117 - - P(15, 9) * _tmp132 + P(2, 9) * _tmp121 + P(3, 9) * _tmp128 + P(5, 9) + - _tmp167 * dt; - _p_new(6, 9) = P(0, 9) * _tmp154 + P(1, 9) * _tmp157 - P(13, 9) * _tmp153 - P(14, 9) * _tmp160 - - P(15, 9) * _tmp151 + P(2, 9) * _tmp158 + P(3, 9) * _tmp155 + P(6, 9) + - _tmp171 * dt; - _p_new(7, 9) = P(4, 9) * dt + P(7, 9) + dt * (P(4, 6) * dt + P(7, 6)); - _p_new(8, 9) = P(5, 9) * dt + P(8, 9) + dt * (P(5, 6) * dt + P(8, 6)); - _p_new(9, 9) = P(6, 9) * dt + P(9, 9) + dt * (P(6, 6) * dt + P(9, 6)); - _p_new(10, 9) = 0; - _p_new(11, 9) = 0; - _p_new(12, 9) = 0; - _p_new(13, 9) = 0; - _p_new(14, 9) = 0; - _p_new(15, 9) = 0; - _p_new(16, 9) = 0; - _p_new(17, 9) = 0; - _p_new(18, 9) = 0; - _p_new(19, 9) = 0; - _p_new(20, 9) = 0; - _p_new(21, 9) = 0; - _p_new(22, 9) = 0; - _p_new(23, 9) = 0; - _p_new(0, 10) = _tmp15; - _p_new(1, 10) = _tmp32; - _p_new(2, 10) = _tmp43; - _p_new(3, 10) = _tmp50; - _p_new(4, 10) = P(0, 10) * _tmp80 + P(1, 10) * _tmp83 - P(13, 10) * _tmp61 - - P(14, 10) * _tmp73 - P(15, 10) * _tmp68 + P(2, 10) * _tmp91 + - P(3, 10) * _tmp88 + P(4, 10); - _p_new(5, 10) = P(0, 10) * _tmp123 + P(1, 10) * _tmp126 - P(13, 10) * _tmp119 - - P(14, 10) * _tmp117 - P(15, 10) * _tmp132 + P(2, 10) * _tmp121 + - P(3, 10) * _tmp128 + P(5, 10); - _p_new(6, 10) = P(0, 10) * _tmp154 + P(1, 10) * _tmp157 - P(13, 10) * _tmp153 - - P(14, 10) * _tmp160 - P(15, 10) * _tmp151 + P(2, 10) * _tmp158 + - P(3, 10) * _tmp155 + P(6, 10); - _p_new(7, 10) = P(4, 10) * dt + P(7, 10); - _p_new(8, 10) = P(5, 10) * dt + P(8, 10); - _p_new(9, 10) = P(6, 10) * dt + P(9, 10); - _p_new(10, 10) = P(10, 10); - _p_new(11, 10) = 0; - _p_new(12, 10) = 0; - _p_new(13, 10) = 0; - _p_new(14, 10) = 0; - _p_new(15, 10) = 0; - _p_new(16, 10) = 0; - _p_new(17, 10) = 0; - _p_new(18, 10) = 0; - _p_new(19, 10) = 0; - _p_new(20, 10) = 0; - _p_new(21, 10) = 0; - _p_new(22, 10) = 0; - _p_new(23, 10) = 0; - _p_new(0, 11) = _tmp14; - _p_new(1, 11) = _tmp33; - _p_new(2, 11) = _tmp42; - _p_new(3, 11) = _tmp51; - _p_new(4, 11) = P(0, 11) * _tmp80 + P(1, 11) * _tmp83 - P(13, 11) * _tmp61 - - P(14, 11) * _tmp73 - P(15, 11) * _tmp68 + P(2, 11) * _tmp91 + - P(3, 11) * _tmp88 + P(4, 11); - _p_new(5, 11) = P(0, 11) * _tmp123 + P(1, 11) * _tmp126 - P(13, 11) * _tmp119 - - P(14, 11) * _tmp117 - P(15, 11) * _tmp132 + P(2, 11) * _tmp121 + - P(3, 11) * _tmp128 + P(5, 11); - _p_new(6, 11) = P(0, 11) * _tmp154 + P(1, 11) * _tmp157 - P(13, 11) * _tmp153 - - P(14, 11) * _tmp160 - P(15, 11) * _tmp151 + P(2, 11) * _tmp158 + - P(3, 11) * _tmp155 + P(6, 11); - _p_new(7, 11) = P(4, 11) * dt + P(7, 11); - _p_new(8, 11) = P(5, 11) * dt + P(8, 11); - _p_new(9, 11) = P(6, 11) * dt + P(9, 11); - _p_new(10, 11) = P(10, 11); - _p_new(11, 11) = P(11, 11); - _p_new(12, 11) = 0; - _p_new(13, 11) = 0; - _p_new(14, 11) = 0; - _p_new(15, 11) = 0; - _p_new(16, 11) = 0; - _p_new(17, 11) = 0; - _p_new(18, 11) = 0; - _p_new(19, 11) = 0; - _p_new(20, 11) = 0; - _p_new(21, 11) = 0; - _p_new(22, 11) = 0; - _p_new(23, 11) = 0; - _p_new(0, 12) = _tmp13; - _p_new(1, 12) = _tmp34; - _p_new(2, 12) = _tmp44; - _p_new(3, 12) = _tmp52; - _p_new(4, 12) = P(0, 12) * _tmp80 + P(1, 12) * _tmp83 - P(13, 12) * _tmp61 - - P(14, 12) * _tmp73 - P(15, 12) * _tmp68 + P(2, 12) * _tmp91 + - P(3, 12) * _tmp88 + P(4, 12); - _p_new(5, 12) = P(0, 12) * _tmp123 + P(1, 12) * _tmp126 - P(13, 12) * _tmp119 - - P(14, 12) * _tmp117 - P(15, 12) * _tmp132 + P(2, 12) * _tmp121 + - P(3, 12) * _tmp128 + P(5, 12); - _p_new(6, 12) = P(0, 12) * _tmp154 + P(1, 12) * _tmp157 - P(13, 12) * _tmp153 - - P(14, 12) * _tmp160 - P(15, 12) * _tmp151 + P(2, 12) * _tmp158 + - P(3, 12) * _tmp155 + P(6, 12); - _p_new(7, 12) = P(4, 12) * dt + P(7, 12); - _p_new(8, 12) = P(5, 12) * dt + P(8, 12); - _p_new(9, 12) = P(6, 12) * dt + P(9, 12); - _p_new(10, 12) = P(10, 12); - _p_new(11, 12) = P(11, 12); - _p_new(12, 12) = P(12, 12); - _p_new(13, 12) = 0; - _p_new(14, 12) = 0; - _p_new(15, 12) = 0; - _p_new(16, 12) = 0; - _p_new(17, 12) = 0; - _p_new(18, 12) = 0; - _p_new(19, 12) = 0; - _p_new(20, 12) = 0; - _p_new(21, 12) = 0; - _p_new(22, 12) = 0; - _p_new(23, 12) = 0; - _p_new(0, 13) = _tmp57; - _p_new(1, 13) = _tmp93; - _p_new(2, 13) = _tmp100; - _p_new(3, 13) = _tmp105; - _p_new(4, 13) = _tmp109; - _p_new(5, 13) = _tmp147; - _p_new(6, 13) = _tmp168; - _p_new(7, 13) = P(4, 13) * dt + P(7, 13); - _p_new(8, 13) = P(5, 13) * dt + P(8, 13); - _p_new(9, 13) = P(6, 13) * dt + P(9, 13); - _p_new(10, 13) = P(10, 13); - _p_new(11, 13) = P(11, 13); - _p_new(12, 13) = P(12, 13); - _p_new(13, 13) = P(13, 13); - _p_new(14, 13) = 0; - _p_new(15, 13) = 0; - _p_new(16, 13) = 0; - _p_new(17, 13) = 0; - _p_new(18, 13) = 0; - _p_new(19, 13) = 0; - _p_new(20, 13) = 0; - _p_new(21, 13) = 0; - _p_new(22, 13) = 0; - _p_new(23, 13) = 0; - _p_new(0, 14) = _tmp69; - _p_new(1, 14) = _tmp95; - _p_new(2, 14) = _tmp98; - _p_new(3, 14) = _tmp104; - _p_new(4, 14) = _tmp111; - _p_new(5, 14) = _tmp143; - _p_new(6, 14) = _tmp170; - _p_new(7, 14) = P(4, 14) * dt + P(7, 14); - _p_new(8, 14) = P(5, 14) * dt + P(8, 14); - _p_new(9, 14) = P(6, 14) * dt + P(9, 14); - _p_new(10, 14) = P(10, 14); - _p_new(11, 14) = P(11, 14); - _p_new(12, 14) = P(12, 14); - _p_new(13, 14) = P(13, 14); - _p_new(14, 14) = P(14, 14); - _p_new(15, 14) = 0; - _p_new(16, 14) = 0; - _p_new(17, 14) = 0; - _p_new(18, 14) = 0; - _p_new(19, 14) = 0; - _p_new(20, 14) = 0; - _p_new(21, 14) = 0; - _p_new(22, 14) = 0; - _p_new(23, 14) = 0; - _p_new(0, 15) = _tmp62; - _p_new(1, 15) = _tmp94; - _p_new(2, 15) = _tmp97; - _p_new(3, 15) = _tmp103; - _p_new(4, 15) = _tmp110; - _p_new(5, 15) = _tmp144; - _p_new(6, 15) = _tmp169; - _p_new(7, 15) = P(4, 15) * dt + P(7, 15); - _p_new(8, 15) = P(5, 15) * dt + P(8, 15); - _p_new(9, 15) = P(6, 15) * dt + P(9, 15); - _p_new(10, 15) = P(10, 15); - _p_new(11, 15) = P(11, 15); - _p_new(12, 15) = P(12, 15); - _p_new(13, 15) = P(13, 15); - _p_new(14, 15) = P(14, 15); - _p_new(15, 15) = P(15, 15); - _p_new(16, 15) = 0; - _p_new(17, 15) = 0; - _p_new(18, 15) = 0; - _p_new(19, 15) = 0; - _p_new(20, 15) = 0; - _p_new(21, 15) = 0; - _p_new(22, 15) = 0; - _p_new(23, 15) = 0; - _p_new(0, 16) = P(0, 16) + P(1, 16) * _tmp3 + P(10, 16) * _tmp12 + P(11, 16) * _tmp10 + - P(12, 16) * _tmp11 + P(2, 16) * _tmp9 + P(3, 16) * _tmp6; - _p_new(1, 16) = P(0, 16) * _tmp30 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 - - P(12, 16) * _tmp10 + P(2, 16) * _tmp31 + P(3, 16) * _tmp9; - _p_new(2, 16) = P(0, 16) * _tmp41 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 + - P(12, 16) * _tmp12 + P(2, 16) + P(3, 16) * _tmp30; - _p_new(3, 16) = P(0, 16) * _tmp31 + P(1, 16) * _tmp41 + P(10, 16) * _tmp10 - - P(11, 16) * _tmp12 - P(12, 16) * _tmp29 + P(2, 16) * _tmp3 + P(3, 16); - _p_new(4, 16) = P(0, 16) * _tmp80 + P(1, 16) * _tmp83 - P(13, 16) * _tmp61 - - P(14, 16) * _tmp73 - P(15, 16) * _tmp68 + P(2, 16) * _tmp91 + - P(3, 16) * _tmp88 + P(4, 16); - _p_new(5, 16) = P(0, 16) * _tmp123 + P(1, 16) * _tmp126 - P(13, 16) * _tmp119 - - P(14, 16) * _tmp117 - P(15, 16) * _tmp132 + P(2, 16) * _tmp121 + - P(3, 16) * _tmp128 + P(5, 16); - _p_new(6, 16) = P(0, 16) * _tmp154 + P(1, 16) * _tmp157 - P(13, 16) * _tmp153 - - P(14, 16) * _tmp160 - P(15, 16) * _tmp151 + P(2, 16) * _tmp158 + - P(3, 16) * _tmp155 + P(6, 16); - _p_new(7, 16) = P(4, 16) * dt + P(7, 16); - _p_new(8, 16) = P(5, 16) * dt + P(8, 16); - _p_new(9, 16) = P(6, 16) * dt + P(9, 16); - _p_new(10, 16) = P(10, 16); - _p_new(11, 16) = P(11, 16); - _p_new(12, 16) = P(12, 16); - _p_new(13, 16) = P(13, 16); - _p_new(14, 16) = P(14, 16); - _p_new(15, 16) = P(15, 16); - _p_new(16, 16) = P(16, 16); - _p_new(17, 16) = 0; - _p_new(18, 16) = 0; - _p_new(19, 16) = 0; - _p_new(20, 16) = 0; - _p_new(21, 16) = 0; - _p_new(22, 16) = 0; - _p_new(23, 16) = 0; - _p_new(0, 17) = P(0, 17) + P(1, 17) * _tmp3 + P(10, 17) * _tmp12 + P(11, 17) * _tmp10 + - P(12, 17) * _tmp11 + P(2, 17) * _tmp9 + P(3, 17) * _tmp6; - _p_new(1, 17) = P(0, 17) * _tmp30 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 - - P(12, 17) * _tmp10 + P(2, 17) * _tmp31 + P(3, 17) * _tmp9; - _p_new(2, 17) = P(0, 17) * _tmp41 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 + - P(12, 17) * _tmp12 + P(2, 17) + P(3, 17) * _tmp30; - _p_new(3, 17) = P(0, 17) * _tmp31 + P(1, 17) * _tmp41 + P(10, 17) * _tmp10 - - P(11, 17) * _tmp12 - P(12, 17) * _tmp29 + P(2, 17) * _tmp3 + P(3, 17); - _p_new(4, 17) = P(0, 17) * _tmp80 + P(1, 17) * _tmp83 - P(13, 17) * _tmp61 - - P(14, 17) * _tmp73 - P(15, 17) * _tmp68 + P(2, 17) * _tmp91 + - P(3, 17) * _tmp88 + P(4, 17); - _p_new(5, 17) = P(0, 17) * _tmp123 + P(1, 17) * _tmp126 - P(13, 17) * _tmp119 - - P(14, 17) * _tmp117 - P(15, 17) * _tmp132 + P(2, 17) * _tmp121 + - P(3, 17) * _tmp128 + P(5, 17); - _p_new(6, 17) = P(0, 17) * _tmp154 + P(1, 17) * _tmp157 - P(13, 17) * _tmp153 - - P(14, 17) * _tmp160 - P(15, 17) * _tmp151 + P(2, 17) * _tmp158 + - P(3, 17) * _tmp155 + P(6, 17); - _p_new(7, 17) = P(4, 17) * dt + P(7, 17); - _p_new(8, 17) = P(5, 17) * dt + P(8, 17); - _p_new(9, 17) = P(6, 17) * dt + P(9, 17); - _p_new(10, 17) = P(10, 17); - _p_new(11, 17) = P(11, 17); - _p_new(12, 17) = P(12, 17); - _p_new(13, 17) = P(13, 17); - _p_new(14, 17) = P(14, 17); - _p_new(15, 17) = P(15, 17); - _p_new(16, 17) = P(16, 17); - _p_new(17, 17) = P(17, 17); - _p_new(18, 17) = 0; - _p_new(19, 17) = 0; - _p_new(20, 17) = 0; - _p_new(21, 17) = 0; - _p_new(22, 17) = 0; - _p_new(23, 17) = 0; - _p_new(0, 18) = P(0, 18) + P(1, 18) * _tmp3 + P(10, 18) * _tmp12 + P(11, 18) * _tmp10 + - P(12, 18) * _tmp11 + P(2, 18) * _tmp9 + P(3, 18) * _tmp6; - _p_new(1, 18) = P(0, 18) * _tmp30 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 - - P(12, 18) * _tmp10 + P(2, 18) * _tmp31 + P(3, 18) * _tmp9; - _p_new(2, 18) = P(0, 18) * _tmp41 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 + - P(12, 18) * _tmp12 + P(2, 18) + P(3, 18) * _tmp30; - _p_new(3, 18) = P(0, 18) * _tmp31 + P(1, 18) * _tmp41 + P(10, 18) * _tmp10 - - P(11, 18) * _tmp12 - P(12, 18) * _tmp29 + P(2, 18) * _tmp3 + P(3, 18); - _p_new(4, 18) = P(0, 18) * _tmp80 + P(1, 18) * _tmp83 - P(13, 18) * _tmp61 - - P(14, 18) * _tmp73 - P(15, 18) * _tmp68 + P(2, 18) * _tmp91 + - P(3, 18) * _tmp88 + P(4, 18); - _p_new(5, 18) = P(0, 18) * _tmp123 + P(1, 18) * _tmp126 - P(13, 18) * _tmp119 - - P(14, 18) * _tmp117 - P(15, 18) * _tmp132 + P(2, 18) * _tmp121 + - P(3, 18) * _tmp128 + P(5, 18); - _p_new(6, 18) = P(0, 18) * _tmp154 + P(1, 18) * _tmp157 - P(13, 18) * _tmp153 - - P(14, 18) * _tmp160 - P(15, 18) * _tmp151 + P(2, 18) * _tmp158 + - P(3, 18) * _tmp155 + P(6, 18); - _p_new(7, 18) = P(4, 18) * dt + P(7, 18); - _p_new(8, 18) = P(5, 18) * dt + P(8, 18); - _p_new(9, 18) = P(6, 18) * dt + P(9, 18); - _p_new(10, 18) = P(10, 18); - _p_new(11, 18) = P(11, 18); - _p_new(12, 18) = P(12, 18); - _p_new(13, 18) = P(13, 18); - _p_new(14, 18) = P(14, 18); - _p_new(15, 18) = P(15, 18); - _p_new(16, 18) = P(16, 18); - _p_new(17, 18) = P(17, 18); - _p_new(18, 18) = P(18, 18); - _p_new(19, 18) = 0; - _p_new(20, 18) = 0; - _p_new(21, 18) = 0; - _p_new(22, 18) = 0; - _p_new(23, 18) = 0; - _p_new(0, 19) = P(0, 19) + P(1, 19) * _tmp3 + P(10, 19) * _tmp12 + P(11, 19) * _tmp10 + - P(12, 19) * _tmp11 + P(2, 19) * _tmp9 + P(3, 19) * _tmp6; - _p_new(1, 19) = P(0, 19) * _tmp30 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 - - P(12, 19) * _tmp10 + P(2, 19) * _tmp31 + P(3, 19) * _tmp9; - _p_new(2, 19) = P(0, 19) * _tmp41 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 + - P(12, 19) * _tmp12 + P(2, 19) + P(3, 19) * _tmp30; - _p_new(3, 19) = P(0, 19) * _tmp31 + P(1, 19) * _tmp41 + P(10, 19) * _tmp10 - - P(11, 19) * _tmp12 - P(12, 19) * _tmp29 + P(2, 19) * _tmp3 + P(3, 19); - _p_new(4, 19) = P(0, 19) * _tmp80 + P(1, 19) * _tmp83 - P(13, 19) * _tmp61 - - P(14, 19) * _tmp73 - P(15, 19) * _tmp68 + P(2, 19) * _tmp91 + - P(3, 19) * _tmp88 + P(4, 19); - _p_new(5, 19) = P(0, 19) * _tmp123 + P(1, 19) * _tmp126 - P(13, 19) * _tmp119 - - P(14, 19) * _tmp117 - P(15, 19) * _tmp132 + P(2, 19) * _tmp121 + - P(3, 19) * _tmp128 + P(5, 19); - _p_new(6, 19) = P(0, 19) * _tmp154 + P(1, 19) * _tmp157 - P(13, 19) * _tmp153 - - P(14, 19) * _tmp160 - P(15, 19) * _tmp151 + P(2, 19) * _tmp158 + - P(3, 19) * _tmp155 + P(6, 19); - _p_new(7, 19) = P(4, 19) * dt + P(7, 19); - _p_new(8, 19) = P(5, 19) * dt + P(8, 19); - _p_new(9, 19) = P(6, 19) * dt + P(9, 19); - _p_new(10, 19) = P(10, 19); - _p_new(11, 19) = P(11, 19); - _p_new(12, 19) = P(12, 19); - _p_new(13, 19) = P(13, 19); - _p_new(14, 19) = P(14, 19); - _p_new(15, 19) = P(15, 19); - _p_new(16, 19) = P(16, 19); - _p_new(17, 19) = P(17, 19); - _p_new(18, 19) = P(18, 19); - _p_new(19, 19) = P(19, 19); - _p_new(20, 19) = 0; - _p_new(21, 19) = 0; - _p_new(22, 19) = 0; - _p_new(23, 19) = 0; - _p_new(0, 20) = P(0, 20) + P(1, 20) * _tmp3 + P(10, 20) * _tmp12 + P(11, 20) * _tmp10 + - P(12, 20) * _tmp11 + P(2, 20) * _tmp9 + P(3, 20) * _tmp6; - _p_new(1, 20) = P(0, 20) * _tmp30 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 - - P(12, 20) * _tmp10 + P(2, 20) * _tmp31 + P(3, 20) * _tmp9; - _p_new(2, 20) = P(0, 20) * _tmp41 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 + - P(12, 20) * _tmp12 + P(2, 20) + P(3, 20) * _tmp30; - _p_new(3, 20) = P(0, 20) * _tmp31 + P(1, 20) * _tmp41 + P(10, 20) * _tmp10 - - P(11, 20) * _tmp12 - P(12, 20) * _tmp29 + P(2, 20) * _tmp3 + P(3, 20); - _p_new(4, 20) = P(0, 20) * _tmp80 + P(1, 20) * _tmp83 - P(13, 20) * _tmp61 - - P(14, 20) * _tmp73 - P(15, 20) * _tmp68 + P(2, 20) * _tmp91 + - P(3, 20) * _tmp88 + P(4, 20); - _p_new(5, 20) = P(0, 20) * _tmp123 + P(1, 20) * _tmp126 - P(13, 20) * _tmp119 - - P(14, 20) * _tmp117 - P(15, 20) * _tmp132 + P(2, 20) * _tmp121 + - P(3, 20) * _tmp128 + P(5, 20); - _p_new(6, 20) = P(0, 20) * _tmp154 + P(1, 20) * _tmp157 - P(13, 20) * _tmp153 - - P(14, 20) * _tmp160 - P(15, 20) * _tmp151 + P(2, 20) * _tmp158 + - P(3, 20) * _tmp155 + P(6, 20); - _p_new(7, 20) = P(4, 20) * dt + P(7, 20); - _p_new(8, 20) = P(5, 20) * dt + P(8, 20); - _p_new(9, 20) = P(6, 20) * dt + P(9, 20); - _p_new(10, 20) = P(10, 20); - _p_new(11, 20) = P(11, 20); - _p_new(12, 20) = P(12, 20); - _p_new(13, 20) = P(13, 20); - _p_new(14, 20) = P(14, 20); - _p_new(15, 20) = P(15, 20); - _p_new(16, 20) = P(16, 20); - _p_new(17, 20) = P(17, 20); - _p_new(18, 20) = P(18, 20); - _p_new(19, 20) = P(19, 20); - _p_new(20, 20) = P(20, 20); - _p_new(21, 20) = 0; - _p_new(22, 20) = 0; - _p_new(23, 20) = 0; - _p_new(0, 21) = P(0, 21) + P(1, 21) * _tmp3 + P(10, 21) * _tmp12 + P(11, 21) * _tmp10 + - P(12, 21) * _tmp11 + P(2, 21) * _tmp9 + P(3, 21) * _tmp6; - _p_new(1, 21) = P(0, 21) * _tmp30 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 - - P(12, 21) * _tmp10 + P(2, 21) * _tmp31 + P(3, 21) * _tmp9; - _p_new(2, 21) = P(0, 21) * _tmp41 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 + - P(12, 21) * _tmp12 + P(2, 21) + P(3, 21) * _tmp30; - _p_new(3, 21) = P(0, 21) * _tmp31 + P(1, 21) * _tmp41 + P(10, 21) * _tmp10 - - P(11, 21) * _tmp12 - P(12, 21) * _tmp29 + P(2, 21) * _tmp3 + P(3, 21); - _p_new(4, 21) = P(0, 21) * _tmp80 + P(1, 21) * _tmp83 - P(13, 21) * _tmp61 - - P(14, 21) * _tmp73 - P(15, 21) * _tmp68 + P(2, 21) * _tmp91 + - P(3, 21) * _tmp88 + P(4, 21); - _p_new(5, 21) = P(0, 21) * _tmp123 + P(1, 21) * _tmp126 - P(13, 21) * _tmp119 - - P(14, 21) * _tmp117 - P(15, 21) * _tmp132 + P(2, 21) * _tmp121 + - P(3, 21) * _tmp128 + P(5, 21); - _p_new(6, 21) = P(0, 21) * _tmp154 + P(1, 21) * _tmp157 - P(13, 21) * _tmp153 - - P(14, 21) * _tmp160 - P(15, 21) * _tmp151 + P(2, 21) * _tmp158 + - P(3, 21) * _tmp155 + P(6, 21); - _p_new(7, 21) = P(4, 21) * dt + P(7, 21); - _p_new(8, 21) = P(5, 21) * dt + P(8, 21); - _p_new(9, 21) = P(6, 21) * dt + P(9, 21); - _p_new(10, 21) = P(10, 21); - _p_new(11, 21) = P(11, 21); - _p_new(12, 21) = P(12, 21); - _p_new(13, 21) = P(13, 21); - _p_new(14, 21) = P(14, 21); - _p_new(15, 21) = P(15, 21); - _p_new(16, 21) = P(16, 21); - _p_new(17, 21) = P(17, 21); - _p_new(18, 21) = P(18, 21); - _p_new(19, 21) = P(19, 21); - _p_new(20, 21) = P(20, 21); - _p_new(21, 21) = P(21, 21); - _p_new(22, 21) = 0; - _p_new(23, 21) = 0; - _p_new(0, 22) = P(0, 22) + P(1, 22) * _tmp3 + P(10, 22) * _tmp12 + P(11, 22) * _tmp10 + - P(12, 22) * _tmp11 + P(2, 22) * _tmp9 + P(3, 22) * _tmp6; - _p_new(1, 22) = P(0, 22) * _tmp30 + P(1, 22) - P(10, 22) * _tmp29 + P(11, 22) * _tmp11 - - P(12, 22) * _tmp10 + P(2, 22) * _tmp31 + P(3, 22) * _tmp9; - _p_new(2, 22) = P(0, 22) * _tmp41 + P(1, 22) * _tmp6 - P(10, 22) * _tmp11 - P(11, 22) * _tmp29 + - P(12, 22) * _tmp12 + P(2, 22) + P(3, 22) * _tmp30; - _p_new(3, 22) = P(0, 22) * _tmp31 + P(1, 22) * _tmp41 + P(10, 22) * _tmp10 - - P(11, 22) * _tmp12 - P(12, 22) * _tmp29 + P(2, 22) * _tmp3 + P(3, 22); - _p_new(4, 22) = P(0, 22) * _tmp80 + P(1, 22) * _tmp83 - P(13, 22) * _tmp61 - - P(14, 22) * _tmp73 - P(15, 22) * _tmp68 + P(2, 22) * _tmp91 + - P(3, 22) * _tmp88 + P(4, 22); - _p_new(5, 22) = P(0, 22) * _tmp123 + P(1, 22) * _tmp126 - P(13, 22) * _tmp119 - - P(14, 22) * _tmp117 - P(15, 22) * _tmp132 + P(2, 22) * _tmp121 + - P(3, 22) * _tmp128 + P(5, 22); - _p_new(6, 22) = P(0, 22) * _tmp154 + P(1, 22) * _tmp157 - P(13, 22) * _tmp153 - - P(14, 22) * _tmp160 - P(15, 22) * _tmp151 + P(2, 22) * _tmp158 + - P(3, 22) * _tmp155 + P(6, 22); - _p_new(7, 22) = P(4, 22) * dt + P(7, 22); - _p_new(8, 22) = P(5, 22) * dt + P(8, 22); - _p_new(9, 22) = P(6, 22) * dt + P(9, 22); - _p_new(10, 22) = P(10, 22); - _p_new(11, 22) = P(11, 22); - _p_new(12, 22) = P(12, 22); - _p_new(13, 22) = P(13, 22); - _p_new(14, 22) = P(14, 22); - _p_new(15, 22) = P(15, 22); - _p_new(16, 22) = P(16, 22); - _p_new(17, 22) = P(17, 22); - _p_new(18, 22) = P(18, 22); - _p_new(19, 22) = P(19, 22); - _p_new(20, 22) = P(20, 22); - _p_new(21, 22) = P(21, 22); - _p_new(22, 22) = P(22, 22); - _p_new(23, 22) = 0; - _p_new(0, 23) = P(0, 23) + P(1, 23) * _tmp3 + P(10, 23) * _tmp12 + P(11, 23) * _tmp10 + - P(12, 23) * _tmp11 + P(2, 23) * _tmp9 + P(3, 23) * _tmp6; - _p_new(1, 23) = P(0, 23) * _tmp30 + P(1, 23) - P(10, 23) * _tmp29 + P(11, 23) * _tmp11 - - P(12, 23) * _tmp10 + P(2, 23) * _tmp31 + P(3, 23) * _tmp9; - _p_new(2, 23) = P(0, 23) * _tmp41 + P(1, 23) * _tmp6 - P(10, 23) * _tmp11 - P(11, 23) * _tmp29 + - P(12, 23) * _tmp12 + P(2, 23) + P(3, 23) * _tmp30; - _p_new(3, 23) = P(0, 23) * _tmp31 + P(1, 23) * _tmp41 + P(10, 23) * _tmp10 - - P(11, 23) * _tmp12 - P(12, 23) * _tmp29 + P(2, 23) * _tmp3 + P(3, 23); - _p_new(4, 23) = P(0, 23) * _tmp80 + P(1, 23) * _tmp83 - P(13, 23) * _tmp61 - - P(14, 23) * _tmp73 - P(15, 23) * _tmp68 + P(2, 23) * _tmp91 + - P(3, 23) * _tmp88 + P(4, 23); - _p_new(5, 23) = P(0, 23) * _tmp123 + P(1, 23) * _tmp126 - P(13, 23) * _tmp119 - - P(14, 23) * _tmp117 - P(15, 23) * _tmp132 + P(2, 23) * _tmp121 + - P(3, 23) * _tmp128 + P(5, 23); - _p_new(6, 23) = P(0, 23) * _tmp154 + P(1, 23) * _tmp157 - P(13, 23) * _tmp153 - - P(14, 23) * _tmp160 - P(15, 23) * _tmp151 + P(2, 23) * _tmp158 + - P(3, 23) * _tmp155 + P(6, 23); - _p_new(7, 23) = P(4, 23) * dt + P(7, 23); - _p_new(8, 23) = P(5, 23) * dt + P(8, 23); - _p_new(9, 23) = P(6, 23) * dt + P(9, 23); - _p_new(10, 23) = P(10, 23); - _p_new(11, 23) = P(11, 23); - _p_new(12, 23) = P(12, 23); - _p_new(13, 23) = P(13, 23); - _p_new(14, 23) = P(14, 23); - _p_new(15, 23) = P(15, 23); - _p_new(16, 23) = P(16, 23); - _p_new(17, 23) = P(17, 23); - _p_new(18, 23) = P(18, 23); - _p_new(19, 23) = P(19, 23); - _p_new(20, 23) = P(20, 23); - _p_new(21, 23) = P(21, 23); - _p_new(22, 23) = P(22, 23); - _p_new(23, 23) = P(23, 23); - } + _res(0, 0) = _tmp10 * _tmp13 + _tmp11 * _tmp15 + _tmp12 * _tmp14 + _tmp16 * _tmp3 + + _tmp17 * _tmp6 + _tmp19 * _tmp9 + _tmp22 + _tmp23 + _tmp28; + _res(1, 0) = 0; + _res(2, 0) = 0; + _res(3, 0) = 0; + _res(4, 0) = 0; + _res(5, 0) = 0; + _res(6, 0) = 0; + _res(7, 0) = 0; + _res(8, 0) = 0; + _res(9, 0) = 0; + _res(10, 0) = 0; + _res(11, 0) = 0; + _res(12, 0) = 0; + _res(13, 0) = 0; + _res(14, 0) = 0; + _res(15, 0) = 0; + _res(16, 0) = 0; + _res(17, 0) = 0; + _res(18, 0) = 0; + _res(19, 0) = 0; + _res(20, 0) = 0; + _res(21, 0) = 0; + _res(22, 0) = 0; + _res(23, 0) = 0; + _res(0, 1) = _tmp11 * _tmp14 - _tmp12 * _tmp15 - _tmp13 * _tmp29 + _tmp16 * _tmp32 + + _tmp17 * _tmp3 + _tmp19 + _tmp23 * _tmp31 - _tmp30 * state(0, 0); + _res(1, 1) = _tmp11 * _tmp35 - _tmp12 * _tmp33 + _tmp25 - _tmp29 * _tmp34 + _tmp3 * _tmp37 + + _tmp31 * _tmp38 + _tmp32 * _tmp36 + _tmp39 + _tmp41; + _res(2, 1) = 0; + _res(3, 1) = 0; + _res(4, 1) = 0; + _res(5, 1) = 0; + _res(6, 1) = 0; + _res(7, 1) = 0; + _res(8, 1) = 0; + _res(9, 1) = 0; + _res(10, 1) = 0; + _res(11, 1) = 0; + _res(12, 1) = 0; + _res(13, 1) = 0; + _res(14, 1) = 0; + _res(15, 1) = 0; + _res(16, 1) = 0; + _res(17, 1) = 0; + _res(18, 1) = 0; + _res(19, 1) = 0; + _res(20, 1) = 0; + _res(21, 1) = 0; + _res(22, 1) = 0; + _res(23, 1) = 0; + _res(0, 2) = _tmp10 * _tmp15 - _tmp11 * _tmp13 - _tmp14 * _tmp29 + _tmp16 + _tmp17 * _tmp31 + + _tmp19 * _tmp6 + _tmp23 * _tmp43 - _tmp42 * state(2, 0); + _res(1, 2) = _tmp10 * _tmp33 - _tmp11 * _tmp34 - _tmp29 * _tmp35 - _tmp30 * state(2, 0) + + _tmp31 * _tmp37 + _tmp36 + _tmp38 * _tmp43 + _tmp39 * _tmp6; + _res(2, 2) = _tmp10 * _tmp49 - _tmp11 * _tmp48 + _tmp28 - _tmp29 * _tmp47 + _tmp31 * _tmp46 + + _tmp40 + _tmp43 * _tmp44 + _tmp45 * _tmp6 + _tmp50; + _res(3, 2) = 0; + _res(4, 2) = 0; + _res(5, 2) = 0; + _res(6, 2) = 0; + _res(7, 2) = 0; + _res(8, 2) = 0; + _res(9, 2) = 0; + _res(10, 2) = 0; + _res(11, 2) = 0; + _res(12, 2) = 0; + _res(13, 2) = 0; + _res(14, 2) = 0; + _res(15, 2) = 0; + _res(16, 2) = 0; + _res(17, 2) = 0; + _res(18, 2) = 0; + _res(19, 2) = 0; + _res(20, 2) = 0; + _res(21, 2) = 0; + _res(22, 2) = 0; + _res(23, 2) = 0; + _res(0, 3) = -_tmp10 * _tmp14 + _tmp12 * _tmp13 - _tmp15 * _tmp29 + _tmp16 * _tmp9 + _tmp17 + + _tmp19 * _tmp43 + _tmp23 * _tmp32 - _tmp42 * state(3, 0); + _res(1, 3) = -_tmp10 * _tmp35 + _tmp12 * _tmp34 - _tmp29 * _tmp33 - _tmp30 * state(3, 0) + + _tmp32 * _tmp38 + _tmp36 * _tmp9 + _tmp37 + _tmp39 * _tmp43; + _res(2, 3) = -_tmp10 * _tmp47 + _tmp12 * _tmp48 - _tmp21 * state(2, 0) * state(3, 0) - + _tmp29 * _tmp49 + _tmp32 * _tmp44 + _tmp43 * _tmp45 + _tmp46 + _tmp50 * _tmp9; + _res(3, 3) = -_tmp10 * _tmp54 + _tmp12 * _tmp56 + _tmp27 - _tmp29 * _tmp55 + _tmp32 * _tmp53 + + _tmp41 + _tmp43 * _tmp52 + _tmp51 * _tmp9 + _tmp57; + _res(4, 3) = 0; + _res(5, 3) = 0; + _res(6, 3) = 0; + _res(7, 3) = 0; + _res(8, 3) = 0; + _res(9, 3) = 0; + _res(10, 3) = 0; + _res(11, 3) = 0; + _res(12, 3) = 0; + _res(13, 3) = 0; + _res(14, 3) = 0; + _res(15, 3) = 0; + _res(16, 3) = 0; + _res(17, 3) = 0; + _res(18, 3) = 0; + _res(19, 3) = 0; + _res(20, 3) = 0; + _res(21, 3) = 0; + _res(22, 3) = 0; + _res(23, 3) = 0; + _res(0, 4) = _tmp16 * _tmp90 + _tmp17 * _tmp93 + _tmp19 * _tmp85 + _tmp23 * _tmp64 - + _tmp65 * _tmp69 - _tmp70 * _tmp75 - _tmp76 * _tmp81 + _tmp95; + _res(1, 4) = _tmp36 * _tmp90 + _tmp37 * _tmp93 + _tmp38 * _tmp64 + _tmp39 * _tmp85 - + _tmp69 * _tmp96 - _tmp75 * _tmp98 - _tmp81 * _tmp97 + _tmp99; + _res(2, 4) = -_tmp100 * _tmp69 - _tmp101 * _tmp81 - _tmp102 * _tmp75 + _tmp103 + _tmp44 * _tmp64 + + _tmp45 * _tmp85 + _tmp46 * _tmp93 + _tmp50 * _tmp90; + _res(3, 4) = -_tmp104 * _tmp69 - _tmp105 * _tmp81 - _tmp106 * _tmp75 + _tmp107 + _tmp51 * _tmp90 + + _tmp52 * _tmp85 + _tmp53 * _tmp64 + _tmp57 * _tmp93; + _res(4, 4) = -_tmp108 * _tmp69 - _tmp109 * _tmp81 - _tmp110 * _tmp75 + _tmp111 * _tmp85 + + _tmp112 * _tmp64 + _tmp113 * _tmp93 + _tmp114 * _tmp90 + _tmp115 + + std::pow(_tmp68, Scalar(2)) * d_vel_var(0, 0) + + std::pow(_tmp74, Scalar(2)) * d_vel_var(2, 0) + + std::pow(_tmp80, Scalar(2)) * d_vel_var(1, 0); + _res(5, 4) = 0; + _res(6, 4) = 0; + _res(7, 4) = 0; + _res(8, 4) = 0; + _res(9, 4) = 0; + _res(10, 4) = 0; + _res(11, 4) = 0; + _res(12, 4) = 0; + _res(13, 4) = 0; + _res(14, 4) = 0; + _res(15, 4) = 0; + _res(16, 4) = 0; + _res(17, 4) = 0; + _res(18, 4) = 0; + _res(19, 4) = 0; + _res(20, 4) = 0; + _res(21, 4) = 0; + _res(22, 4) = 0; + _res(23, 4) = 0; + _res(0, 5) = -_tmp119 * _tmp70 - _tmp122 * _tmp76 - _tmp124 * _tmp65 + _tmp127 * _tmp19 + + _tmp129 * _tmp17 + _tmp131 * _tmp23 + _tmp133 * _tmp16 + _tmp134; + _res(1, 5) = -_tmp119 * _tmp98 - _tmp122 * _tmp97 - _tmp124 * _tmp96 + _tmp127 * _tmp39 + + _tmp129 * _tmp37 + _tmp131 * _tmp38 + _tmp133 * _tmp36 + _tmp135; + _res(2, 5) = -_tmp100 * _tmp124 - _tmp101 * _tmp122 - _tmp102 * _tmp119 + _tmp127 * _tmp45 + + _tmp129 * _tmp46 + _tmp131 * _tmp44 + _tmp133 * _tmp50 + _tmp136; + _res(3, 5) = -_tmp104 * _tmp124 - _tmp105 * _tmp122 - _tmp106 * _tmp119 + _tmp127 * _tmp52 + + _tmp129 * _tmp57 + _tmp131 * _tmp53 + _tmp133 * _tmp51 + _tmp137; + _res(4, 5) = -_tmp108 * _tmp124 - _tmp109 * _tmp122 - _tmp110 * _tmp119 + _tmp111 * _tmp127 + + _tmp112 * _tmp131 + _tmp113 * _tmp129 + _tmp114 * _tmp133 + + _tmp118 * _tmp74 * d_vel_var(2, 0) + _tmp121 * _tmp139 + _tmp138 * _tmp68 + _tmp140; + _res(5, 5) = std::pow(_tmp118, Scalar(2)) * d_vel_var(2, 0) - _tmp119 * _tmp143 + + std::pow(_tmp121, Scalar(2)) * d_vel_var(1, 0) - _tmp122 * _tmp142 + + std::pow(_tmp123, Scalar(2)) * d_vel_var(0, 0) - _tmp124 * _tmp141 + + _tmp127 * _tmp147 + _tmp129 * _tmp146 + _tmp131 * _tmp145 + _tmp133 * _tmp144 + + _tmp148; + _res(6, 5) = 0; + _res(7, 5) = 0; + _res(8, 5) = 0; + _res(9, 5) = 0; + _res(10, 5) = 0; + _res(11, 5) = 0; + _res(12, 5) = 0; + _res(13, 5) = 0; + _res(14, 5) = 0; + _res(15, 5) = 0; + _res(16, 5) = 0; + _res(17, 5) = 0; + _res(18, 5) = 0; + _res(19, 5) = 0; + _res(20, 5) = 0; + _res(21, 5) = 0; + _res(22, 5) = 0; + _res(23, 5) = 0; + _res(0, 6) = -_tmp150 * _tmp76 - _tmp152 * _tmp70 - _tmp154 * _tmp65 + _tmp156 * _tmp19 + + _tmp157 * _tmp16 + _tmp158 * _tmp23 + _tmp159 * _tmp17 + _tmp161; + _res(1, 6) = -_tmp150 * _tmp97 - _tmp152 * _tmp98 - _tmp154 * _tmp96 + _tmp156 * _tmp39 + + _tmp157 * _tmp36 + _tmp158 * _tmp38 + _tmp159 * _tmp37 + _tmp162; + _res(2, 6) = -_tmp100 * _tmp154 - _tmp101 * _tmp150 - _tmp102 * _tmp152 + _tmp156 * _tmp45 + + _tmp157 * _tmp50 + _tmp158 * _tmp44 + _tmp159 * _tmp46 + _tmp163; + _res(3, 6) = -_tmp104 * _tmp154 - _tmp105 * _tmp150 - _tmp106 * _tmp152 + _tmp156 * _tmp52 + + _tmp157 * _tmp51 + _tmp158 * _tmp53 + _tmp159 * _tmp57 + _tmp164; + _res(4, 6) = -_tmp108 * _tmp154 - _tmp109 * _tmp150 - _tmp110 * _tmp152 + _tmp111 * _tmp156 + + _tmp112 * _tmp158 + _tmp113 * _tmp159 + _tmp114 * _tmp157 + _tmp139 * _tmp149 + + _tmp153 * _tmp68 * d_vel_var(0, 0) + _tmp165 * _tmp74 + _tmp166; + _res(5, 6) = _tmp118 * _tmp165 + _tmp121 * _tmp149 * d_vel_var(1, 0) + _tmp138 * _tmp153 - + _tmp141 * _tmp154 - _tmp142 * _tmp150 - _tmp143 * _tmp152 + _tmp144 * _tmp157 + + _tmp145 * _tmp158 + _tmp146 * _tmp159 + _tmp147 * _tmp156 + _tmp167; + _res(6, 6) = + std::pow(_tmp149, Scalar(2)) * d_vel_var(1, 0) - _tmp150 * _tmp169 + + std::pow(_tmp151, Scalar(2)) * d_vel_var(2, 0) - _tmp152 * _tmp170 + + std::pow(_tmp153, Scalar(2)) * d_vel_var(0, 0) - _tmp154 * _tmp168 + + _tmp156 * (P(0, 1) * _tmp158 + P(1, 1) * _tmp156 - P(13, 1) * _tmp154 - P(14, 1) * _tmp150 - + P(15, 1) * _tmp152 + P(2, 1) * _tmp157 + P(3, 1) * _tmp159 + P(6, 1)) + + _tmp157 * (P(0, 2) * _tmp158 + P(1, 2) * _tmp156 - P(13, 2) * _tmp154 - P(14, 2) * _tmp150 - + P(15, 2) * _tmp152 + P(2, 2) * _tmp157 + P(3, 2) * _tmp159 + P(6, 2)) + + _tmp158 * (P(0, 0) * _tmp158 + P(1, 0) * _tmp156 - P(13, 0) * _tmp154 - P(14, 0) * _tmp150 - + P(15, 0) * _tmp152 + P(2, 0) * _tmp157 + P(3, 0) * _tmp159 + P(6, 0)) + + _tmp159 * (P(0, 3) * _tmp158 + P(1, 3) * _tmp156 - P(13, 3) * _tmp154 - P(14, 3) * _tmp150 - + P(15, 3) * _tmp152 + P(2, 3) * _tmp157 + P(3, 3) * _tmp159 + P(6, 3)) + + _tmp171; + _res(7, 6) = 0; + _res(8, 6) = 0; + _res(9, 6) = 0; + _res(10, 6) = 0; + _res(11, 6) = 0; + _res(12, 6) = 0; + _res(13, 6) = 0; + _res(14, 6) = 0; + _res(15, 6) = 0; + _res(16, 6) = 0; + _res(17, 6) = 0; + _res(18, 6) = 0; + _res(19, 6) = 0; + _res(20, 6) = 0; + _res(21, 6) = 0; + _res(22, 6) = 0; + _res(23, 6) = 0; + _res(0, 7) = P(0, 7) + P(1, 7) * _tmp9 + P(10, 7) * _tmp10 + P(11, 7) * _tmp12 + + P(12, 7) * _tmp11 + P(2, 7) * _tmp3 + P(3, 7) * _tmp6 + _tmp95 * d_vel_dt; + _res(1, 7) = P(0, 7) * _tmp31 + P(1, 7) - P(10, 7) * _tmp29 + P(11, 7) * _tmp11 - + P(12, 7) * _tmp12 + P(2, 7) * _tmp32 + P(3, 7) * _tmp3 + _tmp99 * d_vel_dt; + _res(2, 7) = P(0, 7) * _tmp43 + P(1, 7) * _tmp6 - P(10, 7) * _tmp11 - P(11, 7) * _tmp29 + + P(12, 7) * _tmp10 + P(2, 7) + P(3, 7) * _tmp31 + _tmp103 * d_vel_dt; + _res(3, 7) = P(0, 7) * _tmp32 + P(1, 7) * _tmp43 + P(10, 7) * _tmp12 - P(11, 7) * _tmp10 - + P(12, 7) * _tmp29 + P(2, 7) * _tmp9 + P(3, 7) + _tmp107 * d_vel_dt; + _res(4, 7) = P(0, 7) * _tmp64 + P(1, 7) * _tmp85 - P(13, 7) * _tmp69 - P(14, 7) * _tmp81 - + P(15, 7) * _tmp75 + P(2, 7) * _tmp90 + P(3, 7) * _tmp93 + P(4, 7) + + _tmp115 * d_vel_dt; + _res(5, 7) = + P(0, 7) * _tmp131 + P(1, 7) * _tmp127 - P(13, 7) * _tmp124 - P(14, 7) * _tmp122 - + P(15, 7) * _tmp119 + P(2, 7) * _tmp133 + P(3, 7) * _tmp129 + P(5, 7) + + d_vel_dt * (P(0, 4) * _tmp131 + P(1, 4) * _tmp127 - P(13, 4) * _tmp124 - P(14, 4) * _tmp122 - + P(15, 4) * _tmp119 + P(2, 4) * _tmp133 + P(3, 4) * _tmp129 + P(5, 4)); + _res(6, 7) = + P(0, 7) * _tmp158 + P(1, 7) * _tmp156 - P(13, 7) * _tmp154 - P(14, 7) * _tmp150 - + P(15, 7) * _tmp152 + P(2, 7) * _tmp157 + P(3, 7) * _tmp159 + P(6, 7) + + d_vel_dt * (P(0, 4) * _tmp158 + P(1, 4) * _tmp156 - P(13, 4) * _tmp154 - P(14, 4) * _tmp150 - + P(15, 4) * _tmp152 + P(2, 4) * _tmp157 + P(3, 4) * _tmp159 + P(6, 4)); + _res(7, 7) = P(4, 7) * d_vel_dt + P(7, 7) + d_vel_dt * (P(4, 4) * d_vel_dt + P(7, 4)); + _res(8, 7) = 0; + _res(9, 7) = 0; + _res(10, 7) = 0; + _res(11, 7) = 0; + _res(12, 7) = 0; + _res(13, 7) = 0; + _res(14, 7) = 0; + _res(15, 7) = 0; + _res(16, 7) = 0; + _res(17, 7) = 0; + _res(18, 7) = 0; + _res(19, 7) = 0; + _res(20, 7) = 0; + _res(21, 7) = 0; + _res(22, 7) = 0; + _res(23, 7) = 0; + _res(0, 8) = P(0, 8) + P(1, 8) * _tmp9 + P(10, 8) * _tmp10 + P(12, 8) * _tmp11 + P(2, 8) * _tmp3 + + P(3, 8) * _tmp6 + _tmp134 * d_vel_dt + _tmp172 * state(2, 0); + _res(1, 8) = P(0, 8) * _tmp31 + P(1, 8) - P(10, 8) * _tmp29 - P(12, 8) * _tmp12 + + P(2, 8) * _tmp32 + P(3, 8) * _tmp3 + _tmp135 * d_vel_dt + _tmp172 * state(3, 0); + _res(2, 8) = P(0, 8) * _tmp43 + P(1, 8) * _tmp6 - P(10, 8) * _tmp11 + P(12, 8) * _tmp10 + + P(2, 8) + P(3, 8) * _tmp31 + _tmp136 * d_vel_dt - _tmp172 * state(0, 0); + _res(3, 8) = P(0, 8) * _tmp32 + P(1, 8) * _tmp43 + P(10, 8) * _tmp12 - P(12, 8) * _tmp29 + + P(2, 8) * _tmp9 + P(3, 8) + _tmp137 * d_vel_dt - _tmp172 * state(1, 0); + _res(4, 8) = P(0, 8) * _tmp64 + P(1, 8) * _tmp85 - P(13, 8) * _tmp69 - P(14, 8) * _tmp81 - + P(15, 8) * _tmp75 + P(2, 8) * _tmp90 + P(3, 8) * _tmp93 + P(4, 8) + + _tmp140 * d_vel_dt; + _res(5, 8) = P(0, 8) * _tmp131 + P(1, 8) * _tmp127 - P(13, 8) * _tmp124 - P(14, 8) * _tmp122 - + P(15, 8) * _tmp119 + P(2, 8) * _tmp133 + P(3, 8) * _tmp129 + P(5, 8) + + _tmp148 * d_vel_dt; + _res(6, 8) = + P(0, 8) * _tmp158 + P(1, 8) * _tmp156 - P(13, 8) * _tmp154 - P(14, 8) * _tmp150 - + P(15, 8) * _tmp152 + P(2, 8) * _tmp157 + P(3, 8) * _tmp159 + P(6, 8) + + d_vel_dt * (P(0, 5) * _tmp158 + P(1, 5) * _tmp156 - P(13, 5) * _tmp154 - P(14, 5) * _tmp150 - + P(15, 5) * _tmp152 + P(2, 5) * _tmp157 + P(3, 5) * _tmp159 + P(6, 5)); + _res(7, 8) = P(4, 8) * d_vel_dt + P(7, 8) + d_vel_dt * (P(4, 5) * d_vel_dt + P(7, 5)); + _res(8, 8) = P(5, 8) * d_vel_dt + P(8, 8) + d_vel_dt * (P(5, 5) * d_vel_dt + P(8, 5)); + _res(9, 8) = 0; + _res(10, 8) = 0; + _res(11, 8) = 0; + _res(12, 8) = 0; + _res(13, 8) = 0; + _res(14, 8) = 0; + _res(15, 8) = 0; + _res(16, 8) = 0; + _res(17, 8) = 0; + _res(18, 8) = 0; + _res(19, 8) = 0; + _res(20, 8) = 0; + _res(21, 8) = 0; + _res(22, 8) = 0; + _res(23, 8) = 0; + _res(0, 9) = P(0, 9) + P(1, 9) * _tmp9 + P(10, 9) * _tmp10 + P(11, 9) * _tmp12 + + P(12, 9) * _tmp11 + P(2, 9) * _tmp3 + P(3, 9) * _tmp6 + _tmp161 * d_vel_dt; + _res(1, 9) = P(0, 9) * _tmp31 + P(1, 9) - P(10, 9) * _tmp29 + P(11, 9) * _tmp11 - + P(12, 9) * _tmp12 + P(2, 9) * _tmp32 + P(3, 9) * _tmp3 + _tmp162 * d_vel_dt; + _res(2, 9) = P(0, 9) * _tmp43 + P(1, 9) * _tmp6 - P(10, 9) * _tmp11 - P(11, 9) * _tmp29 + + P(12, 9) * _tmp10 + P(2, 9) + P(3, 9) * _tmp31 + _tmp163 * d_vel_dt; + _res(3, 9) = P(0, 9) * _tmp32 + P(1, 9) * _tmp43 + P(10, 9) * _tmp12 - P(11, 9) * _tmp10 - + P(12, 9) * _tmp29 + P(2, 9) * _tmp9 + P(3, 9) + _tmp164 * d_vel_dt; + _res(4, 9) = P(0, 9) * _tmp64 + P(1, 9) * _tmp85 - P(13, 9) * _tmp69 - P(14, 9) * _tmp81 - + P(15, 9) * _tmp75 + P(2, 9) * _tmp90 + P(3, 9) * _tmp93 + P(4, 9) + + _tmp166 * d_vel_dt; + _res(5, 9) = P(0, 9) * _tmp131 + P(1, 9) * _tmp127 - P(13, 9) * _tmp124 - P(14, 9) * _tmp122 - + P(15, 9) * _tmp119 + P(2, 9) * _tmp133 + P(3, 9) * _tmp129 + P(5, 9) + + _tmp167 * d_vel_dt; + _res(6, 9) = P(0, 9) * _tmp158 + P(1, 9) * _tmp156 - P(13, 9) * _tmp154 - P(14, 9) * _tmp150 - + P(15, 9) * _tmp152 + P(2, 9) * _tmp157 + P(3, 9) * _tmp159 + P(6, 9) + + _tmp171 * d_vel_dt; + _res(7, 9) = P(4, 9) * d_vel_dt + P(7, 9) + d_vel_dt * (P(4, 6) * d_vel_dt + P(7, 6)); + _res(8, 9) = P(5, 9) * d_vel_dt + P(8, 9) + d_vel_dt * (P(5, 6) * d_vel_dt + P(8, 6)); + _res(9, 9) = P(6, 9) * d_vel_dt + P(9, 9) + d_vel_dt * (P(6, 6) * d_vel_dt + P(9, 6)); + _res(10, 9) = 0; + _res(11, 9) = 0; + _res(12, 9) = 0; + _res(13, 9) = 0; + _res(14, 9) = 0; + _res(15, 9) = 0; + _res(16, 9) = 0; + _res(17, 9) = 0; + _res(18, 9) = 0; + _res(19, 9) = 0; + _res(20, 9) = 0; + _res(21, 9) = 0; + _res(22, 9) = 0; + _res(23, 9) = 0; + _res(0, 10) = _tmp13; + _res(1, 10) = _tmp34; + _res(2, 10) = _tmp48; + _res(3, 10) = _tmp56; + _res(4, 10) = P(0, 10) * _tmp64 + P(1, 10) * _tmp85 - P(13, 10) * _tmp69 - P(14, 10) * _tmp81 - + P(15, 10) * _tmp75 + P(2, 10) * _tmp90 + P(3, 10) * _tmp93 + P(4, 10); + _res(5, 10) = P(0, 10) * _tmp131 + P(1, 10) * _tmp127 - P(13, 10) * _tmp124 - + P(14, 10) * _tmp122 - P(15, 10) * _tmp119 + P(2, 10) * _tmp133 + + P(3, 10) * _tmp129 + P(5, 10); + _res(6, 10) = P(0, 10) * _tmp158 + P(1, 10) * _tmp156 - P(13, 10) * _tmp154 - + P(14, 10) * _tmp150 - P(15, 10) * _tmp152 + P(2, 10) * _tmp157 + + P(3, 10) * _tmp159 + P(6, 10); + _res(7, 10) = P(4, 10) * d_vel_dt + P(7, 10); + _res(8, 10) = P(5, 10) * d_vel_dt + P(8, 10); + _res(9, 10) = P(6, 10) * d_vel_dt + P(9, 10); + _res(10, 10) = P(10, 10); + _res(11, 10) = 0; + _res(12, 10) = 0; + _res(13, 10) = 0; + _res(14, 10) = 0; + _res(15, 10) = 0; + _res(16, 10) = 0; + _res(17, 10) = 0; + _res(18, 10) = 0; + _res(19, 10) = 0; + _res(20, 10) = 0; + _res(21, 10) = 0; + _res(22, 10) = 0; + _res(23, 10) = 0; + _res(0, 11) = _tmp14; + _res(1, 11) = _tmp35; + _res(2, 11) = _tmp47; + _res(3, 11) = _tmp54; + _res(4, 11) = P(0, 11) * _tmp64 + P(1, 11) * _tmp85 - P(13, 11) * _tmp69 - P(14, 11) * _tmp81 - + P(15, 11) * _tmp75 + P(2, 11) * _tmp90 + P(3, 11) * _tmp93 + P(4, 11); + _res(5, 11) = P(0, 11) * _tmp131 + P(1, 11) * _tmp127 - P(13, 11) * _tmp124 - + P(14, 11) * _tmp122 - P(15, 11) * _tmp119 + P(2, 11) * _tmp133 + + P(3, 11) * _tmp129 + P(5, 11); + _res(6, 11) = P(0, 11) * _tmp158 + P(1, 11) * _tmp156 - P(13, 11) * _tmp154 - + P(14, 11) * _tmp150 - P(15, 11) * _tmp152 + P(2, 11) * _tmp157 + + P(3, 11) * _tmp159 + P(6, 11); + _res(7, 11) = P(4, 11) * d_vel_dt + P(7, 11); + _res(8, 11) = P(5, 11) * d_vel_dt + P(8, 11); + _res(9, 11) = P(6, 11) * d_vel_dt + P(9, 11); + _res(10, 11) = P(10, 11); + _res(11, 11) = P(11, 11); + _res(12, 11) = 0; + _res(13, 11) = 0; + _res(14, 11) = 0; + _res(15, 11) = 0; + _res(16, 11) = 0; + _res(17, 11) = 0; + _res(18, 11) = 0; + _res(19, 11) = 0; + _res(20, 11) = 0; + _res(21, 11) = 0; + _res(22, 11) = 0; + _res(23, 11) = 0; + _res(0, 12) = _tmp15; + _res(1, 12) = _tmp33; + _res(2, 12) = _tmp49; + _res(3, 12) = _tmp55; + _res(4, 12) = P(0, 12) * _tmp64 + P(1, 12) * _tmp85 - P(13, 12) * _tmp69 - P(14, 12) * _tmp81 - + P(15, 12) * _tmp75 + P(2, 12) * _tmp90 + P(3, 12) * _tmp93 + P(4, 12); + _res(5, 12) = P(0, 12) * _tmp131 + P(1, 12) * _tmp127 - P(13, 12) * _tmp124 - + P(14, 12) * _tmp122 - P(15, 12) * _tmp119 + P(2, 12) * _tmp133 + + P(3, 12) * _tmp129 + P(5, 12); + _res(6, 12) = P(0, 12) * _tmp158 + P(1, 12) * _tmp156 - P(13, 12) * _tmp154 - + P(14, 12) * _tmp150 - P(15, 12) * _tmp152 + P(2, 12) * _tmp157 + + P(3, 12) * _tmp159 + P(6, 12); + _res(7, 12) = P(4, 12) * d_vel_dt + P(7, 12); + _res(8, 12) = P(5, 12) * d_vel_dt + P(8, 12); + _res(9, 12) = P(6, 12) * d_vel_dt + P(9, 12); + _res(10, 12) = P(10, 12); + _res(11, 12) = P(11, 12); + _res(12, 12) = P(12, 12); + _res(13, 12) = 0; + _res(14, 12) = 0; + _res(15, 12) = 0; + _res(16, 12) = 0; + _res(17, 12) = 0; + _res(18, 12) = 0; + _res(19, 12) = 0; + _res(20, 12) = 0; + _res(21, 12) = 0; + _res(22, 12) = 0; + _res(23, 12) = 0; + _res(0, 13) = _tmp65; + _res(1, 13) = _tmp96; + _res(2, 13) = _tmp100; + _res(3, 13) = _tmp104; + _res(4, 13) = _tmp108; + _res(5, 13) = _tmp141; + _res(6, 13) = _tmp168; + _res(7, 13) = P(4, 13) * d_vel_dt + P(7, 13); + _res(8, 13) = P(5, 13) * d_vel_dt + P(8, 13); + _res(9, 13) = P(6, 13) * d_vel_dt + P(9, 13); + _res(10, 13) = P(10, 13); + _res(11, 13) = P(11, 13); + _res(12, 13) = P(12, 13); + _res(13, 13) = P(13, 13); + _res(14, 13) = 0; + _res(15, 13) = 0; + _res(16, 13) = 0; + _res(17, 13) = 0; + _res(18, 13) = 0; + _res(19, 13) = 0; + _res(20, 13) = 0; + _res(21, 13) = 0; + _res(22, 13) = 0; + _res(23, 13) = 0; + _res(0, 14) = _tmp76; + _res(1, 14) = _tmp97; + _res(2, 14) = _tmp101; + _res(3, 14) = _tmp105; + _res(4, 14) = _tmp109; + _res(5, 14) = _tmp142; + _res(6, 14) = _tmp169; + _res(7, 14) = P(4, 14) * d_vel_dt + P(7, 14); + _res(8, 14) = P(5, 14) * d_vel_dt + P(8, 14); + _res(9, 14) = P(6, 14) * d_vel_dt + P(9, 14); + _res(10, 14) = P(10, 14); + _res(11, 14) = P(11, 14); + _res(12, 14) = P(12, 14); + _res(13, 14) = P(13, 14); + _res(14, 14) = P(14, 14); + _res(15, 14) = 0; + _res(16, 14) = 0; + _res(17, 14) = 0; + _res(18, 14) = 0; + _res(19, 14) = 0; + _res(20, 14) = 0; + _res(21, 14) = 0; + _res(22, 14) = 0; + _res(23, 14) = 0; + _res(0, 15) = _tmp70; + _res(1, 15) = _tmp98; + _res(2, 15) = _tmp102; + _res(3, 15) = _tmp106; + _res(4, 15) = _tmp110; + _res(5, 15) = _tmp143; + _res(6, 15) = _tmp170; + _res(7, 15) = P(4, 15) * d_vel_dt + P(7, 15); + _res(8, 15) = P(5, 15) * d_vel_dt + P(8, 15); + _res(9, 15) = P(6, 15) * d_vel_dt + P(9, 15); + _res(10, 15) = P(10, 15); + _res(11, 15) = P(11, 15); + _res(12, 15) = P(12, 15); + _res(13, 15) = P(13, 15); + _res(14, 15) = P(14, 15); + _res(15, 15) = P(15, 15); + _res(16, 15) = 0; + _res(17, 15) = 0; + _res(18, 15) = 0; + _res(19, 15) = 0; + _res(20, 15) = 0; + _res(21, 15) = 0; + _res(22, 15) = 0; + _res(23, 15) = 0; + _res(0, 16) = P(0, 16) + P(1, 16) * _tmp9 + P(10, 16) * _tmp10 + P(11, 16) * _tmp12 + + P(12, 16) * _tmp11 + P(2, 16) * _tmp3 + P(3, 16) * _tmp6; + _res(1, 16) = P(0, 16) * _tmp31 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 - + P(12, 16) * _tmp12 + P(2, 16) * _tmp32 + P(3, 16) * _tmp3; + _res(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 + + P(12, 16) * _tmp10 + P(2, 16) + P(3, 16) * _tmp31; + _res(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp12 - P(11, 16) * _tmp10 - + P(12, 16) * _tmp29 + P(2, 16) * _tmp9 + P(3, 16); + _res(4, 16) = P(0, 16) * _tmp64 + P(1, 16) * _tmp85 - P(13, 16) * _tmp69 - P(14, 16) * _tmp81 - + P(15, 16) * _tmp75 + P(2, 16) * _tmp90 + P(3, 16) * _tmp93 + P(4, 16); + _res(5, 16) = P(0, 16) * _tmp131 + P(1, 16) * _tmp127 - P(13, 16) * _tmp124 - + P(14, 16) * _tmp122 - P(15, 16) * _tmp119 + P(2, 16) * _tmp133 + + P(3, 16) * _tmp129 + P(5, 16); + _res(6, 16) = P(0, 16) * _tmp158 + P(1, 16) * _tmp156 - P(13, 16) * _tmp154 - + P(14, 16) * _tmp150 - P(15, 16) * _tmp152 + P(2, 16) * _tmp157 + + P(3, 16) * _tmp159 + P(6, 16); + _res(7, 16) = P(4, 16) * d_vel_dt + P(7, 16); + _res(8, 16) = P(5, 16) * d_vel_dt + P(8, 16); + _res(9, 16) = P(6, 16) * d_vel_dt + P(9, 16); + _res(10, 16) = P(10, 16); + _res(11, 16) = P(11, 16); + _res(12, 16) = P(12, 16); + _res(13, 16) = P(13, 16); + _res(14, 16) = P(14, 16); + _res(15, 16) = P(15, 16); + _res(16, 16) = P(16, 16); + _res(17, 16) = 0; + _res(18, 16) = 0; + _res(19, 16) = 0; + _res(20, 16) = 0; + _res(21, 16) = 0; + _res(22, 16) = 0; + _res(23, 16) = 0; + _res(0, 17) = P(0, 17) + P(1, 17) * _tmp9 + P(10, 17) * _tmp10 + P(11, 17) * _tmp12 + + P(12, 17) * _tmp11 + P(2, 17) * _tmp3 + P(3, 17) * _tmp6; + _res(1, 17) = P(0, 17) * _tmp31 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 - + P(12, 17) * _tmp12 + P(2, 17) * _tmp32 + P(3, 17) * _tmp3; + _res(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 + + P(12, 17) * _tmp10 + P(2, 17) + P(3, 17) * _tmp31; + _res(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp12 - P(11, 17) * _tmp10 - + P(12, 17) * _tmp29 + P(2, 17) * _tmp9 + P(3, 17); + _res(4, 17) = P(0, 17) * _tmp64 + P(1, 17) * _tmp85 - P(13, 17) * _tmp69 - P(14, 17) * _tmp81 - + P(15, 17) * _tmp75 + P(2, 17) * _tmp90 + P(3, 17) * _tmp93 + P(4, 17); + _res(5, 17) = P(0, 17) * _tmp131 + P(1, 17) * _tmp127 - P(13, 17) * _tmp124 - + P(14, 17) * _tmp122 - P(15, 17) * _tmp119 + P(2, 17) * _tmp133 + + P(3, 17) * _tmp129 + P(5, 17); + _res(6, 17) = P(0, 17) * _tmp158 + P(1, 17) * _tmp156 - P(13, 17) * _tmp154 - + P(14, 17) * _tmp150 - P(15, 17) * _tmp152 + P(2, 17) * _tmp157 + + P(3, 17) * _tmp159 + P(6, 17); + _res(7, 17) = P(4, 17) * d_vel_dt + P(7, 17); + _res(8, 17) = P(5, 17) * d_vel_dt + P(8, 17); + _res(9, 17) = P(6, 17) * d_vel_dt + P(9, 17); + _res(10, 17) = P(10, 17); + _res(11, 17) = P(11, 17); + _res(12, 17) = P(12, 17); + _res(13, 17) = P(13, 17); + _res(14, 17) = P(14, 17); + _res(15, 17) = P(15, 17); + _res(16, 17) = P(16, 17); + _res(17, 17) = P(17, 17); + _res(18, 17) = 0; + _res(19, 17) = 0; + _res(20, 17) = 0; + _res(21, 17) = 0; + _res(22, 17) = 0; + _res(23, 17) = 0; + _res(0, 18) = P(0, 18) + P(1, 18) * _tmp9 + P(10, 18) * _tmp10 + P(11, 18) * _tmp12 + + P(12, 18) * _tmp11 + P(2, 18) * _tmp3 + P(3, 18) * _tmp6; + _res(1, 18) = P(0, 18) * _tmp31 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 - + P(12, 18) * _tmp12 + P(2, 18) * _tmp32 + P(3, 18) * _tmp3; + _res(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 + + P(12, 18) * _tmp10 + P(2, 18) + P(3, 18) * _tmp31; + _res(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp12 - P(11, 18) * _tmp10 - + P(12, 18) * _tmp29 + P(2, 18) * _tmp9 + P(3, 18); + _res(4, 18) = P(0, 18) * _tmp64 + P(1, 18) * _tmp85 - P(13, 18) * _tmp69 - P(14, 18) * _tmp81 - + P(15, 18) * _tmp75 + P(2, 18) * _tmp90 + P(3, 18) * _tmp93 + P(4, 18); + _res(5, 18) = P(0, 18) * _tmp131 + P(1, 18) * _tmp127 - P(13, 18) * _tmp124 - + P(14, 18) * _tmp122 - P(15, 18) * _tmp119 + P(2, 18) * _tmp133 + + P(3, 18) * _tmp129 + P(5, 18); + _res(6, 18) = P(0, 18) * _tmp158 + P(1, 18) * _tmp156 - P(13, 18) * _tmp154 - + P(14, 18) * _tmp150 - P(15, 18) * _tmp152 + P(2, 18) * _tmp157 + + P(3, 18) * _tmp159 + P(6, 18); + _res(7, 18) = P(4, 18) * d_vel_dt + P(7, 18); + _res(8, 18) = P(5, 18) * d_vel_dt + P(8, 18); + _res(9, 18) = P(6, 18) * d_vel_dt + P(9, 18); + _res(10, 18) = P(10, 18); + _res(11, 18) = P(11, 18); + _res(12, 18) = P(12, 18); + _res(13, 18) = P(13, 18); + _res(14, 18) = P(14, 18); + _res(15, 18) = P(15, 18); + _res(16, 18) = P(16, 18); + _res(17, 18) = P(17, 18); + _res(18, 18) = P(18, 18); + _res(19, 18) = 0; + _res(20, 18) = 0; + _res(21, 18) = 0; + _res(22, 18) = 0; + _res(23, 18) = 0; + _res(0, 19) = P(0, 19) + P(1, 19) * _tmp9 + P(10, 19) * _tmp10 + P(11, 19) * _tmp12 + + P(12, 19) * _tmp11 + P(2, 19) * _tmp3 + P(3, 19) * _tmp6; + _res(1, 19) = P(0, 19) * _tmp31 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 - + P(12, 19) * _tmp12 + P(2, 19) * _tmp32 + P(3, 19) * _tmp3; + _res(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 + + P(12, 19) * _tmp10 + P(2, 19) + P(3, 19) * _tmp31; + _res(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp12 - P(11, 19) * _tmp10 - + P(12, 19) * _tmp29 + P(2, 19) * _tmp9 + P(3, 19); + _res(4, 19) = P(0, 19) * _tmp64 + P(1, 19) * _tmp85 - P(13, 19) * _tmp69 - P(14, 19) * _tmp81 - + P(15, 19) * _tmp75 + P(2, 19) * _tmp90 + P(3, 19) * _tmp93 + P(4, 19); + _res(5, 19) = P(0, 19) * _tmp131 + P(1, 19) * _tmp127 - P(13, 19) * _tmp124 - + P(14, 19) * _tmp122 - P(15, 19) * _tmp119 + P(2, 19) * _tmp133 + + P(3, 19) * _tmp129 + P(5, 19); + _res(6, 19) = P(0, 19) * _tmp158 + P(1, 19) * _tmp156 - P(13, 19) * _tmp154 - + P(14, 19) * _tmp150 - P(15, 19) * _tmp152 + P(2, 19) * _tmp157 + + P(3, 19) * _tmp159 + P(6, 19); + _res(7, 19) = P(4, 19) * d_vel_dt + P(7, 19); + _res(8, 19) = P(5, 19) * d_vel_dt + P(8, 19); + _res(9, 19) = P(6, 19) * d_vel_dt + P(9, 19); + _res(10, 19) = P(10, 19); + _res(11, 19) = P(11, 19); + _res(12, 19) = P(12, 19); + _res(13, 19) = P(13, 19); + _res(14, 19) = P(14, 19); + _res(15, 19) = P(15, 19); + _res(16, 19) = P(16, 19); + _res(17, 19) = P(17, 19); + _res(18, 19) = P(18, 19); + _res(19, 19) = P(19, 19); + _res(20, 19) = 0; + _res(21, 19) = 0; + _res(22, 19) = 0; + _res(23, 19) = 0; + _res(0, 20) = P(0, 20) + P(1, 20) * _tmp9 + P(10, 20) * _tmp10 + P(11, 20) * _tmp12 + + P(12, 20) * _tmp11 + P(2, 20) * _tmp3 + P(3, 20) * _tmp6; + _res(1, 20) = P(0, 20) * _tmp31 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 - + P(12, 20) * _tmp12 + P(2, 20) * _tmp32 + P(3, 20) * _tmp3; + _res(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 + + P(12, 20) * _tmp10 + P(2, 20) + P(3, 20) * _tmp31; + _res(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp12 - P(11, 20) * _tmp10 - + P(12, 20) * _tmp29 + P(2, 20) * _tmp9 + P(3, 20); + _res(4, 20) = P(0, 20) * _tmp64 + P(1, 20) * _tmp85 - P(13, 20) * _tmp69 - P(14, 20) * _tmp81 - + P(15, 20) * _tmp75 + P(2, 20) * _tmp90 + P(3, 20) * _tmp93 + P(4, 20); + _res(5, 20) = P(0, 20) * _tmp131 + P(1, 20) * _tmp127 - P(13, 20) * _tmp124 - + P(14, 20) * _tmp122 - P(15, 20) * _tmp119 + P(2, 20) * _tmp133 + + P(3, 20) * _tmp129 + P(5, 20); + _res(6, 20) = P(0, 20) * _tmp158 + P(1, 20) * _tmp156 - P(13, 20) * _tmp154 - + P(14, 20) * _tmp150 - P(15, 20) * _tmp152 + P(2, 20) * _tmp157 + + P(3, 20) * _tmp159 + P(6, 20); + _res(7, 20) = P(4, 20) * d_vel_dt + P(7, 20); + _res(8, 20) = P(5, 20) * d_vel_dt + P(8, 20); + _res(9, 20) = P(6, 20) * d_vel_dt + P(9, 20); + _res(10, 20) = P(10, 20); + _res(11, 20) = P(11, 20); + _res(12, 20) = P(12, 20); + _res(13, 20) = P(13, 20); + _res(14, 20) = P(14, 20); + _res(15, 20) = P(15, 20); + _res(16, 20) = P(16, 20); + _res(17, 20) = P(17, 20); + _res(18, 20) = P(18, 20); + _res(19, 20) = P(19, 20); + _res(20, 20) = P(20, 20); + _res(21, 20) = 0; + _res(22, 20) = 0; + _res(23, 20) = 0; + _res(0, 21) = P(0, 21) + P(1, 21) * _tmp9 + P(10, 21) * _tmp10 + P(11, 21) * _tmp12 + + P(12, 21) * _tmp11 + P(2, 21) * _tmp3 + P(3, 21) * _tmp6; + _res(1, 21) = P(0, 21) * _tmp31 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 - + P(12, 21) * _tmp12 + P(2, 21) * _tmp32 + P(3, 21) * _tmp3; + _res(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 + + P(12, 21) * _tmp10 + P(2, 21) + P(3, 21) * _tmp31; + _res(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp12 - P(11, 21) * _tmp10 - + P(12, 21) * _tmp29 + P(2, 21) * _tmp9 + P(3, 21); + _res(4, 21) = P(0, 21) * _tmp64 + P(1, 21) * _tmp85 - P(13, 21) * _tmp69 - P(14, 21) * _tmp81 - + P(15, 21) * _tmp75 + P(2, 21) * _tmp90 + P(3, 21) * _tmp93 + P(4, 21); + _res(5, 21) = P(0, 21) * _tmp131 + P(1, 21) * _tmp127 - P(13, 21) * _tmp124 - + P(14, 21) * _tmp122 - P(15, 21) * _tmp119 + P(2, 21) * _tmp133 + + P(3, 21) * _tmp129 + P(5, 21); + _res(6, 21) = P(0, 21) * _tmp158 + P(1, 21) * _tmp156 - P(13, 21) * _tmp154 - + P(14, 21) * _tmp150 - P(15, 21) * _tmp152 + P(2, 21) * _tmp157 + + P(3, 21) * _tmp159 + P(6, 21); + _res(7, 21) = P(4, 21) * d_vel_dt + P(7, 21); + _res(8, 21) = P(5, 21) * d_vel_dt + P(8, 21); + _res(9, 21) = P(6, 21) * d_vel_dt + P(9, 21); + _res(10, 21) = P(10, 21); + _res(11, 21) = P(11, 21); + _res(12, 21) = P(12, 21); + _res(13, 21) = P(13, 21); + _res(14, 21) = P(14, 21); + _res(15, 21) = P(15, 21); + _res(16, 21) = P(16, 21); + _res(17, 21) = P(17, 21); + _res(18, 21) = P(18, 21); + _res(19, 21) = P(19, 21); + _res(20, 21) = P(20, 21); + _res(21, 21) = P(21, 21); + _res(22, 21) = 0; + _res(23, 21) = 0; + _res(0, 22) = P(0, 22) + P(1, 22) * _tmp9 + P(10, 22) * _tmp10 + P(11, 22) * _tmp12 + + P(12, 22) * _tmp11 + P(2, 22) * _tmp3 + P(3, 22) * _tmp6; + _res(1, 22) = P(0, 22) * _tmp31 + P(1, 22) - P(10, 22) * _tmp29 + P(11, 22) * _tmp11 - + P(12, 22) * _tmp12 + P(2, 22) * _tmp32 + P(3, 22) * _tmp3; + _res(2, 22) = P(0, 22) * _tmp43 + P(1, 22) * _tmp6 - P(10, 22) * _tmp11 - P(11, 22) * _tmp29 + + P(12, 22) * _tmp10 + P(2, 22) + P(3, 22) * _tmp31; + _res(3, 22) = P(0, 22) * _tmp32 + P(1, 22) * _tmp43 + P(10, 22) * _tmp12 - P(11, 22) * _tmp10 - + P(12, 22) * _tmp29 + P(2, 22) * _tmp9 + P(3, 22); + _res(4, 22) = P(0, 22) * _tmp64 + P(1, 22) * _tmp85 - P(13, 22) * _tmp69 - P(14, 22) * _tmp81 - + P(15, 22) * _tmp75 + P(2, 22) * _tmp90 + P(3, 22) * _tmp93 + P(4, 22); + _res(5, 22) = P(0, 22) * _tmp131 + P(1, 22) * _tmp127 - P(13, 22) * _tmp124 - + P(14, 22) * _tmp122 - P(15, 22) * _tmp119 + P(2, 22) * _tmp133 + + P(3, 22) * _tmp129 + P(5, 22); + _res(6, 22) = P(0, 22) * _tmp158 + P(1, 22) * _tmp156 - P(13, 22) * _tmp154 - + P(14, 22) * _tmp150 - P(15, 22) * _tmp152 + P(2, 22) * _tmp157 + + P(3, 22) * _tmp159 + P(6, 22); + _res(7, 22) = P(4, 22) * d_vel_dt + P(7, 22); + _res(8, 22) = P(5, 22) * d_vel_dt + P(8, 22); + _res(9, 22) = P(6, 22) * d_vel_dt + P(9, 22); + _res(10, 22) = P(10, 22); + _res(11, 22) = P(11, 22); + _res(12, 22) = P(12, 22); + _res(13, 22) = P(13, 22); + _res(14, 22) = P(14, 22); + _res(15, 22) = P(15, 22); + _res(16, 22) = P(16, 22); + _res(17, 22) = P(17, 22); + _res(18, 22) = P(18, 22); + _res(19, 22) = P(19, 22); + _res(20, 22) = P(20, 22); + _res(21, 22) = P(21, 22); + _res(22, 22) = P(22, 22); + _res(23, 22) = 0; + _res(0, 23) = P(0, 23) + P(1, 23) * _tmp9 + P(10, 23) * _tmp10 + P(11, 23) * _tmp12 + + P(12, 23) * _tmp11 + P(2, 23) * _tmp3 + P(3, 23) * _tmp6; + _res(1, 23) = P(0, 23) * _tmp31 + P(1, 23) - P(10, 23) * _tmp29 + P(11, 23) * _tmp11 - + P(12, 23) * _tmp12 + P(2, 23) * _tmp32 + P(3, 23) * _tmp3; + _res(2, 23) = P(0, 23) * _tmp43 + P(1, 23) * _tmp6 - P(10, 23) * _tmp11 - P(11, 23) * _tmp29 + + P(12, 23) * _tmp10 + P(2, 23) + P(3, 23) * _tmp31; + _res(3, 23) = P(0, 23) * _tmp32 + P(1, 23) * _tmp43 + P(10, 23) * _tmp12 - P(11, 23) * _tmp10 - + P(12, 23) * _tmp29 + P(2, 23) * _tmp9 + P(3, 23); + _res(4, 23) = P(0, 23) * _tmp64 + P(1, 23) * _tmp85 - P(13, 23) * _tmp69 - P(14, 23) * _tmp81 - + P(15, 23) * _tmp75 + P(2, 23) * _tmp90 + P(3, 23) * _tmp93 + P(4, 23); + _res(5, 23) = P(0, 23) * _tmp131 + P(1, 23) * _tmp127 - P(13, 23) * _tmp124 - + P(14, 23) * _tmp122 - P(15, 23) * _tmp119 + P(2, 23) * _tmp133 + + P(3, 23) * _tmp129 + P(5, 23); + _res(6, 23) = P(0, 23) * _tmp158 + P(1, 23) * _tmp156 - P(13, 23) * _tmp154 - + P(14, 23) * _tmp150 - P(15, 23) * _tmp152 + P(2, 23) * _tmp157 + + P(3, 23) * _tmp159 + P(6, 23); + _res(7, 23) = P(4, 23) * d_vel_dt + P(7, 23); + _res(8, 23) = P(5, 23) * d_vel_dt + P(8, 23); + _res(9, 23) = P(6, 23) * d_vel_dt + P(9, 23); + _res(10, 23) = P(10, 23); + _res(11, 23) = P(11, 23); + _res(12, 23) = P(12, 23); + _res(13, 23) = P(13, 23); + _res(14, 23) = P(14, 23); + _res(15, 23) = P(15, 23); + _res(16, 23) = P(16, 23); + _res(17, 23) = P(17, 23); + _res(18, 23) = P(18, 23); + _res(19, 23) = P(19, 23); + _res(20, 23) = P(20, 23); + _res(21, 23) = P(21, 23); + _res(22, 23) = P(22, 23); + _res(23, 23) = P(23, 23); + + return _res; } // NOLINT(readability/fn_size) // NOLINTNEXTLINE(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h new file mode 100644 index 0000000000..8b2166f223 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h @@ -0,0 +1,123 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: rot_var_ned_to_lower_triangular_quat_cov + * + * Args: + * state: Matrix24_1 + * rot_var_ned: Matrix31 + * + * Outputs: + * q_cov_lower_triangle: Matrix44 + */ +template +void RotVarNedToLowerTriangularQuatCov( + const matrix::Matrix& state, const matrix::Matrix& rot_var_ned, + matrix::Matrix* const q_cov_lower_triangle = nullptr) { + // Total ops: 185 + + // Input arrays + + // Intermediate terms (54) + const Scalar _tmp0 = 2 * state(0, 0) * state(2, 0); + const Scalar _tmp1 = 2 * state(3, 0); + const Scalar _tmp2 = _tmp1 * state(1, 0); + const Scalar _tmp3 = _tmp0 + _tmp2; + const Scalar _tmp4 = _tmp1 * state(0, 0); + const Scalar _tmp5 = 2 * state(1, 0); + const Scalar _tmp6 = _tmp5 * state(2, 0); + const Scalar _tmp7 = -_tmp4 + _tmp6; + const Scalar _tmp8 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp9 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp10 = _tmp8 + _tmp9 + 1; + const Scalar _tmp11 = _tmp1 * state(2, 0); + const Scalar _tmp12 = _tmp5 * state(0, 0); + const Scalar _tmp13 = _tmp11 - _tmp12; + const Scalar _tmp14 = _tmp13 * rot_var_ned(1, 0); + const Scalar _tmp15 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp16 = _tmp15 + _tmp9; + const Scalar _tmp17 = _tmp11 + _tmp12; + const Scalar _tmp18 = _tmp17 * rot_var_ned(2, 0); + const Scalar _tmp19 = _tmp10 * _tmp14 + _tmp16 * _tmp18 + _tmp3 * _tmp7 * rot_var_ned(0, 0); + const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp21 = -_tmp19 * _tmp20; + const Scalar _tmp22 = std::pow(_tmp10, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp17, Scalar(2)) * rot_var_ned(2, 0) + + std::pow(_tmp7, Scalar(2)) * rot_var_ned(0, 0); + const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp24 = _tmp15 + _tmp8; + const Scalar _tmp25 = _tmp24 * rot_var_ned(0, 0); + const Scalar _tmp26 = _tmp4 + _tmp6; + const Scalar _tmp27 = -_tmp0 + _tmp2; + const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10 * _tmp26 * rot_var_ned(1, 0) + + (Scalar(1) / Scalar(2)) * _tmp18 * _tmp27 + + (Scalar(1) / Scalar(2)) * _tmp25 * _tmp7; + const Scalar _tmp29 = _tmp28 * state(1, 0); + const Scalar _tmp30 = std::pow(_tmp24, Scalar(2)) * rot_var_ned(0, 0) + + std::pow(_tmp26, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp27, Scalar(2)) * rot_var_ned(2, 0); + const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp32 = _tmp14 * _tmp26 + _tmp16 * _tmp27 * rot_var_ned(2, 0) + _tmp25 * _tmp3; + const Scalar _tmp33 = _tmp20 * _tmp32; + const Scalar _tmp34 = -_tmp28 * state(2, 0); + const Scalar _tmp35 = _tmp19 * _tmp23; + const Scalar _tmp36 = std::pow(_tmp13, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp16, Scalar(2)) * rot_var_ned(2, 0) + + std::pow(_tmp3, Scalar(2)) * rot_var_ned(0, 0); + const Scalar _tmp37 = -_tmp31 * _tmp32; + const Scalar _tmp38 = _tmp28 * state(0, 0); + const Scalar _tmp39 = -_tmp20 * _tmp22 + _tmp35 + _tmp38; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp41 = _tmp23 * _tmp32; + const Scalar _tmp42 = _tmp28 * state(3, 0); + const Scalar _tmp43 = _tmp30 * _tmp40 + _tmp41 - _tmp42; + const Scalar _tmp44 = _tmp32 * _tmp40; + const Scalar _tmp45 = _tmp21 + _tmp23 * _tmp36 + _tmp44; + const Scalar _tmp46 = _tmp19 * _tmp31; + const Scalar _tmp47 = _tmp22 * _tmp40 + _tmp42 - _tmp46; + const Scalar _tmp48 = _tmp20 * _tmp30 + _tmp37 + _tmp38; + const Scalar _tmp49 = _tmp19 * _tmp40; + const Scalar _tmp50 = -_tmp31 * _tmp36 + _tmp33 + _tmp49; + const Scalar _tmp51 = _tmp22 * _tmp31 + _tmp34 + _tmp49; + const Scalar _tmp52 = -_tmp23 * _tmp30 + _tmp29 + _tmp44; + const Scalar _tmp53 = _tmp36 * _tmp40 - _tmp41 + _tmp46; + + // Output terms (1) + if (q_cov_lower_triangle != nullptr) { + matrix::Matrix& _q_cov_lower_triangle = (*q_cov_lower_triangle); + + _q_cov_lower_triangle(0, 0) = -_tmp20 * (-_tmp20 * _tmp36 - _tmp35 + _tmp37) - + _tmp23 * (_tmp21 - _tmp22 * _tmp23 - _tmp29) - + _tmp31 * (-_tmp30 * _tmp31 - _tmp33 + _tmp34); + _q_cov_lower_triangle(1, 0) = -_tmp20 * _tmp45 - _tmp23 * _tmp39 - _tmp31 * _tmp43; + _q_cov_lower_triangle(2, 0) = -_tmp20 * _tmp50 - _tmp23 * _tmp47 - _tmp31 * _tmp48; + _q_cov_lower_triangle(3, 0) = -_tmp20 * _tmp53 - _tmp23 * _tmp51 - _tmp31 * _tmp52; + _q_cov_lower_triangle(0, 1) = 0; + _q_cov_lower_triangle(1, 1) = -_tmp20 * _tmp39 + _tmp23 * _tmp45 + _tmp40 * _tmp43; + _q_cov_lower_triangle(2, 1) = -_tmp20 * _tmp47 + _tmp23 * _tmp50 + _tmp40 * _tmp48; + _q_cov_lower_triangle(3, 1) = -_tmp20 * _tmp51 + _tmp23 * _tmp53 + _tmp40 * _tmp52; + _q_cov_lower_triangle(0, 2) = 0; + _q_cov_lower_triangle(1, 2) = 0; + _q_cov_lower_triangle(2, 2) = _tmp20 * _tmp48 - _tmp31 * _tmp50 + _tmp40 * _tmp47; + _q_cov_lower_triangle(3, 2) = _tmp20 * _tmp52 - _tmp31 * _tmp53 + _tmp40 * _tmp51; + _q_cov_lower_triangle(0, 3) = 0; + _q_cov_lower_triangle(1, 3) = 0; + _q_cov_lower_triangle(2, 3) = 0; + _q_cov_lower_triangle(3, 3) = -_tmp23 * _tmp52 + _tmp31 * _tmp51 + _tmp40 * _tmp53; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h new file mode 100644 index 0000000000..200af9c0e4 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h @@ -0,0 +1,55 @@ +// -------------------------------------------------- +// This file was autogenerated, do NOT modify by hand +// -------------------------------------------------- + +#ifndef EKF_STATE_H +#define EKF_STATE_H + +#include + +namespace estimator +{ +struct StateSample { + matrix::Quaternion quat_nominal{}; + matrix::Vector3 vel{}; + matrix::Vector3 pos{}; + matrix::Vector3 gyro_bias{}; + matrix::Vector3 accel_bias{}; + matrix::Vector3 mag_I{}; + matrix::Vector3 mag_B{}; + matrix::Vector2 wind_vel{}; + + matrix::Vector Data() const { + matrix::Vector state; + state.slice<4, 1>(0, 0) = quat_nominal; + state.slice<3, 1>(4, 0) = vel; + state.slice<3, 1>(7, 0) = pos; + state.slice<3, 1>(10, 0) = gyro_bias; + state.slice<3, 1>(13, 0) = accel_bias; + state.slice<3, 1>(16, 0) = mag_I; + state.slice<3, 1>(19, 0) = mag_B; + state.slice<2, 1>(22, 0) = wind_vel; + return state; + }; + + const matrix::Vector& vector() const { + return *reinterpret_cast*>(const_cast(reinterpret_cast(&quat_nominal))); + }; + +}; +static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); + +struct IdxDof { unsigned idx; unsigned dof; }; +namespace State { + static constexpr IdxDof quat_nominal{0, 4}; + static constexpr IdxDof vel{4, 3}; + static constexpr IdxDof pos{7, 3}; + static constexpr IdxDof gyro_bias{10, 3}; + static constexpr IdxDof accel_bias{13, 3}; + static constexpr IdxDof mag_I{16, 3}; + static constexpr IdxDof mag_B{19, 3}; + static constexpr IdxDof wind_vel{22, 2}; + static constexpr uint8_t size{24}; +}; +} +#endif // !EKF_STATE_H diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h index b2af651030..6b82a6534f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -35,13 +35,13 @@ void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P const Scalar R, const Scalar epsilon, matrix::Matrix* const innov_var = nullptr, Scalar* const H = nullptr) { - // Total ops: 28 + // Total ops: 27 // Input arrays // Intermediate terms (4) - const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) - - std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2)); + const Scalar _tmp0 = + -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1; const Scalar _tmp1 = pos_z - terrain_vpos; const Scalar _tmp2 = -_tmp1 + epsilon * (2 * math::min(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h index 0bf7cb85f0..461db323cf 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -34,13 +34,13 @@ void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P, const matrix::Matrix& v, const Scalar pos_z, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, Scalar* const H = nullptr) { - // Total ops: 26 + // Total ops: 25 // Input arrays // Intermediate terms (3) - const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) - - std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2)); + const Scalar _tmp0 = + -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1; const Scalar _tmp1 = pos_z - terrain_vpos; const Scalar _tmp2 = -_tmp1 + epsilon * (2 * math::min(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h index 19d384712b..4f0048b7ac 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h index 352f9d75b0..02f48e62b7 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h deleted file mode 100644 index 687f524627..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h +++ /dev/null @@ -1,101 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: yaw_var_to_lower_triangular_quat_cov - * - * Args: - * state: Matrix24_1 - * yaw_var: Scalar - * - * Outputs: - * q_cov_lower_triangle: Matrix44 - */ -template -void YawVarToLowerTriangularQuatCov( - const matrix::Matrix& state, const Scalar yaw_var, - matrix::Matrix* const q_cov_lower_triangle = nullptr) { - // Total ops: 136 - - // Input arrays - - // Intermediate terms (39) - const Scalar _tmp0 = (Scalar(1) / Scalar(2)) * state(3, 0); - const Scalar _tmp1 = - -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; - const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)) * yaw_var; - const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0); - const Scalar _tmp4 = 2 * state(2, 0); - const Scalar _tmp5 = 2 * state(1, 0); - const Scalar _tmp6 = -_tmp4 * state(0, 0) + _tmp5 * state(3, 0); - const Scalar _tmp7 = _tmp1 * _tmp6 * yaw_var; - const Scalar _tmp8 = -_tmp3 * _tmp7; - const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(2, 0); - const Scalar _tmp10 = _tmp4 * state(3, 0) + _tmp5 * state(0, 0); - const Scalar _tmp11 = _tmp10 * yaw_var; - const Scalar _tmp12 = _tmp1 * _tmp11; - const Scalar _tmp13 = _tmp12 * _tmp9; - const Scalar _tmp14 = std::pow(_tmp10, Scalar(2)) * yaw_var; - const Scalar _tmp15 = _tmp11 * _tmp6; - const Scalar _tmp16 = _tmp15 * _tmp3; - const Scalar _tmp17 = -_tmp0 * _tmp12; - const Scalar _tmp18 = std::pow(_tmp6, Scalar(2)) * yaw_var; - const Scalar _tmp19 = _tmp0 * _tmp7; - const Scalar _tmp20 = -_tmp15 * _tmp9; - const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * state(0, 0); - const Scalar _tmp22 = _tmp21 * _tmp7; - const Scalar _tmp23 = _tmp17 + _tmp2 * _tmp9 + _tmp22; - const Scalar _tmp24 = _tmp15 * _tmp21; - const Scalar _tmp25 = -_tmp0 * _tmp14 + _tmp13 + _tmp24; - const Scalar _tmp26 = _tmp7 * _tmp9; - const Scalar _tmp27 = _tmp0 * _tmp15; - const Scalar _tmp28 = _tmp18 * _tmp21 + _tmp26 - _tmp27; - const Scalar _tmp29 = _tmp12 * _tmp21; - const Scalar _tmp30 = _tmp19 - _tmp2 * _tmp3 + _tmp29; - const Scalar _tmp31 = _tmp12 * _tmp3; - const Scalar _tmp32 = _tmp14 * _tmp21 + _tmp27 - _tmp31; - const Scalar _tmp33 = _tmp0 * _tmp18 + _tmp24 + _tmp8; - const Scalar _tmp34 = _tmp2 * _tmp21 - _tmp26 + _tmp31; - const Scalar _tmp35 = _tmp14 * _tmp3 + _tmp20 + _tmp29; - const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp35; - const Scalar _tmp37 = _tmp16 - _tmp18 * _tmp9 + _tmp22; - const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp34; - - // Output terms (1) - if (q_cov_lower_triangle != nullptr) { - matrix::Matrix& _q_cov_lower_triangle = (*q_cov_lower_triangle); - - _q_cov_lower_triangle(0, 0) = -_tmp0 * (-_tmp0 * _tmp2 - _tmp13 + _tmp8) - - _tmp3 * (-_tmp18 * _tmp3 - _tmp19 + _tmp20) - - _tmp9 * (-_tmp14 * _tmp9 - _tmp16 + _tmp17); - _q_cov_lower_triangle(1, 0) = -_tmp0 * _tmp23 - _tmp25 * _tmp9 - _tmp28 * _tmp3; - _q_cov_lower_triangle(2, 0) = -_tmp0 * _tmp30 - _tmp3 * _tmp33 - _tmp32 * _tmp9; - _q_cov_lower_triangle(3, 0) = -_tmp0 * _tmp34 - _tmp3 * _tmp37 - _tmp36 * state(2, 0); - _q_cov_lower_triangle(0, 1) = 0; - _q_cov_lower_triangle(1, 1) = -_tmp0 * _tmp25 + _tmp21 * _tmp28 + _tmp23 * _tmp9; - _q_cov_lower_triangle(2, 1) = -_tmp0 * _tmp32 + _tmp21 * _tmp33 + _tmp30 * _tmp9; - _q_cov_lower_triangle(3, 1) = -_tmp0 * _tmp35 + _tmp21 * _tmp37 + _tmp34 * _tmp9; - _q_cov_lower_triangle(0, 2) = 0; - _q_cov_lower_triangle(1, 2) = 0; - _q_cov_lower_triangle(2, 2) = _tmp0 * _tmp33 + _tmp21 * _tmp32 - _tmp3 * _tmp30; - _q_cov_lower_triangle(3, 2) = _tmp0 * _tmp37 + _tmp36 * state(0, 0) - _tmp38 * state(1, 0); - _q_cov_lower_triangle(0, 3) = 0; - _q_cov_lower_triangle(1, 3) = 0; - _q_cov_lower_triangle(2, 3) = 0; - _q_cov_lower_triangle(3, 3) = _tmp36 * state(1, 0) - _tmp37 * _tmp9 + _tmp38 * state(0, 0); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py index 7ca6b8f733..c1035a4bf6 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py @@ -68,7 +68,7 @@ def getAllData(logfile): getData(log, 'vehicle_attitude', 'q[3]')]) t_q = ms2s(getData(log, 'vehicle_attitude', 'timestamp')) - gnss_h = getData(log, 'vehicle_gps_position', 'alt') * 1e-3 + gnss_h = getData(log, 'vehicle_gps_position', 'altitude_msl_m') t_gnss = ms2s(getData(log, 'vehicle_gps_position', 'timestamp')) (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias) diff --git a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt index 34ac465f48..8e93d98669 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt +++ b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt @@ -1,5 +1,6 @@ matplotlib==3.5.1 numpy==1.22.2 +numpy==1.21.5 +numpy_quaternion==2022.4.3 pyulog==0.9.0 -quaternion==3.5.2.post4 scipy==1.8.0 diff --git a/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py b/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py deleted file mode 100644 index c79cfd1aed..0000000000 --- a/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- -""" -Created on Tue Oct 29 14:11:58 2019 - -@author: roman -""" - -from sympy import * - -################## Here are the variables you can change to see the effects on the cov matrix ########################### -yaw_init = 0.5 - -# ground speed in body frame (comes from ekf2) -groundspeed_body_x_init = 5 -groundspeed_body_y_init = 5 - -# true airspeed measured by pitot tube -V_init = 7 - -# heading variance -R_yaw_init = rad(15.0)**2 - -# sideslip variance -R_beta_init = rad(15.0)**2 - -# True airspeed measurement variance -R_tas_init = 1.4**2 - -######################################################################################################################### - -# define symbols: true airspeed, sidslip angle, -V, beta, yaw, groundspeed_body_x, groundspeed_body_y = symbols('V beta yaw vx_body vy_body') -R_tas, R_beta, R_yaw = symbols('R_tas R_beta R_yaw') - - -# body x/y component of relative wind vector ( V is what the airspeed sensor measures) -Vx = V * cos(beta) -Vy = V * sin(beta) - - -# wind in body frame -wind_body_x = groundspeed_body_x - Vx -wind_body_y = groundspeed_body_y - Vy - -# wind in earth frame -wind_n = cos(yaw) * wind_body_x - sin(yaw) * wind_body_y -wind_e = sin(yaw) * wind_body_x + cos(yaw) * wind_body_y -wind_earth = Matrix([wind_n, wind_e]) - -# jacobian of earth wind vector with respect to states with known uncertainties -G = wind_earth.jacobian([V, beta, yaw]) - -# initial covariance matrix -P = Matrix([[R_tas, 0, 0], [0, R_beta,0], [0,0,R_yaw]]) - -# earth wind covariance matrix, assume 0 sideslip angle -P_wind_earth = (G*P*G.T).subs([(beta, 0)]) - -P_wind_earth_numeric = P_wind_earth.subs([(V, V_init),(yaw, yaw_init), (R_tas, R_tas_init), (R_yaw, R_yaw_init), (R_beta, R_beta_init)]) -P_wind_earth_numeric = P_wind_earth_numeric.subs([(groundspeed_body_x, groundspeed_body_x_init), (groundspeed_body_y, groundspeed_body_y_init) ]) - - -print('P[22][22] = ' + str(P_wind_earth_numeric[0,0])) -print('P[22][23] = ' + str(P_wind_earth_numeric[0,1])) -print('P[23][22] = ' + str(P_wind_earth_numeric[1,0])) -print('P[23][23] = ' + str(P_wind_earth_numeric[1,1])) diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index 0d8daee7de..453da560b4 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -37,8 +37,12 @@ #include "range_finder_consistency_check.hpp" -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us) +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us) { + if (horizontal_motion) { + _time_last_horizontal_motion = time_us; + } + const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; if ((_time_last_update_us == 0) @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { + if (fabsf(vz) < _min_vz_for_valid_consistency) { + // We can only check consistency if there is vertical motion + return; + } + if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { - _is_kinematically_consistent = false; - _time_last_inconsistent_us = time_us; + if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { + _is_kinematically_consistent = false; + _time_last_inconsistent_us = time_us; + } } else { - if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { + if (_test_ratio < 1.f + && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { _is_kinematically_consistent = true; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp index 1498a4aa6c..d031e12d97 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -37,7 +37,8 @@ * using the estimated velocity as a reference in order to detect sensor faults */ -#pragma once +#ifndef EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP +#define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP #include @@ -47,7 +48,7 @@ public: RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; - void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us); + void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us); void setGate(float gate) { _gate = gate; } @@ -71,9 +72,12 @@ private: bool _is_kinematically_consistent{true}; uint64_t _time_last_inconsistent_us{}; + uint64_t _time_last_horizontal_motion{}; static constexpr float _signed_test_ratio_tau = 2.f; static constexpr float _min_vz_for_valid_consistency = .5f; static constexpr uint64_t _consistency_hyst_time_us = 1e6; }; + +#endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 88a9cb7197..177b841393 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion() const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - // Run the kinematic consistency check when not moving horizontally - if (_control_status.flags.in_air && !_control_status.flags.fixed_wing - && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + if (_control_status.flags.in_air) { + const bool horizontal_motion = _control_status.flags.fixed_wing + || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us); } } else { @@ -117,7 +117,7 @@ void Ekf::controlRangeHeightFusion() if (measurement_valid && _range_sensor.isDataHealthy()) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding @@ -131,8 +131,6 @@ void Ekf::controlRangeHeightFusion() && _range_sensor.isRegularlySendingData(); if (_control_status.flags.rng_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { fuseVerticalPosition(aid_src); @@ -204,6 +202,7 @@ void Ekf::controlRangeHeightFusion() bool Ekf::isConditionalRangeAidSuitable() { +#if defined(CONFIG_EKF2_TERRAIN) if (_control_status.flags.in_air && _range_sensor.isHealthy() && isTerrainEstimateValid()) { @@ -212,7 +211,10 @@ bool Ekf::isConditionalRangeAidSuitable() float range_hagl_max = _params.max_hagl_for_range_aid; float max_vel_xy = _params.max_vel_for_range_aid; - const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)); + const float hagl_innov = _aid_src_terrain_range_finder.innovation; + const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; + + const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var)); bool is_hagl_stable = (hagl_test_ratio < 1.f); @@ -234,6 +236,7 @@ bool Ekf::isConditionalRangeAidSuitable() return is_in_range && is_hagl_stable && is_below_max_speed; } +#endif // CONFIG_EKF2_TERRAIN return false; } diff --git a/src/modules/ekf2/EKF/sensor_range_finder.cpp b/src/modules/ekf2/EKF/sensor_range_finder.cpp index 94d3b891ea..54bd752598 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/sensor_range_finder.hpp index 94ed284613..8523ae3306 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 47063d5f0f..939013f363 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -42,8 +42,8 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h" -#include "python/ekf_derivation/generated/compute_sideslip_h_and_k.h" +#include +#include #include @@ -87,15 +87,13 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const float innov = 0.f; float innov_var = 0.f; - sym::ComputeSideslipInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, R, FLT_EPSILON, &innov, &innov_var); + sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var); sideslip.observation = 0.f; sideslip.observation_variance = R; sideslip.innovation = innov; sideslip.innovation_variance = innov_var; - sideslip.fusion_enabled = _control_status.flags.fuse_aspd; - sideslip.timestamp_sample = _time_delayed_us; const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); @@ -135,15 +133,15 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; - Vector24f H; // Observation jacobian - Vector24f K; // Kalman gain vector + VectorState H; // Observation jacobian + VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); + sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); if (update_wind_only) { - for (unsigned row = 0; row <= 21; row++) { - K(row) = 0.f; - } + const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); + K.setZero(); + K.slice(State::wind_vel.idx, 0) = K_wind; } const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation); diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 657b989f48..62cf8367c3 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -49,7 +49,9 @@ void Ekf::initHagl() stopHaglFlowFusion(); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_RANGE_FINDER) stopHaglRngFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER // assume a ground clearance _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; @@ -68,10 +70,14 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) predictHagl(imu_delayed); +#if defined(CONFIG_EKF2_RANGE_FINDER) controlHaglRngFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) controlHaglFlowFusion(); #endif // CONFIG_EKF2_OPTICAL_FLOW + controlHaglFakeFusion(); // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) @@ -95,6 +101,7 @@ void Ekf::predictHagl(const imuSample &imu_delayed) _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); } +#if defined(CONFIG_EKF2_RANGE_FINDER) void Ekf::controlHaglRngFusion() { if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) @@ -104,20 +111,29 @@ void Ekf::controlHaglRngFusion() return; } + if (_range_sensor.isDataReady()) { + updateHaglRng(_aid_src_terrain_range_finder); + } + if (_range_sensor.isDataHealthy()) { - const bool continuing_conditions_passing = _control_status.flags.in_air && _rng_consistency_check.isKinematicallyConsistent(); - //const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height - const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f); + + const bool continuing_conditions_passing = _control_status.flags.in_air + //&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height + && _rng_consistency_check.isKinematicallyConsistent(); + + const bool starting_conditions_passing = continuing_conditions_passing + && _range_sensor.isRegularlySendingData() + && (_rng_consistency_check.getTestRatio() < 1.f); _time_last_healthy_rng_data = _time_delayed_us; if (_hagl_sensor_status.flags.range_finder) { if (continuing_conditions_passing) { - fuseHaglRng(); + fuseHaglRng(_aid_src_terrain_range_finder); // We have been rejecting range data for too long const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); - const bool is_fusion_failing = isTimedOut(_time_last_hagl_fuse, timeout); + const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout); if (is_fusion_failing) { if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) { @@ -143,7 +159,16 @@ void Ekf::controlHaglRngFusion() } else { if (starting_conditions_passing) { - startHaglRngFusion(); + _hagl_sensor_status.flags.range_finder = true; + + // Reset the state to the measurement only if the test ratio is large, + // otherwise let it converge through the fusion + if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) { + fuseHaglRng(_aid_src_terrain_range_finder); + + } else { + resetHaglRng(); + } } } @@ -153,42 +178,9 @@ void Ekf::controlHaglRngFusion() } } -void Ekf::startHaglRngFusion() +float Ekf::getRngVar() const { - _hagl_sensor_status.flags.range_finder = true; - resetHaglRngIfNeeded(); -} - -void Ekf::resetHaglRngIfNeeded() -{ - if (_hagl_sensor_status.flags.flow) { - const float meas_hagl = _range_sensor.getDistBottom(); - const float pred_hagl = _terrain_vpos - _state.pos(2); - const float hagl_innov = pred_hagl - meas_hagl; - const float obs_variance = getRngVar(); - - const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var); - - // Reset the state to the measurement only if the test ratio is large, - // otherwise let it converge through the fusion - if (hagl_test_ratio > 0.2f) { - resetHaglRng(); - - } else { - fuseHaglRng(); - } - - } else { - resetHaglRng(); - } -} - -float Ekf::getRngVar() -{ - return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) + return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getRange()); } @@ -198,26 +190,25 @@ void Ekf::resetHaglRng() _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; - _time_last_hagl_fuse = _time_delayed_us; - _time_last_healthy_rng_data = 0; + + _aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us; } void Ekf::stopHaglRngFusion() { if (_hagl_sensor_status.flags.range_finder) { + ECL_INFO("stopping HAGL range fusion"); + + // reset flags + resetEstimatorAidStatus(_aid_src_terrain_range_finder); - _hagl_innov = 0.f; - _hagl_innov_var = 0.f; - _hagl_test_ratio = 0.f; _innov_check_fail_status.flags.reject_hagl = false; _hagl_sensor_status.flags.range_finder = false; } - - _time_last_healthy_rng_data = 0; } -void Ekf::fuseHaglRng() +void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const { // get a height above ground measurement from the range finder assuming a flat earth const float meas_hagl = _range_sensor.getDistBottom(); @@ -226,33 +217,55 @@ void Ekf::fuseHaglRng() const float pred_hagl = _terrain_vpos - _state.pos(2); // calculate the innovation - _hagl_innov = pred_hagl - meas_hagl; + const float hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty const float obs_variance = getRngVar(); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion - _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); // perform an innovation consistency check and only fuse data if it passes - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f); - if (_hagl_test_ratio <= 1.0f) { + + aid_src.timestamp_sample = _time_delayed_us; // TODO + + aid_src.observation = pred_hagl; + aid_src.observation_variance = obs_variance; + + aid_src.innovation = hagl_innov; + aid_src.innovation_variance = hagl_innov_var; + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + aid_src.fused = false; +} + +void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src) +{ + if (!aid_src.innovation_rejected) { // calculate the Kalman gain - const float gain = _terrain_var / _hagl_innov_var; + const float gain = _terrain_var / aid_src.innovation_variance; + // correct the state - _terrain_vpos -= gain * _hagl_innov; + _terrain_vpos -= gain * aid_src.innovation; + // correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); + // record last successful fusion event - _time_last_hagl_fuse = _time_delayed_us; _innov_check_fail_status.flags.reject_hagl = false; + aid_src.time_last_fuse = _time_delayed_us; + aid_src.fused = true; + } else { _innov_check_fail_status.flags.reject_hagl = true; + aid_src.fused = false; } } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) void Ekf::controlHaglFlowFusion() @@ -266,10 +279,10 @@ void Ekf::controlHaglFlowFusion() updateOptFlow(_aid_src_terrain_optical_flow); const bool continuing_conditions_passing = _control_status.flags.in_air - && !_control_status.flags.opt_flow - && _control_status.flags.gps - && isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead? - /* && !_hagl_sensor_status.flags.range_finder; */ + && !_control_status.flags.opt_flow + && _control_status.flags.gps + && !_hagl_sensor_status.flags.range_finder; + const bool starting_conditions_passing = continuing_conditions_passing; if (_hagl_sensor_status.flags.flow) { @@ -277,10 +290,9 @@ void Ekf::controlHaglFlowFusion() // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon fuseFlowForTerrain(_aid_src_terrain_optical_flow); - _flow_data_ready = false; // TODO: do something when failing continuously the innovation check - /* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */ + /* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */ /* if (is_fusion_failing) { */ /* resetHaglFlow(); */ @@ -292,28 +304,25 @@ void Ekf::controlHaglFlowFusion() } else { if (starting_conditions_passing) { - startHaglFlowFusion(); + _hagl_sensor_status.flags.flow = true; + // TODO: do a reset instead of trying to fuse the data? + fuseFlowForTerrain(_aid_src_terrain_optical_flow); } } - } else if (_hagl_sensor_status.flags.flow - && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { + _flow_data_ready = false; + + } else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { // No data anymore. Stop until it comes back. stopHaglFlowFusion(); } } -void Ekf::startHaglFlowFusion() -{ - _hagl_sensor_status.flags.flow = true; - // TODO: do a reset instead of trying to fuse the data? - fuseFlowForTerrain(_aid_src_terrain_optical_flow); - _flow_data_ready = false; -} - void Ekf::stopHaglFlowFusion() { if (_hagl_sensor_status.flags.flow) { + ECL_INFO("stopping HAGL flow fusion"); + _hagl_sensor_status.flags.flow = false; resetEstimatorAidStatus(_aid_src_terrain_optical_flow); } @@ -325,11 +334,13 @@ void Ekf::resetHaglFlow() _terrain_vpos = fmaxf(0.0f, _state.pos(2)); _terrain_var = 100.0f; _terrain_vpos_reset_counter++; + + _aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us; } void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) { - flow.fusion_enabled = true; + flow.fused = true; const float R_LOS = flow.observation_variance[0]; @@ -397,9 +408,8 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; - _time_last_flow_terrain_fuse = _time_delayed_us; - //_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain? - _aid_src_optical_flow.fused = true; + flow.time_last_fuse = _time_delayed_us; + flow.fused = true; } #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -416,19 +426,27 @@ void Ekf::controlHaglFakeFusion() bool Ekf::isTerrainEstimateValid() const { #if defined(CONFIG_EKF2_RANGE_FINDER) + // we have been fusing range finder measurements in the last 5 seconds - if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) { + if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) { return true; } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) + // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds // this can only be the case if the main filter does not fuse optical flow - if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) { + if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) { return true; } + #endif // CONFIG_EKF2_OPTICAL_FLOW return false; } + +void Ekf::terrainHandleVerticalPositionReset(const float delta_z) { + _terrain_vpos += delta_z; +} diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 17a57518b6..e615dc2d20 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -53,7 +53,8 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i]; + const int state_index = State::vel.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; } setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -71,7 +72,8 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i]; + const int state_index = State::vel.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; } setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -96,7 +98,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa aid_src.innovation = _state.pos(2) - aid_src.observation; aid_src.observation_variance = math::max(sq(0.01f), obs_var); - aid_src.innovation_variance = P(9, 9) + aid_src.observation_variance; + aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -121,7 +123,8 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(7 + i, 7 + i) + aid_src.observation_variance[i]; + const int state_index = State::pos.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; } setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -131,10 +134,10 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) { - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { // vx, vy - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -147,11 +150,11 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { // vx, vy, vz - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) - && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], 2) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) + && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -165,9 +168,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 3) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 4) + if (!aid_src.innovation_rejected) { + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -181,8 +184,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { - if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, 5)) { + if (!aid_src.innovation_rejected) { + if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; } @@ -190,29 +193,28 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) } // Helper function that fuses a single velocity or position measurement -bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) +bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index) { - Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state + VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < _k_num_states; row++) { + for (int row = 0; row < State::size; row++) { Kfusion(row) = P(row, state_index) / innov_var; } clearInhibitedStateKalmanGains(Kfusion); - SquareMatrix24f KHP; + SquareMatrixState KHP; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { + for (unsigned row = 0; row < State::size; row++) { + for (unsigned column = 0; column < State::size; column++) { KHP(row, column) = Kfusion(row) * P(state_index, column); } } const bool healthy = checkAndFixCovarianceUpdate(KHP); - setVelPosStatus(obs_index, healthy); + setVelPosStatus(state_index, healthy); if (healthy) { // apply the covariance corrections @@ -229,10 +231,10 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o return false; } -void Ekf::setVelPosStatus(const int index, const bool healthy) +void Ekf::setVelPosStatus(const int state_index, const bool healthy) { - switch (index) { - case 0: + switch (state_index) { + case State::vel.idx: if (healthy) { _fault_status.flags.bad_vel_N = false; _time_last_hor_vel_fuse = _time_delayed_us; @@ -243,7 +245,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 1: + case State::vel.idx + 1: if (healthy) { _fault_status.flags.bad_vel_E = false; _time_last_hor_vel_fuse = _time_delayed_us; @@ -254,7 +256,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 2: + case State::vel.idx + 2: if (healthy) { _fault_status.flags.bad_vel_D = false; _time_last_ver_vel_fuse = _time_delayed_us; @@ -265,7 +267,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 3: + case State::pos.idx: if (healthy) { _fault_status.flags.bad_pos_N = false; _time_last_hor_pos_fuse = _time_delayed_us; @@ -276,7 +278,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 4: + case State::pos.idx + 1: if (healthy) { _fault_status.flags.bad_pos_E = false; _time_last_hor_pos_fuse = _time_delayed_us; @@ -287,7 +289,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 5: + case State::pos.idx + 2: if (healthy) { _fault_status.flags.bad_pos_D = false; _time_last_hgt_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp new file mode 100644 index 0000000000..ddab6ee634 --- /dev/null +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "ekf.h" + +#include +#include + +#include + +// update quaternion states and covariances using the yaw innovation and yaw observation variance +bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) +{ + VectorState H_YAW; + computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); + + return fuseYaw(aid_src_status, H_YAW); +} + +bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW) +{ + // define the innovation gate size + float gate_sigma = math::max(_params.heading_innov_gate, 1.f); + + // innovation test ratio + setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); + + // check if the innovation variance calculation is badly conditioned + if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { + // the innovation variance contribution from the state covariances is not negative, no fault + _fault_status.flags.bad_hdg = false; + + } else { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.flags.bad_hdg = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + ECL_ERR("yaw fusion numerical error - covariance reset"); + + return false; + } + + // calculate the Kalman gains + // only calculate gains for states we are using + VectorState Kfusion; + const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; + + for (uint8_t row = 0; row < State::size; row++) { + for (uint8_t col = 0; col <= 3; col++) { + Kfusion(row) += P(row, col) * H_YAW(col); + } + + Kfusion(row) *= heading_innov_var_inv; + } + + // set the magnetometer unhealthy if the test fails + if (aid_src_status.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; + + // if we are in air we don't want to fuse the measurement + // we allow to use it when on the ground because the large innovation could be caused + // by interference or a large initial gyro bias + if (!_control_status.flags.in_air + && isTimedOut(_time_last_in_air, (uint64_t)5e6) + && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) + ) { + // constrain the innovation to the maximum set by the gate + // we need to delay this forced fusion to avoid starting it + // immediately after touchdown, when the drone is still armed + float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); + aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); + + // also reset the yaw gyro variance to converge faster and avoid + // being stuck on a previous bad estimate + resetGyroBiasZCov(); + + } else { + return false; + } + + } else { + _innov_check_fail_status.flags.reject_yaw = false; + } + + if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { + + _time_last_heading_fuse = _time_delayed_us; + + aid_src_status.time_last_fuse = _time_delayed_us; + aid_src_status.fused = true; + + _fault_status.flags.bad_hdg = false; + + return true; + + } else { + _fault_status.flags.bad_hdg = true; + } + + // otherwise + aid_src_status.fused = false; + return false; +} + +void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const +{ + if (shouldUse321RotationSequence(_R_to_earth)) { + sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + + } else { + sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + } +} diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 16cc545819..1bae77c7c8 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -56,13 +56,12 @@ void Ekf::controlZeroInnovationHeadingUpdate() aid_src_status.observation_variance = obs_var; aid_src_status.innovation = 0.f; - Vector24f H_YAW; + VectorState H_YAW; computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { // The yaw variance is too large, fuse fake measurement - aid_src_status.fusion_enabled = true; fuseYaw(aid_src_status, H_YAW); } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b5fd0e8675..b2c56bdf83 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -56,13 +56,12 @@ 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)), +#if defined(CONFIG_EKF2_WIND) _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)), +#endif // CONFIG_EKF2_WIND _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), _param_ekf2_imu_ctrl(_params->imu_ctrl), - _param_ekf2_mag_delay(_params->mag_delay_ms), - _param_ekf2_baro_delay(_params->baro_delay_ms), - _param_ekf2_gps_delay(_params->gps_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), #endif // CONFIG_EKF2_AUXVEL @@ -70,18 +69,46 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_acc_noise(_params->accel_noise), _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise), _param_ekf2_acc_b_noise(_params->accel_bias_p_noise), - _param_ekf2_mag_e_noise(_params->mage_p_noise), - _param_ekf2_mag_b_noise(_params->magb_p_noise), +#if defined(CONFIG_EKF2_WIND) _param_ekf2_wind_nsd(_params->wind_vel_nsd), +#endif // CONFIG_EKF2_WIND + _param_ekf2_noaid_noise(_params->pos_noaid_noise), +#if defined(CONFIG_EKF2_GNSS) + _param_ekf2_gps_ctrl(_params->gnss_ctrl), + _param_ekf2_gps_delay(_params->gps_delay_ms), + _param_ekf2_gps_pos_x(_params->gps_pos_body(0)), + _param_ekf2_gps_pos_y(_params->gps_pos_body(1)), + _param_ekf2_gps_pos_z(_params->gps_pos_body(2)), _param_ekf2_gps_v_noise(_params->gps_vel_noise), _param_ekf2_gps_p_noise(_params->gps_pos_noise), - _param_ekf2_noaid_noise(_params->pos_noaid_noise), + _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), + _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), + _param_ekf2_gps_check(_params->gps_check_mask), + _param_ekf2_req_eph(_params->req_hacc), + _param_ekf2_req_epv(_params->req_vacc), + _param_ekf2_req_sacc(_params->req_sacc), + _param_ekf2_req_nsats(_params->req_nsats), + _param_ekf2_req_pdop(_params->req_pdop), + _param_ekf2_req_hdrift(_params->req_hdrift), + _param_ekf2_req_vdrift(_params->req_vdrift), + _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default), +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_BAROMETER) + _param_ekf2_baro_ctrl(_params->baro_ctrl), + _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_baro_noise(_params->baro_noise), _param_ekf2_baro_gate(_params->baro_innov_gate), _param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone), _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt), - _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), - _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + _param_ekf2_aspd_max(_params->max_correction_airspeed), + _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp), + _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn), + _param_ekf2_pcoef_yp(_params->static_pressure_coef_yp), + _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), + _param_ekf2_pcoef_z(_params->static_pressure_coef_z), +# endif // CONFIG_EKF2_BARO_COMPENSATION +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) _param_ekf2_asp_delay(_params->airspeed_delay_ms), _param_ekf2_tas_gate(_params->tas_innov_gate), @@ -93,6 +120,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_beta_noise(_params->beta_noise), _param_ekf2_fuse_beta(_params->beta_fusion_enabled), #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_MAGNETOMETER) + _param_ekf2_mag_delay(_params->mag_delay_ms), + _param_ekf2_mag_e_noise(_params->mage_p_noise), + _param_ekf2_mag_b_noise(_params->magb_p_noise), _param_ekf2_head_noise(_params->mag_heading_noise), _param_ekf2_mag_noise(_params->mag_noise), _param_ekf2_mag_decl(_params->mag_declination_deg), @@ -102,25 +133,27 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_type(_params->mag_fusion_type), _param_ekf2_mag_acclim(_params->mag_acc_gate), _param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate), - _param_ekf2_gps_check(_params->gps_check_mask), - _param_ekf2_req_eph(_params->req_hacc), - _param_ekf2_req_epv(_params->req_vacc), - _param_ekf2_req_sacc(_params->req_sacc), - _param_ekf2_req_nsats(_params->req_nsats), - _param_ekf2_req_pdop(_params->req_pdop), - _param_ekf2_req_hdrift(_params->req_hdrift), - _param_ekf2_req_vdrift(_params->req_vdrift), + _param_ekf2_mag_check(_params->mag_check), + _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), + _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), + _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), +#endif // CONFIG_EKF2_MAGNETOMETER _param_ekf2_hgt_ref(_params->height_sensor_ref), - _param_ekf2_baro_ctrl(_params->baro_ctrl), - _param_ekf2_gps_ctrl(_params->gnss_ctrl), _param_ekf2_noaid_tout(_params->valid_timeout_max), +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + _param_ekf2_min_rng(_params->rng_gnd_clearance), +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_TERRAIN) + _param_ekf2_terr_mask(_params->terrain_fusion_mode), + _param_ekf2_terr_noise(_params->terrain_p_noise), + _param_ekf2_terr_grad(_params->terrain_gradient), +#endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) _param_ekf2_rng_ctrl(_params->rng_ctrl), _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_noise(_params->range_noise), _param_ekf2_rng_sfe(_params->range_noise_scaler), _param_ekf2_rng_gate(_params->range_innov_gate), - _param_ekf2_min_rng(_params->rng_gnd_clearance), _param_ekf2_rng_pitch(_params->rng_sens_pitch), _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), @@ -130,9 +163,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), - _param_ekf2_terr_mask(_params->terrain_fusion_mode), - _param_ekf2_terr_noise(_params->terrain_p_noise), - _param_ekf2_terr_grad(_params->terrain_gradient), #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _param_ekf2_ev_delay(_params->ev_delay_ms), @@ -147,7 +177,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_ev_pos_y(_params->ev_pos_body(1)), _param_ekf2_ev_pos_z(_params->ev_pos_body(2)), #endif // CONFIG_EKF2_EXTERNAL_VISION - _param_ekf2_grav_noise(_params->gravity_noise), #if defined(CONFIG_EKF2_OPTICAL_FLOW) _param_ekf2_of_ctrl(_params->flow_ctrl), _param_ekf2_of_delay(_params->flow_delay_ms), @@ -160,20 +189,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_of_pos_y(_params->flow_pos_body(1)), _param_ekf2_of_pos_z(_params->flow_pos_body(2)), #endif // CONFIG_EKF2_OPTICAL_FLOW - _param_ekf2_imu_pos_x(_params->imu_pos_body(0)), - _param_ekf2_imu_pos_y(_params->imu_pos_body(1)), - _param_ekf2_imu_pos_z(_params->imu_pos_body(2)), - _param_ekf2_gps_pos_x(_params->gps_pos_body(0)), - _param_ekf2_gps_pos_y(_params->gps_pos_body(1)), - _param_ekf2_gps_pos_z(_params->gps_pos_body(2)), - _param_ekf2_gbias_init(_params->switch_on_gyro_bias), - _param_ekf2_abias_init(_params->switch_on_accel_bias), - _param_ekf2_angerr_init(_params->initial_tilt_err), - _param_ekf2_abl_lim(_params->acc_bias_lim), - _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim), - _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim), - _param_ekf2_abl_tau(_params->acc_bias_learn_tc), - _param_ekf2_gyr_b_lim(_params->gyro_bias_lim), #if defined(CONFIG_EKF2_DRAG_FUSION) _param_ekf2_drag_ctrl(_params->drag_ctrl), _param_ekf2_drag_noise(_params->drag_noise), @@ -181,19 +196,20 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_bcoef_y(_params->bcoef_y), _param_ekf2_mcoef(_params->mcoef), #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - _param_ekf2_aspd_max(_params->max_correction_airspeed), - _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp), - _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn), - _param_ekf2_pcoef_yp(_params->static_pressure_coef_yp), - _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), - _param_ekf2_pcoef_z(_params->static_pressure_coef_z), -#endif // CONFIG_EKF2_BARO_COMPENSATION - _param_ekf2_mag_check(_params->mag_check), - _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), - _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), - _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), - _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + _param_ekf2_grav_noise(_params->gravity_noise), +#endif // CONFIG_EKF2_GRAVITY_FUSION + _param_ekf2_imu_pos_x(_params->imu_pos_body(0)), + _param_ekf2_imu_pos_y(_params->imu_pos_body(1)), + _param_ekf2_imu_pos_z(_params->imu_pos_body(2)), + _param_ekf2_gbias_init(_params->switch_on_gyro_bias), + _param_ekf2_abias_init(_params->switch_on_accel_bias), + _param_ekf2_angerr_init(_params->initial_tilt_err), + _param_ekf2_abl_lim(_params->acc_bias_lim), + _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim), + _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim), + _param_ekf2_abl_tau(_params->acc_bias_learn_tc), + _param_ekf2_gyr_b_lim(_params->gyro_bias_lim) { // advertise expected minimal topic set immediately to ensure logging _attitude_pub.advertise(); @@ -214,18 +230,24 @@ EKF2::~EKF2() perf_free(_ecl_ekf_update_perf); perf_free(_ecl_ekf_update_full_perf); perf_free(_msg_missed_imu_perf); +#if defined(CONFIG_EKF2_BAROMETER) perf_free(_msg_missed_air_data_perf); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) perf_free(_msg_missed_airspeed_perf); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) perf_free(_msg_missed_distance_sensor_perf); #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) perf_free(_msg_missed_gps_perf); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AUXVEL) perf_free(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) perf_free(_msg_missed_magnetometer_perf); +#endif // CONFIG_EKF2_MAGNETOMETER perf_free(_msg_missed_odometry_perf); #if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_free(_msg_missed_optical_flow_perf); @@ -248,7 +270,11 @@ bool EKF2::multi_init(int imu, int mag) _estimator_states_pub.advertise(); _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); +#if defined(CONFIG_EKF2_GNSS) _yaw_est_pub.advertise(); +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_BAROMETER) // baro advertise if (_param_ekf2_baro_ctrl.get()) { @@ -256,6 +282,8 @@ bool EKF2::multi_init(int imu, int mag) _estimator_baro_bias_pub.advertise(); } +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_EXTERNAL_VISION) // EV advertise @@ -279,6 +307,8 @@ bool EKF2::multi_init(int imu, int mag) #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + // GNSS advertise if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { _estimator_aid_src_gnss_hgt_pub.advertise(); @@ -294,13 +324,14 @@ bool EKF2::multi_init(int imu, int mag) _estimator_aid_src_gnss_vel_pub.advertise(); } -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { _estimator_aid_src_gnss_yaw_pub.advertise(); } -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -312,6 +343,7 @@ bool EKF2::multi_init(int imu, int mag) #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_MAGNETOMETER) // mag advertise if (_param_ekf2_mag_type.get() != MagFuseType::NONE) { @@ -319,13 +351,26 @@ bool EKF2::multi_init(int imu, int mag) _estimator_aid_src_mag_pub.advertise(); } +#endif // CONFIG_EKF2_MAGNETOMETER + _attitude_pub.advertise(); _local_position_pub.advertise(); _global_position_pub.advertise(); _odometry_pub.advertise(); - _wind_pub.advertise(); - bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); +#if defined(CONFIG_EKF2_WIND) + _wind_pub.advertise(); +#endif // CONFIG_EKF2_WIND + + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + + if (!_magnetometer_sub.ChangeInstance(mag)) { + changed_instance = false; + } + +#endif // CONFIG_EKF2_MAGNETOMETER const int status_instance = _estimator_states_pub.get_instance(); @@ -356,18 +401,24 @@ int EKF2::print_status() perf_print_counter(_ecl_ekf_update_perf); perf_print_counter(_ecl_ekf_update_full_perf); perf_print_counter(_msg_missed_imu_perf); +#if defined(CONFIG_EKF2_BAROMETER) perf_print_counter(_msg_missed_air_data_perf); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) perf_print_counter(_msg_missed_airspeed_perf); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) perf_print_counter(_msg_missed_distance_sensor_perf); #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) perf_print_counter(_msg_missed_gps_perf); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AUXVEL) perf_print_counter(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) perf_print_counter(_msg_missed_magnetometer_perf); +#endif // CONFIG_EKF2_MAGNETOMETER perf_print_counter(_msg_missed_odometry_perf); #if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_print_counter(_msg_missed_optical_flow_perf); @@ -400,7 +451,9 @@ void EKF2::Run() VerifyParams(); +#if defined(CONFIG_EKF2_GNSS) _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); +#endif // CONFIG_EKF2_GNSS const matrix::Vector3f imu_pos_body(_param_ekf2_imu_pos_x.get(), _param_ekf2_imu_pos_y.get(), @@ -419,6 +472,8 @@ void EKF2::Run() #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_BAROMETER) + // if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output if (_params->baro_ctrl == 1) { float sens_baro_rate = 0.f; @@ -435,6 +490,10 @@ void EKF2::Run() } } +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_MAGNETOMETER) + // if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output if (_params->mag_fusion_type != MagFuseType::NONE) { float sens_mag_rate = 0.f; @@ -450,6 +509,8 @@ void EKF2::Run() } } } + +#endif // CONFIG_EKF2_MAGNETOMETER } if (!_callback_registered) { @@ -670,15 +731,21 @@ void EKF2::Run() #if defined(CONFIG_EKF2_AUXVEL) UpdateAuxVelSample(ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) UpdateBaroSample(ekf2_timestamps); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) UpdateExtVisionSample(ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) UpdateFlowSample(ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) UpdateGpsSample(ekf2_timestamps); +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagSample(ekf2_timestamps); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) UpdateRangeSample(ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER @@ -703,31 +770,53 @@ void EKF2::Run() UpdateMagCalibration(now); // publish other state output used by the system not dependent on output predictor + + // update sensor calibration (in-run bias) before publishing sensor bias + UpdateAccelCalibration(now); + UpdateGyroCalibration(now); +#if defined(CONFIG_EKF2_MAGNETOMETER) + UpdateMagCalibration(now); +#endif // CONFIG_EKF2_MAGNETOMETER + PublishSensorBias(now); + +#if defined(CONFIG_EKF2_WIND) PublishWindEstimate(now); +#endif // CONFIG_EKF2_WIND // publish status/logging messages - PublishAidSourceStatus(now); - PublishBaroBias(now); - PublishGnssHgtBias(now); -#if defined(CONFIG_EKF2_RANGE_FINDER) - PublishRngHgtBias(now); -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - PublishEvPosBias(now); -#endif // CONFIG_EKF2_EXTERNAL_VISION PublishEventFlags(now); - PublishGpsStatus(now); + PublishStatusFlags(now); + PublishStatus(now); + + // verbose logging + PublishAidSourceStatus(now); PublishInnovations(now); PublishInnovationTestRatios(now); PublishInnovationVariances(now); + PublishStates(now); + +#if defined(CONFIG_EKF2_BAROMETER) + PublishBaroBias(now); +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_RANGE_FINDER) + PublishRngHgtBias(now); +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + PublishEvPosBias(now); +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + PublishGnssHgtBias(now); + PublishGpsStatus(now); + PublishYawEstimatorStatus(now); +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_OPTICAL_FLOW) PublishOpticalFlowVel(now); #endif // CONFIG_EKF2_OPTICAL_FLOW - PublishStates(now); - PublishStatus(now); - PublishStatusFlags(now); - PublishYawEstimatorStatus(now); } else { // ekf no update @@ -744,6 +833,8 @@ void EKF2::Run() void EKF2::VerifyParams() { +#if defined(CONFIG_EKF2_GNSS) + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS) || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) { _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS | @@ -768,6 +859,10 @@ void EKF2::VerifyParams() "GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get()); } +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_BAROMETER) + if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) { _param_ekf2_baro_ctrl.set(1); _param_ekf2_baro_ctrl.commit(); @@ -779,6 +874,8 @@ void EKF2::VerifyParams() "Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get()); } +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_RANGE_FINDER) if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) { @@ -794,6 +891,8 @@ void EKF2::VerifyParams() #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) + if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) { _param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL)); _param_ekf2_gps_ctrl.commit(); @@ -805,6 +904,8 @@ void EKF2::VerifyParams() "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get()); } +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV) @@ -923,6 +1024,7 @@ void EKF2::VerifyParams() #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_MAGNETOMETER) // EKF2_MAG_TYPE obsolete options if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO) @@ -940,6 +1042,8 @@ void EKF2::VerifyParams() _param_ekf2_mag_type.set(0); _param_ekf2_mag_type.commit(); } + +#endif // CONFIG_EKF2_MAGNETOMETER } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -952,8 +1056,15 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // sideslip PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_BAROMETER) // baro height PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_DRAG_FUSION) + // drag + PublishAidSourceStatus(_ekf.aid_src_drag(), _status_drag_pub_last, _estimator_aid_src_drag_pub); +#endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_RANGE_FINDER) // RNG height @@ -972,22 +1083,28 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) // GNSS hgt/pos/vel/yaw PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub); -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub); -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_MAGNETOMETER) // mag heading PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub); // mag 3d PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub); +#endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_AUXVEL) // aux velocity @@ -997,9 +1114,22 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + // range finder + PublishAidSourceStatus(_ekf.aid_src_terrain_range_finder(), _status_terrain_range_finder_pub_last, + _estimator_aid_src_terrain_range_finder_pub); +#endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + // optical flow PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last, _estimator_aid_src_terrain_optical_flow_pub); -#endif // CONFIG_EKF2_OPTICAL_FLOW +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -1022,6 +1152,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_BAROMETER) void EKF2::PublishBaroBias(const hrt_abstime ×tamp) { if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) { @@ -1035,7 +1166,9 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) { if (_ekf.get_gps_sample_delayed().time_us != 0) { @@ -1048,6 +1181,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) @@ -1201,7 +1335,11 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) _ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon); global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters +#if defined(CONFIG_EKF2_GNSS) global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); +#else + global_pos.alt_ellipsoid = global_pos.alt; +#endif // delta_alt, alt_reset_counter // global altitude has opposite sign of local down position @@ -1222,7 +1360,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt = NAN; global_pos.terrain_alt_valid = false; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) if (_ekf.isTerrainEstimateValid()) { // Terrain altitude in m, WGS84 @@ -1230,7 +1368,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt_valid = true; } -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning || _ekf.control_status_flags().wind_dead_reckoning; @@ -1240,6 +1378,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_GNSS) void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us; @@ -1274,45 +1413,100 @@ void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) _last_gps_status_published = timestamp_sample; } +#endif // CONFIG_EKF2_GNSS void EKF2::PublishInnovations(const hrt_abstime ×tamp) { // publish estimator innovation data estimator_innovations_s innovations{}; innovations.timestamp_sample = _ekf.time_delayed_us(); - _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos); + +#if defined(CONFIG_EKF2_GNSS) + // GPS + innovations.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation[0]; + innovations.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation[1]; + innovations.gps_vvel = _ekf.aid_src_gnss_vel().innovation[2]; + innovations.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation[0]; + innovations.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation[1]; + innovations.gps_vpos = _ekf.aid_src_gnss_hgt().innovation; +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); + // External Vision + innovations.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation[0]; + innovations.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation[1]; + innovations.ev_vvel = _ekf.aid_src_ev_vel().innovation[2]; + innovations.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation[0]; + innovations.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation[1]; + innovations.ev_vpos = _ekf.aid_src_ev_hgt().innovation; #endif // CONFIG_EKF2_EXTERNAL_VISION - _ekf.getBaroHgtInnov(innovations.baro_vpos); + + // Height sensors #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getRngHgtInnov(innovations.rng_vpos); + innovations.rng_vpos = _ekf.aid_src_rng_hgt().innovation; #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_BAROMETER) + innovations.baro_vpos = _ekf.aid_src_baro_hgt().innovation; +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_AUXVEL) - _ekf.getAuxVelInnov(innovations.aux_hvel); + // Auxiliary velocity + innovations.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation[0]; + innovations.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation[1]; #endif // CONFIG_EKF2_AUXVEL + #if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getFlowInnov(innovations.flow); - _ekf.getTerrainFlowInnov(innovations.terr_flow); + // Optical flow + innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0]; + innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1]; +# if defined(CONFIG_EKF2_TERRAIN) + innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0]; + innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1]; +# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW - _ekf.getHeadingInnov(innovations.heading); - _ekf.getMagInnov(innovations.mag_field); + + // heading + innovations.heading = _ekf.getHeadingInnov(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + // mag_field + innovations.mag_field[0] = _ekf.aid_src_mag().innovation[0]; + innovations.mag_field[1] = _ekf.aid_src_mag().innovation[1]; + innovations.mag_field[2] = _ekf.aid_src_mag().innovation[2]; +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + // gravity + innovations.gravity[0] = _ekf.aid_src_gravity().innovation[0]; + innovations.gravity[1] = _ekf.aid_src_gravity().innovation[1]; + innovations.gravity[2] = _ekf.aid_src_gravity().innovation[2]; +#endif // CONFIG_EKF2_GRAVITY_FUSION + #if defined(CONFIG_EKF2_DRAG_FUSION) - _ekf.getDragInnov(innovations.drag); + // drag + innovations.drag[0] = _ekf.aid_src_drag().innovation[0]; + innovations.drag[1] = _ekf.aid_src_drag().innovation[1]; #endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnov(innovations.airspeed); + // airspeed + innovations.airspeed = _ekf.aid_src_airspeed().innovation; #endif // CONFIG_EKF2_AIRSPEED + #if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnov(innovations.beta); + // beta + innovations.beta = _ekf.aid_src_sideslip().innovation; #endif // CONFIG_EKF2_SIDESLIP + +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) + // hagl + innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation; +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnov(innovations.hagl); - _ekf.getHaglRateInnov(innovations.hagl_rate); + // hagl_rate + innovations.hagl_rate = _ekf.getHaglRateInnov(); #endif // CONFIG_EKF2_RANGE_FINDER - _ekf.getGravityInnov(innovations.gravity); - // Not yet supported - innovations.aux_vvel = NAN; innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); @@ -1328,19 +1522,22 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); #if defined(CONFIG_EKF2_OPTICAL_FLOW) _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW) // set dist bottom to scale flow innovation const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2); _preflt_checker.setDistBottom(dist_bottom); -#endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); #endif // CONFIG_EKF2_EXTERNAL_VISION - +#if defined(CONFIG_EKF2_BAROMETER) _preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt); +#endif // CONFIG_EKF2_BAROMETER _preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt); #if defined(CONFIG_EKF2_RANGE_FINDER) _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); @@ -1357,40 +1554,93 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) // publish estimator innovation test ratio data estimator_innovations_s test_ratios{}; test_ratios.timestamp_sample = _ekf.time_delayed_us(); - _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], - test_ratios.gps_vpos); + +#if defined(CONFIG_EKF2_GNSS) + // GPS + test_ratios.gps_hvel[0] = _ekf.aid_src_gnss_vel().test_ratio[0]; + test_ratios.gps_hvel[1] = _ekf.aid_src_gnss_vel().test_ratio[1]; + test_ratios.gps_vvel = _ekf.aid_src_gnss_vel().test_ratio[2]; + test_ratios.gps_hpos[0] = _ekf.aid_src_gnss_pos().test_ratio[0]; + test_ratios.gps_hpos[1] = _ekf.aid_src_gnss_pos().test_ratio[1]; + test_ratios.gps_vpos = _ekf.aid_src_gnss_hgt().test_ratio; +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); + // External Vision + test_ratios.ev_hvel[0] = _ekf.aid_src_ev_vel().test_ratio[0]; + test_ratios.ev_hvel[1] = _ekf.aid_src_ev_vel().test_ratio[1]; + test_ratios.ev_vvel = _ekf.aid_src_ev_vel().test_ratio[2]; + test_ratios.ev_hpos[0] = _ekf.aid_src_ev_pos().test_ratio[0]; + test_ratios.ev_hpos[1] = _ekf.aid_src_ev_pos().test_ratio[1]; + test_ratios.ev_vpos = _ekf.aid_src_ev_hgt().test_ratio; #endif // CONFIG_EKF2_EXTERNAL_VISION - _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); + + // Height sensors #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); + test_ratios.rng_vpos = _ekf.aid_src_rng_hgt().test_ratio; #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_BAROMETER) + test_ratios.baro_vpos = _ekf.aid_src_baro_hgt().test_ratio; +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_AUXVEL) - _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); + // Auxiliary velocity + test_ratios.aux_hvel[0] = _ekf.aid_src_aux_vel().test_ratio[0]; + test_ratios.aux_hvel[1] = _ekf.aid_src_aux_vel().test_ratio[1]; #endif // CONFIG_EKF2_AUXVEL + #if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getFlowInnovRatio(test_ratios.flow[0]); - _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); + // Optical flow + test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0]; + test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1]; +# if defined(CONFIG_EKF2_TERRAIN) + test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0]; + test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1]; +# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW - _ekf.getHeadingInnovRatio(test_ratios.heading); - _ekf.getMagInnovRatio(test_ratios.mag_field[0]); + + // heading + test_ratios.heading = _ekf.getHeadingInnovRatio(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + // mag_field + test_ratios.mag_field[0] = _ekf.aid_src_mag().test_ratio[0]; + test_ratios.mag_field[1] = _ekf.aid_src_mag().test_ratio[1]; + test_ratios.mag_field[2] = _ekf.aid_src_mag().test_ratio[2]; +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + // gravity + test_ratios.gravity[0] = _ekf.aid_src_gravity().test_ratio[0]; + test_ratios.gravity[1] = _ekf.aid_src_gravity().test_ratio[1]; + test_ratios.gravity[2] = _ekf.aid_src_gravity().test_ratio[2]; +#endif // CONFIG_EKF2_GRAVITY_FUSION + #if defined(CONFIG_EKF2_DRAG_FUSION) - _ekf.getDragInnovRatio(&test_ratios.drag[0]); + // drag + test_ratios.drag[0] = _ekf.aid_src_drag().test_ratio[0]; + test_ratios.drag[1] = _ekf.aid_src_drag().test_ratio[1]; #endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnovRatio(test_ratios.airspeed); + // airspeed + test_ratios.airspeed = _ekf.aid_src_airspeed().test_ratio; #endif // CONFIG_EKF2_AIRSPEED + #if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnovRatio(test_ratios.beta); + // beta + test_ratios.beta = _ekf.aid_src_sideslip().test_ratio; #endif // CONFIG_EKF2_SIDESLIP + +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) + // hagl + test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio; +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnovRatio(test_ratios.hagl); - _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); + // hagl_rate + test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio(); #endif // CONFIG_EKF2_RANGE_FINDER - _ekf.getGravityInnovRatio(test_ratios.gravity[0]); - // Not yet supported - test_ratios.aux_vvel = NAN; test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovation_test_ratios_pub.publish(test_ratios); @@ -1401,39 +1651,93 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) // publish estimator innovation variance data estimator_innovations_s variances{}; variances.timestamp_sample = _ekf.time_delayed_us(); - _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos); + +#if defined(CONFIG_EKF2_GNSS) + // GPS + variances.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation_variance[0]; + variances.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation_variance[1]; + variances.gps_vvel = _ekf.aid_src_gnss_vel().innovation_variance[2]; + variances.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation_variance[0]; + variances.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation_variance[1]; + variances.gps_vpos = _ekf.aid_src_gnss_hgt().innovation_variance; +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); + // External Vision + variances.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation_variance[0]; + variances.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation_variance[1]; + variances.ev_vvel = _ekf.aid_src_ev_vel().innovation_variance[2]; + variances.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation_variance[0]; + variances.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation_variance[1]; + variances.ev_vpos = _ekf.aid_src_ev_hgt().innovation_variance; #endif // CONFIG_EKF2_EXTERNAL_VISION - _ekf.getBaroHgtInnovVar(variances.baro_vpos); + + // Height sensors #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getRngHgtInnovVar(variances.rng_vpos); + variances.rng_vpos = _ekf.aid_src_rng_hgt().innovation_variance; #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_BAROMETER) + variances.baro_vpos = _ekf.aid_src_baro_hgt().innovation_variance; +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_AUXVEL) - _ekf.getAuxVelInnovVar(variances.aux_hvel); + // Auxiliary velocity + variances.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation_variance[0]; + variances.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation_variance[1]; #endif // CONFIG_EKF2_AUXVEL + #if defined(CONFIG_EKF2_OPTICAL_FLOW) - _ekf.getFlowInnovVar(variances.flow); - _ekf.getTerrainFlowInnovVar(variances.terr_flow); + // Optical flow + variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0]; + variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1]; +# if defined(CONFIG_EKF2_TERRAIN) + variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0]; + variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1]; +# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW - _ekf.getHeadingInnovVar(variances.heading); - _ekf.getMagInnovVar(variances.mag_field); + + // heading + variances.heading = _ekf.getHeadingInnovVar(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + // mag_field + variances.mag_field[0] = _ekf.aid_src_mag().innovation_variance[0]; + variances.mag_field[1] = _ekf.aid_src_mag().innovation_variance[1]; + variances.mag_field[2] = _ekf.aid_src_mag().innovation_variance[2]; +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + // gravity + variances.gravity[0] = _ekf.aid_src_gravity().innovation_variance[0]; + variances.gravity[1] = _ekf.aid_src_gravity().innovation_variance[1]; + variances.gravity[2] = _ekf.aid_src_gravity().innovation_variance[2]; +#endif // CONFIG_EKF2_GRAVITY_FUSION + #if defined(CONFIG_EKF2_DRAG_FUSION) - _ekf.getDragInnovVar(variances.drag); + // drag + variances.drag[0] = _ekf.aid_src_drag().innovation_variance[0]; + variances.drag[1] = _ekf.aid_src_drag().innovation_variance[1]; #endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnovVar(variances.airspeed); + // airspeed + variances.airspeed = _ekf.aid_src_airspeed().innovation_variance; #endif // CONFIG_EKF2_AIRSPEED + #if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnovVar(variances.beta); + // beta + variances.beta = _ekf.aid_src_sideslip().innovation_variance; #endif // CONFIG_EKF2_SIDESLIP + +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) + // hagl + variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance; +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnovVar(variances.hagl); - _ekf.getHaglRateInnovVar(variances.hagl_rate); + // hagl_rate + variances.hagl_rate = _ekf.getHaglRateInnovVar(); #endif // CONFIG_EKF2_RANGE_FINDER - _ekf.getGravityInnovVar(variances.gravity); - // Not yet supported - variances.aux_vvel = NAN; variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovation_variances_pub.publish(variances); @@ -1495,13 +1799,16 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) _ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter); lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); + lpos.unaided_heading = _ekf.getUnaidedYaw(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); +#if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); +#endif // CONFIG_EKF2_TERRAIN _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); @@ -1563,10 +1870,10 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sa angular_velocity.copyTo(odom.angular_velocity); // velocity covariances - _ekf.velocity_covariances().diag().copyTo(odom.velocity_variance); + _ekf.getVelocityVariance().copyTo(odom.velocity_variance); // position covariances - _ekf.position_covariances().diag().copyTo(odom.position_variance); + _ekf.getPositionVariance().copyTo(odom.position_variance); // orientation covariance _ekf.calcRotVecVariances().copyTo(odom.orientation_variance); @@ -1587,12 +1894,17 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) // estimator_sensor_bias const Vector3f gyro_bias{_ekf.getGyroBias()}; const Vector3f accel_bias{_ekf.getAccelBias()}; - const Vector3f mag_bias{_ekf.getMagBias()}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) + const Vector3f mag_bias {_ekf.getMagBias()}; +#endif // CONFIG_EKF2_MAGNETOMETER // publish at ~1 Hz, or sooner if there's a change if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f) || (accel_bias - _last_accel_bias_published).longerThan(0.001f) +#if defined(CONFIG_EKF2_MAGNETOMETER) || (mag_bias - _last_mag_bias_published).longerThan(0.001f) +#endif // CONFIG_EKF2_MAGNETOMETER || (timestamp >= _last_sensor_bias_published + 1_s)) { estimator_sensor_bias_s bias{}; @@ -1623,6 +1935,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) _last_accel_bias_published = accel_bias; } +#if defined(CONFIG_EKF2_MAGNETOMETER) + if (_device_id_mag != 0) { const Vector3f bias_var{_ekf.getMagBiasVariance()}; @@ -1635,6 +1949,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) _last_mag_bias_published = mag_bias; } +#endif // CONFIG_EKF2_MAGNETOMETER + bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); @@ -1647,7 +1963,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) // publish estimator states estimator_states_s states; states.timestamp_sample = _ekf.time_delayed_us(); - states.n_states = Ekf::_k_num_states; + states.n_states = Ekf::getNumberOfStates(); _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); _ekf.covariances_diagonal().copyTo(states.covariances); states.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); @@ -1661,9 +1977,11 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); +#if defined(CONFIG_EKF2_GNSS) // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include // the GPS Fix bit, which is always checked) status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1); +#endif // CONFIG_EKF2_GNSS status.control_mode_flags = _ekf.control_status().value; status.filter_fault_flags = _ekf.fault_status().value; @@ -1697,12 +2015,17 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.accel_device_id = _device_id_accel; +#if defined(CONFIG_EKF2_BAROMETER) status.baro_device_id = _device_id_baro; +#endif // CONFIG_EKF2_BAROMETER status.gyro_device_id = _device_id_gyro; + +#if defined(CONFIG_EKF2_MAGNETOMETER) status.mag_device_id = _device_id_mag; _ekf.get_mag_checks(status.mag_inclination_deg, status.mag_inclination_ref_deg, status.mag_strength_gs, status.mag_strength_ref_gs); +#endif // CONFIG_EKF2_MAGNETOMETER status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_pub.publish(status); @@ -1776,6 +2099,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_gravity_vector = _ekf.control_status_flags().gravity_vector; status_flags.cs_mag = _ekf.control_status_flags().mag; status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault; + status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -1816,6 +2140,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_GNSS) void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) { static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, @@ -1835,7 +2160,9 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) _yaw_est_pub.publish(yaw_est_test_data); } } +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_WIND) void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) { if (_ekf.get_wind_status()) { @@ -1847,12 +2174,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); #if defined(CONFIG_EKF2_AIRSPEED) - _ekf.getAirspeedInnov(wind.tas_innov); - _ekf.getAirspeedInnovVar(wind.tas_innov_var); + wind.tas_innov = _ekf.aid_src_airspeed().innovation; + wind.tas_innov_var = _ekf.aid_src_airspeed().innovation_variance; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - _ekf.getBetaInnov(wind.beta_innov); - _ekf.getBetaInnovVar(wind.beta_innov_var); + wind.beta_innov = _ekf.aid_src_sideslip().innovation; + wind.beta_innov = _ekf.aid_src_sideslip().innovation_variance; #endif // CONFIG_EKF2_SIDESLIP wind.windspeed_north = wind_vel(0); @@ -1864,13 +2191,14 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) _wind_pub.publish(wind); } } +#endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_OPTICAL_FLOW) void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; - if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) { + if ((timestamp_sample != 0) && (timestamp_sample > _optical_flow_vel_pub_last)) { vehicle_optical_flow_vel_s flow_vel{}; flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; @@ -1884,13 +2212,20 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate); _ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral); + _ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias); + _ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro); + _ekf.getMeasuredBodyRate().copyTo(flow_vel.meas_gyro); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); + + _optical_flow_vel_pub_last = timestamp_sample; } } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) float EKF2::filter_altitude_ellipsoid(float amsl_hgt) { float height_diff = static_cast(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt; @@ -1911,6 +2246,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt) return amsl_hgt + _wgs84_hgt_offset; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AIRSPEED) void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2011,6 +2347,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) } #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF baro sample @@ -2054,6 +2391,7 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) (int64_t)ekf2_timestamps.timestamp / 100); } } +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2272,6 +2610,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF GPS message @@ -2315,7 +2654,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _gps_alttitude_ellipsoid = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)); } } +#endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) { const unsigned last_generation = _magnetometer_sub.get_last_generation(); @@ -2359,6 +2700,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) (int64_t)ekf2_timestamps.timestamp / 100); } } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2559,6 +2901,7 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) bias_valid, learning_valid); } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { const Vector3f mag_bias = _ekf.getMagBias(); @@ -2588,6 +2931,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_MAGNETOMETER int EKF2::custom_command(int argc, char *argv[]) { @@ -2627,6 +2971,7 @@ int EKF2::task_spawn(int argc, char *argv[]) imu_instances = imu_instances_limited; } +#if defined(CONFIG_EKF2_MAGNETOMETER) int32_t sens_mag_mode = 1; const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE"); param_get(param_sens_mag_mode, &sens_mag_mode); @@ -2657,6 +3002,8 @@ int EKF2::task_spawn(int argc, char *argv[]) } else { mag_instances = 1; } + +#endif // CONFIG_EKF2_MAGNETOMETER } if (multi_mode && !replay_mode) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 63f7fff540..88d1e59cbf 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -77,18 +76,14 @@ #include #include #include -#include #include #include #include -#include #include #include #include -#include #include #include -#include #include #if defined(CONFIG_EKF2_AIRSPEED) @@ -100,6 +95,19 @@ # include #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) +# include +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_GNSS) +# include +# include +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_MAGNETOMETER) +# include +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) # include # include @@ -109,6 +117,10 @@ # include #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_WIND) +# include +#endif // CONFIG_EKF2_WIND + extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -167,8 +179,10 @@ private: void PublishAidSourceStatus(const hrt_abstime ×tamp); void PublishAttitude(const hrt_abstime ×tamp); + +#if defined(CONFIG_EKF2_BAROMETER) void PublishBaroBias(const hrt_abstime ×tamp); - void PublishGnssHgtBias(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void PublishRngHgtBias(const hrt_abstime ×tamp); @@ -181,22 +195,18 @@ private: uint64_t timestamp, uint32_t device_id = 0); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); - void PublishGpsStatus(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp); void PublishInnovationTestRatios(const hrt_abstime ×tamp); void PublishInnovationVariances(const hrt_abstime ×tamp); void PublishLocalPosition(const hrt_abstime ×tamp); void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample); - void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - void PublishOpticalFlowVel(const hrt_abstime ×tamp); -#endif // CONFIG_EKF2_OPTICAL_FLOW void PublishSensorBias(const hrt_abstime ×tamp); void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); void PublishStatusFlags(const hrt_abstime ×tamp); +#if defined(CONFIG_EKF2_WIND) void PublishWindEstimate(const hrt_abstime ×tamp); - void PublishYawEstimatorStatus(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_AIRSPEED) void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); @@ -204,18 +214,34 @@ private: #if defined(CONFIG_EKF2_AUXVEL) void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + /* + * Calculate filtered WGS84 height from estimated AMSL height + */ + float filter_altitude_ellipsoid(float amsl_hgt); + + void PublishGpsStatus(const hrt_abstime ×tamp); + void PublishGnssHgtBias(const hrt_abstime ×tamp); + void PublishYawEstimatorStatus(const hrt_abstime ×tamp); + void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_OPTICAL_FLOW) bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); + void PublishOpticalFlowVel(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_OPTICAL_FLOW - void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); +#if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER + void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps); // Used to check, save and use learned accel/gyro/mag biases @@ -230,7 +256,9 @@ private: const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid); void UpdateAccelCalibration(const hrt_abstime ×tamp); void UpdateGyroCalibration(const hrt_abstime ×tamp); +#if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagCalibration(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_MAGNETOMETER // publish helper for estimator_aid_source topics template @@ -248,11 +276,6 @@ private: } } - /* - * Calculate filtered WGS84 height from estimated AMSL height - */ - float filter_altitude_ellipsoid(float amsl_hgt); - static constexpr float sq(float x) { return x * x; }; const bool _replay_mode{false}; ///< true when we use replay data from a log @@ -269,45 +292,45 @@ private: perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; - perf_counter_t _msg_missed_air_data_perf{nullptr}; - perf_counter_t _msg_missed_gps_perf{nullptr}; - perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; + InFlightCalibration _accel_cal{}; + InFlightCalibration _gyro_cal{}; + + uint8_t _accel_calibration_count{0}; + uint8_t _gyro_calibration_count{0}; + + uint32_t _device_id_accel{0}; + uint32_t _device_id_gyro{0}; + + Vector3f _last_accel_bias_published{}; + Vector3f _last_gyro_bias_published{}; + + hrt_abstime _last_sensor_bias_published{0}; + + hrt_abstime _status_fake_hgt_pub_last{0}; + hrt_abstime _status_fake_pos_pub_last{0}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) + uint32_t _device_id_mag {0}; + + perf_counter_t _msg_missed_magnetometer_perf {nullptr}; + // Used to control saving of mag declination to be used on next startup bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved - InFlightCalibration _accel_cal{}; - InFlightCalibration _gyro_cal{}; InFlightCalibration _mag_cal{}; - - uint64_t _gps_time_usec{0}; - int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid - uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt - float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 - - uint8_t _accel_calibration_count{0}; - uint8_t _baro_calibration_count{0}; - uint8_t _gyro_calibration_count{0}; uint8_t _mag_calibration_count{0}; - - uint32_t _device_id_accel{0}; - uint32_t _device_id_baro{0}; - uint32_t _device_id_gyro{0}; - uint32_t _device_id_mag{0}; - - Vector3f _last_accel_bias_published{}; - Vector3f _last_gyro_bias_published{}; Vector3f _last_mag_bias_published{}; - hrt_abstime _last_sensor_bias_published{0}; - hrt_abstime _last_gps_status_published{0}; + hrt_abstime _status_mag_pub_last{0}; + hrt_abstime _status_mag_heading_pub_last{0}; - hrt_abstime _status_baro_hgt_pub_last{0}; - hrt_abstime _status_rng_hgt_pub_last{0}; + uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; - hrt_abstime _status_fake_hgt_pub_last{0}; - hrt_abstime _status_fake_pos_pub_last{0}; + uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; + uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)}; @@ -326,18 +349,6 @@ private: uORB::PublicationMulti _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)}; #endif // CONFIG_EKF2_EXTERNAL_VISION - hrt_abstime _status_gnss_hgt_pub_last{0}; - hrt_abstime _status_gnss_pos_pub_last{0}; - hrt_abstime _status_gnss_vel_pub_last{0}; -#if defined(CONFIG_EKF2_GNSS_YAW) - hrt_abstime _status_gnss_yaw_pub_last {0}; -#endif // CONFIG_EKF2_GNSS_YAW - - hrt_abstime _status_mag_pub_last{0}; - hrt_abstime _status_mag_heading_pub_last{0}; - - hrt_abstime _status_gravity_pub_last{0}; - #if defined(CONFIG_EKF2_AUXVEL) uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)}; @@ -347,20 +358,49 @@ private: perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_TERRAIN) + +# if defined(CONFIG_EKF2_RANGE_FINDER) + uORB::PublicationMulti _estimator_aid_src_terrain_range_finder_pub {ORB_ID(estimator_aid_src_terrain_range_finder)}; + hrt_abstime _status_terrain_range_finder_pub_last{0}; +# endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub {ORB_ID(estimator_aid_src_terrain_optical_flow)}; + hrt_abstime _status_terrain_optical_flow_pub_last{0}; +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + #if defined(CONFIG_EKF2_OPTICAL_FLOW) uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)}; uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; - uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; - hrt_abstime _status_terrain_optical_flow_pub_last{0}; + hrt_abstime _optical_flow_vel_pub_last{0}; + perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_BAROMETER) + perf_counter_t _msg_missed_air_data_perf {nullptr}; + + uint8_t _baro_calibration_count {0}; + uint32_t _device_id_baro{0}; + hrt_abstime _status_baro_hgt_pub_last{0}; + float _last_baro_bias_published{}; - float _last_gnss_hgt_bias_published{}; - float _last_rng_hgt_bias_published{}; + + uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; + + uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; + uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_DRAG_FUSION) + uORB::PublicationMulti _estimator_aid_src_drag_pub {ORB_ID(estimator_aid_src_drag)}; + hrt_abstime _status_drag_pub_last{0}; +#endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_AIRSPEED) uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; @@ -385,18 +425,18 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; #if defined(CONFIG_EKF2_RANGE_FINDER) + hrt_abstime _status_rng_hgt_pub_last {0}; + float _last_rng_hgt_bias_published{}; + uORB::PublicationMulti _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; @@ -423,10 +463,7 @@ private: uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; - uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; - uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; @@ -434,32 +471,56 @@ private: uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - - uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; - uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; - uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; - uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; -#if defined(CONFIG_EKF2_GNSS_YAW) - uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; -#endif // CONFIG_EKF2_GNSS_YAW - - uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; - uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; - - uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; - // 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; +#if defined(CONFIG_EKF2_WIND) + uORB::PublicationMulti _wind_pub; +#endif // CONFIG_EKF2_WIND + +#if defined(CONFIG_EKF2_GNSS) + perf_counter_t _msg_missed_gps_perf {nullptr}; + + uint64_t _gps_time_usec{0}; + int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid + uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt + float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + + hrt_abstime _last_gps_status_published{0}; + + hrt_abstime _status_gnss_hgt_pub_last{0}; + hrt_abstime _status_gnss_pos_pub_last{0}; + hrt_abstime _status_gnss_vel_pub_last{0}; + + float _last_gnss_hgt_bias_published{}; + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + + uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; + uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; + uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; + uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; + uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; + + uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; + +# if defined(CONFIG_EKF2_GNSS_YAW) + hrt_abstime _status_gnss_yaw_pub_last {0}; + uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + hrt_abstime _status_gravity_pub_last {0}; + uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; +#endif // CONFIG_EKF2_GRAVITY_FUSION PreFlightChecker _preflt_checker; @@ -471,13 +532,6 @@ private: (ParamExtInt) _param_ekf2_predict_us, (ParamExtInt) _param_ekf2_imu_ctrl, - (ParamExtFloat) - _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) - #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) _param_ekf2_avel_delay, ///< auxiliary velocity measurement delay relative to the IMU (mSec) @@ -493,31 +547,59 @@ private: _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) (ParamExtFloat) _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) - (ParamExtFloat) - _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) - (ParamExtFloat) - _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) - (ParamExtFloat) - _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) - (ParamExtFloat) - _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) - (ParamExtFloat) - _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) - (ParamExtFloat) - _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) - (ParamExtFloat) - _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) - (ParamExtFloat) - _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) +#if defined(CONFIG_EKF2_WIND) + (ParamExtFloat) _param_ekf2_wind_nsd, +#endif // CONFIG_EKF2_WIND + + (ParamExtFloat) _param_ekf2_noaid_noise, + +#if defined(CONFIG_EKF2_GNSS) + (ParamExtInt) _param_ekf2_gps_ctrl, + (ParamExtFloat) _param_ekf2_gps_delay, + + (ParamExtFloat) _param_ekf2_gps_pos_x, + (ParamExtFloat) _param_ekf2_gps_pos_y, + (ParamExtFloat) _param_ekf2_gps_pos_z, + + (ParamExtFloat) _param_ekf2_gps_v_noise, + (ParamExtFloat) _param_ekf2_gps_p_noise, + + (ParamExtFloat) _param_ekf2_gps_p_gate, + (ParamExtFloat) _param_ekf2_gps_v_gate, + + (ParamExtInt) _param_ekf2_gps_check, + (ParamExtFloat) _param_ekf2_req_eph, + (ParamExtFloat) _param_ekf2_req_epv, + (ParamExtFloat) _param_ekf2_req_sacc, + (ParamExtInt) _param_ekf2_req_nsats, + (ParamExtFloat) _param_ekf2_req_pdop, + (ParamExtFloat) _param_ekf2_req_hdrift, + (ParamExtFloat) _param_ekf2_req_vdrift, + (ParamFloat) _param_ekf2_req_gps_h, + + // Used by EKF-GSF experimental yaw estimator + (ParamExtFloat) _param_ekf2_gsf_tas_default, +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_BAROMETER) + (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection + (ParamExtFloat) _param_ekf2_baro_delay, + (ParamExtFloat) _param_ekf2_baro_noise, + (ParamExtFloat) _param_ekf2_baro_gate, + (ParamExtFloat) _param_ekf2_gnd_eff_dz, + (ParamExtFloat) _param_ekf2_gnd_max_hgt, + +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth + (ParamExtFloat) _param_ekf2_aspd_max, + (ParamExtFloat) _param_ekf2_pcoef_xp, + (ParamExtFloat) _param_ekf2_pcoef_xn, + (ParamExtFloat) _param_ekf2_pcoef_yp, + (ParamExtFloat) _param_ekf2_pcoef_yn, + (ParamExtFloat) _param_ekf2_pcoef_z, +# endif // CONFIG_EKF2_BARO_COMPENSATION +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) (ParamExtFloat) @@ -541,84 +623,57 @@ private: _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables #endif // CONFIG_EKF2_SIDESLIP - // control of magnetometer fusion - (ParamExtFloat) - _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) - (ParamExtFloat) - _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - - (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) - (ParamExtFloat) - _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) - (ParamExtInt) - _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data - (ParamExtInt) - _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) - - (ParamExtInt) - _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used - (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) - (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) - (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) - (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count - (ParamExtFloat) - _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision - (ParamExtFloat) - _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) - (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) +#if defined(CONFIG_EKF2_MAGNETOMETER) + (ParamExtFloat) _param_ekf2_mag_delay, + (ParamExtFloat) _param_ekf2_mag_e_noise, + (ParamExtFloat) _param_ekf2_mag_b_noise, + (ParamExtFloat) _param_ekf2_head_noise, + (ParamExtFloat) _param_ekf2_mag_noise, + (ParamExtFloat) _param_ekf2_mag_decl, + (ParamExtFloat) _param_ekf2_hdg_gate, + (ParamExtFloat) _param_ekf2_mag_gate, + (ParamExtInt) _param_ekf2_decl_type, + (ParamExtInt) _param_ekf2_mag_type, + (ParamExtFloat) _param_ekf2_mag_acclim, + (ParamExtFloat) _param_ekf2_mag_yawlim, + (ParamExtInt) _param_ekf2_mag_check, + (ParamExtFloat) _param_ekf2_mag_chk_str, + (ParamExtFloat) _param_ekf2_mag_chk_inc, + (ParamExtInt) _param_ekf2_synthetic_mag_z, +#endif // CONFIG_EKF2_MAGNETOMETER // measurement source control (ParamInt) _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used (ParamExtInt) _param_ekf2_hgt_ref, ///< selects the primary source for height data - (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection - (ParamExtInt) _param_ekf2_gps_ctrl, ///< GPS control selection (ParamExtInt) _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + (ParamExtFloat) _param_ekf2_min_rng, +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_TERRAIN) + (ParamExtInt) _param_ekf2_terr_mask, + (ParamExtFloat) _param_ekf2_terr_noise, + (ParamExtFloat) _param_ekf2_terr_grad, +#endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion - (ParamExtInt) _param_ekf2_rng_ctrl, ///< range finder control selection - (ParamExtFloat) - _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) - (ParamExtFloat) - _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) - (ParamExtFloat) - _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) - (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) - (ParamExtFloat) - _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) - (ParamExtFloat) - _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) - (ParamExtFloat) - _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) - (ParamExtFloat) - _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) - (ParamExtFloat) - _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) - - (ParamExtInt) - _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation - (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) - (ParamExtFloat) - _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) + (ParamExtInt) _param_ekf2_rng_ctrl, + (ParamExtFloat) _param_ekf2_rng_delay, + (ParamExtFloat) _param_ekf2_rng_noise, + (ParamExtFloat) _param_ekf2_rng_sfe, + (ParamExtFloat) _param_ekf2_rng_gate, + (ParamExtFloat) _param_ekf2_rng_pitch, + (ParamExtFloat) _param_ekf2_rng_a_vmax, + (ParamExtFloat) _param_ekf2_rng_a_hmax, + (ParamExtFloat) _param_ekf2_rng_a_igate, + (ParamExtFloat) _param_ekf2_rng_qlty_t, + (ParamExtFloat) _param_ekf2_rng_k_gate, + (ParamExtFloat) _param_ekf2_rng_pos_x, + (ParamExtFloat) _param_ekf2_rng_pos_y, + (ParamExtFloat) _param_ekf2_rng_pos_z, #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -647,10 +702,6 @@ private: (ParamExtFloat) _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) #endif // CONFIG_EKF2_EXTERNAL_VISION - - (ParamExtFloat) - _param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2) - #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion (ParamExtInt) @@ -675,19 +726,24 @@ private: _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_DRAG_FUSION) + (ParamExtInt) _param_ekf2_drag_ctrl, ///< drag fusion selection + // Multi-rotor drag specific force fusion + (ParamExtFloat) + _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 + (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) +#endif // CONFIG_EKF2_DRAG_FUSION + +#if defined(CONFIG_EKF2_GRAVITY_FUSION) + (ParamExtFloat) _param_ekf2_grav_noise, +#endif // CONFIG_EKF2_GRAVITY_FUSION + // sensor positions in body frame (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) - - // output predictor filter time constants - (ParamFloat) - _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) - (ParamFloat) - _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) // IMU switch on bias parameters (ParamExtFloat) @@ -708,44 +764,9 @@ private: (ParamExtFloat) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s) -#if defined(CONFIG_EKF2_DRAG_FUSION) - (ParamExtInt) _param_ekf2_drag_ctrl, ///< drag fusion selection - // Multi-rotor drag specific force fusion - (ParamExtFloat) - _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 - (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) - (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) - (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) -#endif // CONFIG_EKF2_DRAG_FUSION - -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth - // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 - (ParamExtFloat) - _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) - (ParamExtFloat) - _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis - (ParamExtFloat) - _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis - (ParamExtFloat) - _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis - (ParamExtFloat) - _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis - (ParamExtFloat) - _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis -#endif // CONFIG_EKF2_BARO_COMPENSATION - - (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time - (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check - (ParamExtFloat) _param_ekf2_mag_chk_str, - (ParamExtFloat) _param_ekf2_mag_chk_inc, - (ParamExtInt) - _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. - - // Used by EKF-GSF experimental yaw estimator - (ParamExtFloat) - _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation - + // output predictor filter time constants + (ParamFloat) _param_ekf2_tau_vel, + (ParamFloat) _param_ekf2_tau_pos ) }; #endif // !EKF2_HPP diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 6d27130363..442d30b6ee 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -18,6 +18,7 @@ depends on MODULES_EKF2 bool "airspeed fusion support" default y depends on EKF2_SIDESLIP + depends on EKF2_WIND ---help--- EKF2 airspeed fusion support. @@ -28,10 +29,19 @@ depends on MODULES_EKF2 ---help--- EKF2 auxiliary velocity fusion support. +menuconfig EKF2_BAROMETER +depends on MODULES_EKF2 + bool "barometer support" + default y + ---help--- + EKF2 barometer support. + menuconfig EKF2_BARO_COMPENSATION depends on MODULES_EKF2 bool "barometer compensation support" default y + depends on EKF2_BAROMETER + depends on EKF2_WIND ---help--- EKF2 pressure compensation support. @@ -39,6 +49,7 @@ menuconfig EKF2_DRAG_FUSION depends on MODULES_EKF2 bool "drag fusion support" default y + depends on EKF2_WIND ---help--- EKF2 drag fusion support. @@ -49,17 +60,40 @@ depends on MODULES_EKF2 ---help--- EKF2 external vision (EV) fusion support. +menuconfig EKF2_GNSS +depends on MODULES_EKF2 + bool "GNSS fusion support" + default y + ---help--- + EKF2 GNSS fusion support. + menuconfig EKF2_GNSS_YAW depends on MODULES_EKF2 bool "GNSS yaw fusion support" default y + depends on EKF2_GNSS ---help--- EKF2 GNSS yaw fusion support. +menuconfig EKF2_GRAVITY_FUSION +depends on MODULES_EKF2 + bool "gravity fusion support" + default y + ---help--- + EKF2 gravity fusion support. + +menuconfig EKF2_MAGNETOMETER +depends on MODULES_EKF2 + bool "magnetometer support" + default y + ---help--- + EKF2 magnetometer support. + menuconfig EKF2_OPTICAL_FLOW depends on MODULES_EKF2 bool "optical flow fusion support" default y + select EKF2_TERRAIN depends on EKF2_RANGE_FINDER ---help--- EKF2 optical flow fusion support. @@ -75,9 +109,25 @@ menuconfig EKF2_SIDESLIP depends on MODULES_EKF2 bool "sideslip fusion support" default y + depends on EKF2_WIND ---help--- EKF2 sideslip fusion support. +menuconfig EKF2_TERRAIN +depends on MODULES_EKF2 + bool "terrain estimator support" + default y + depends on EKF2_OPTICAL_FLOW || EKF2_RANGE_FINDER + ---help--- + EKF2 terrain estimator support. + +menuconfig EKF2_WIND +depends on MODULES_EKF2 + bool "wind estimation support" + default y + ---help--- + EKF2 wind estimation support. + menuconfig USER_EKF2 bool "ekf2 running as userspace module" default n diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 76fd6192a9..e3d5decc1a 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2016 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -1082,11 +1082,10 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); /** * Airspeed fusion threshold. * -* A value of zero will deactivate airspeed fusion. Any other positive -* value will determine the minimum airspeed which will still be fused. Set to about 90% of the vehicles stall speed. -* Both airspeed fusion and sideslip fusion must be active for the EKF to continue navigating after loss of GPS. -* Use EKF2_FUSE_BETA to activate sideslip fusion. -* Note: side slip fusion is currently not supported for tailsitters. +* Airspeed data is fused for wind estimation if above this threshold. +* Set to 0 to disable airspeed fusion. +* For reliable wind estimation both sideslip (see EKF2_FUSE_BETA) and airspeed fusion should be enabled. +* Only applies to fixed-wing vehicles (or VTOLs in fixed-wing mode). * * @group EKF2 * @min 0.0 @@ -1096,11 +1095,11 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f); /** -* Boolean determining if synthetic sideslip measurements should fused. +* Enable synthetic sideslip fusion. * -* A value of 1 indicates that fusion is active -* Both sideslip fusion and airspeed fusion must be active for the EKF to continue navigating after loss of GPS. -* Use EKF2_ARSP_THR to activate airspeed fusion. +* For reliable wind estimation both sideslip and airspeed fusion (see EKF2_ARSP_THR) should be enabled. +* Only applies to fixed-wing vehicles (or VTOLs in fixed-wing mode). +* Note: side slip fusion is currently not supported for tailsitters. * * @group EKF2 * @boolean diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 152aafc944..7d0af8e502 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -58,7 +58,7 @@ px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF e px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) -px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 1f234f77b2..f6259843a8 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,391 +1,391 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.00026,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.00033,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -190000,1,-0.0094,-0.011,2.9e-05,6.9e-05,0.0039,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.6e-13,0,0,-2e-09,0,0,0,0,0,0,0,0,5.6e-07,0.0027,0.0027,0.00044,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -290000,1,-0.0094,-0.011,6.4e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-1.7e-11,0,0,-4.8e-07,0,0,0,0,0,0,0,0,6.2e-07,0.0029,0.0029,0.00021,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -390000,1,-0.0095,-0.011,7.2e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,2.7e-10,0,0,-4.5e-06,0,0,0,0,0,0,0,0,6.8e-07,0.0031,0.0031,0.0003,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -490000,1,-0.0095,-0.012,2.2e-05,0.0039,0.0048,-0.06,0.00024,0.00048,-0.011,1.6e-06,-3.7e-07,-3.7e-08,0,0,-1.6e-05,0,0,0,0,0,0,0,0,7.7e-07,0.0034,0.0034,0.00018,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -590000,1,-0.0095,-0.012,2.9e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,-3.6e-08,0,0,-3.4e-05,0,0,0,0,0,0,0,0,8.7e-07,0.0037,0.0037,0.00026,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -690000,1,-0.0097,-0.012,8.9e-05,0.0063,0.0051,-0.06,0.0005,0.00054,-0.013,5.5e-06,-3.2e-06,-1.3e-07,0,0,-6.3e-05,0,0,0,0,0,0,0,0,9.9e-07,0.004,0.004,0.00016,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0026,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -790000,1,-0.0098,-0.013,0.0001,0.0086,0.0072,-0.063,0.0012,0.0011,-0.014,5.4e-06,-3.1e-06,-1.3e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.1e-06,0.0044,0.0044,0.00022,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.0026,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -890000,1,-0.0099,-0.013,0.00012,0.01,0.0059,-0.077,0.00096,0.00071,-0.021,1.6e-05,-1.5e-05,-3.8e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.3e-06,0.0048,0.0048,0.00014,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.0016,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -990000,1,-0.0099,-0.013,0.00013,0.015,0.0062,-0.092,0.0022,0.0013,-0.029,1.6e-05,-1.5e-05,-3.8e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.5e-06,0.0053,0.0053,0.00018,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.0016,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1090000,1,-0.01,-0.014,0.00013,0.016,0.0048,-0.11,0.0018,0.00084,-0.039,4.1e-05,-6.2e-05,-9.5e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.6e-06,0.0057,0.0057,0.00013,0.92,0.92,2,0.17,0.17,0.84,0.0098,0.0098,0.001,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1190000,1,-0.01,-0.014,0.00011,0.02,0.005,-0.12,0.0035,0.0013,-0.051,4.1e-05,-6.2e-05,-9.5e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.8e-06,0.0063,0.0063,0.00016,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.001,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1290000,1,-0.01,-0.014,0.00016,0.02,0.0041,-0.14,0.0027,0.00085,-0.064,8.2e-05,-0.00019,-1.8e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,1.9e-06,0.0064,0.0064,0.00011,0.88,0.88,2,0.15,0.15,1.4,0.0095,0.0095,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1390000,1,-0.01,-0.014,0.00017,0.026,0.0038,-0.15,0.0051,0.0013,-0.078,8.2e-05,-0.00019,-1.8e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,2.2e-06,0.0071,0.0071,0.00014,1.1,1.1,2,0.21,0.21,1.7,0.0095,0.0095,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1490000,1,-0.01,-0.014,0.00015,0.024,0.0026,-0.16,0.0038,0.00078,-0.093,0.00014,-0.00046,-2.9e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,2.2e-06,0.0067,0.0067,0.0001,0.95,0.95,2,0.14,0.14,2.1,0.0088,0.0088,0.00048,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1590000,1,-0.011,-0.014,0.00014,0.031,0.003,-0.18,0.0065,0.0011,-0.11,0.00014,-0.00046,-2.9e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,2.4e-06,0.0074,0.0074,0.00012,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00048,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1690000,1,-0.011,-0.014,0.00011,0.028,-0.00047,-0.19,0.0045,0.00057,-0.13,0.00019,-0.00088,-3.4e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.2e-06,0.0064,0.0064,9.2e-05,1,1,2,0.14,0.14,3,0.0078,0.0078,0.00035,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1790000,1,-0.011,-0.014,8e-05,0.035,-0.0024,-0.2,0.0076,0.00044,-0.15,0.00019,-0.00088,-3.4e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.4e-06,0.007,0.007,0.00011,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.00035,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1890000,1,-0.011,-0.015,6e-05,0.043,-0.0037,-0.22,0.011,0.00014,-0.17,0.00019,-0.00088,-3.4e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.7e-06,0.0076,0.0076,0.00012,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.00035,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1990000,1,-0.011,-0.014,5.4e-05,0.036,-0.0051,-0.23,0.0082,-0.00038,-0.19,0.0002,-0.0014,-2.5e-06,0,0,-0.00011,0,0,0,0,0,0,0,0,2.2e-06,0.0061,0.0061,9.6e-05,1.3,1.3,2,0.2,0.2,4.7,0.0066,0.0066,0.00026,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2090000,1,-0.011,-0.014,1.4e-05,0.041,-0.0077,-0.24,0.012,-0.001,-0.22,0.0002,-0.0014,-2.5e-06,0,0,-0.00011,0,0,0,0,0,0,0,0,2.4e-06,0.0066,0.0066,0.00011,1.7,1.7,2.1,0.31,0.31,5.3,0.0066,0.0066,0.00026,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2190000,1,-0.011,-0.014,8.2e-06,0.033,-0.0072,-0.26,0.0079,-0.0011,-0.24,0.00015,-0.002,-7.3e-08,0,0,-0.00011,0,0,0,0,0,0,0,0,1.8e-06,0.005,0.005,8.7e-05,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2290000,1,-0.011,-0.014,-6.9e-06,0.039,-0.0098,-0.27,0.011,-0.0019,-0.27,0.00015,-0.002,-7.3e-08,0,0,-0.00011,0,0,0,0,0,0,0,0,2e-06,0.0054,0.0054,9.9e-05,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2390000,1,-0.011,-0.013,-7.1e-06,0.03,-0.009,-0.29,0.0074,-0.0016,-0.3,5.8e-05,-0.0025,3.1e-06,0,0,-0.00011,0,0,0,0,0,0,0,0,1.5e-06,0.004,0.004,7.9e-05,1,1,2.1,0.19,0.19,7.4,0.0045,0.0045,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2490000,1,-0.011,-0.013,-2.7e-05,0.034,-0.011,-0.3,0.011,-0.0025,-0.32,5.8e-05,-0.0025,3.1e-06,0,0,-0.00011,0,0,0,0,0,0,0,0,1.6e-06,0.0044,0.0044,8.9e-05,1.3,1.3,2.1,0.28,0.28,8.2,0.0045,0.0045,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2590000,1,-0.011,-0.013,-2.9e-05,0.026,-0.0093,-0.31,0.0068,-0.0018,-0.36,-5.2e-05,-0.003,6.7e-06,0,0,-0.00011,0,0,0,0,0,0,0,0,1.2e-06,0.0032,0.0032,7.3e-05,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,0.00012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2690000,1,-0.011,-0.013,-3.5e-05,0.03,-0.011,-0.33,0.0096,-0.0029,-0.39,-5.2e-05,-0.003,6.7e-06,0,0,-0.00011,0,0,0,0,0,0,0,0,1.3e-06,0.0035,0.0035,8.1e-05,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,0.00012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2790000,1,-0.011,-0.013,-5.5e-05,0.023,-0.0096,-0.34,0.0061,-0.002,-0.42,-0.00017,-0.0033,1e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1e-06,0.0027,0.0027,6.7e-05,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,9.9e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2890000,1,-0.011,-0.013,-0.00011,0.027,-0.011,-0.35,0.0086,-0.003,-0.46,-0.00017,-0.0033,1e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.1e-06,0.0029,0.0029,7.5e-05,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,9.9e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -2990000,1,-0.011,-0.013,-7.3e-05,0.022,-0.0097,-0.36,0.0056,-0.0021,-0.49,-0.00028,-0.0036,1.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,8.4e-07,0.0023,0.0023,6.3e-05,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,8.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3090000,1,-0.011,-0.013,-7.4e-05,0.024,-0.011,-0.38,0.0079,-0.0032,-0.53,-0.00028,-0.0036,1.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,9.3e-07,0.0025,0.0025,6.9e-05,0.82,0.82,2.2,0.22,0.22,14,0.0027,0.0027,8.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3190000,1,-0.011,-0.013,-0.00014,0.019,-0.0088,-0.39,0.0052,-0.0022,-0.57,-0.00039,-0.0039,1.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,7.2e-07,0.002,0.002,5.8e-05,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,6.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3290000,1,-0.011,-0.013,-0.00011,0.023,-0.011,-0.4,0.0073,-0.0031,-0.61,-0.00039,-0.0039,1.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,7.9e-07,0.0022,0.0022,6.4e-05,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,6.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3390000,1,-0.011,-0.012,-0.00015,0.018,-0.0094,-0.42,0.0049,-0.0022,-0.65,-0.00049,-0.0041,1.9e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,6.2e-07,0.0017,0.0017,5.5e-05,0.53,0.53,2.3,0.14,0.14,17,0.0019,0.0019,5.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3490000,1,-0.011,-0.013,-0.00016,0.021,-0.012,-0.43,0.0068,-0.0032,-0.69,-0.00049,-0.0041,1.9e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,6.8e-07,0.0019,0.0019,6e-05,0.65,0.65,2.3,0.19,0.19,19,0.0019,0.0019,5.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3590000,1,-0.011,-0.012,-0.00014,0.017,-0.011,-0.44,0.0046,-0.0023,-0.73,-0.0006,-0.0044,2.2e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.4e-07,0.0015,0.0015,5.2e-05,0.48,0.48,2.4,0.13,0.13,20,0.0016,0.0016,4.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3690000,1,-0.011,-0.012,-2.6e-05,0.019,-0.014,-0.46,0.0064,-0.0036,-0.78,-0.0006,-0.0044,2.2e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.9e-07,0.0017,0.0017,5.6e-05,0.59,0.59,2.4,0.18,0.18,21,0.0016,0.0016,4.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3790000,1,-0.011,-0.012,9.4e-06,0.015,-0.014,-0.47,0.0043,-0.0026,-0.83,-0.00072,-0.0045,2.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,4.7e-07,0.0014,0.0014,4.9e-05,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,4.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3890000,1,-0.011,-0.012,-2.8e-05,0.017,-0.015,-0.48,0.006,-0.004,-0.87,-0.00072,-0.0045,2.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.1e-07,0.0015,0.0015,5.3e-05,0.54,0.54,2.4,0.17,0.17,24,0.0014,0.0014,4.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -3990000,1,-0.011,-0.012,-2.9e-05,0.02,-0.016,-0.5,0.0078,-0.0056,-0.92,-0.00072,-0.0045,2.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.5e-07,0.0016,0.0016,5.7e-05,0.66,0.66,2.5,0.23,0.23,26,0.0014,0.0014,4.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4090000,1,-0.011,-0.012,-4.4e-05,0.017,-0.014,-0.51,0.0056,-0.0042,-0.97,-0.00084,-0.0047,2.8e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,4.3e-07,0.0013,0.0013,5e-05,0.5,0.5,2.5,0.16,0.16,27,0.0012,0.0012,3.5e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4190000,1,-0.011,-0.012,-7.2e-05,0.02,-0.017,-0.53,0.0075,-0.0057,-1,-0.00084,-0.0047,2.8e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,4.7e-07,0.0015,0.0015,5.3e-05,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,3.5e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4290000,1,-0.01,-0.012,-0.00013,0.017,-0.012,-0.54,0.0054,-0.0042,-1.1,-0.00097,-0.0049,3.1e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,3.7e-07,0.0012,0.0012,4.7e-05,0.46,0.46,2.6,0.15,0.15,31,0.00095,0.00096,3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4390000,1,-0.01,-0.012,-0.00011,0.018,-0.013,-0.55,0.0072,-0.0055,-1.1,-0.00097,-0.0049,3.1e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,4e-07,0.0013,0.0013,5e-05,0.56,0.56,2.6,0.2,0.2,33,0.00095,0.00096,3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4490000,1,-0.01,-0.012,-6e-05,0.015,-0.01,-0.57,0.0051,-0.0038,-1.2,-0.0011,-0.005,3.4e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,3.2e-07,0.001,0.001,4.5e-05,0.43,0.43,2.6,0.14,0.14,34,0.00078,0.00079,2.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4590000,1,-0.01,-0.012,-3.5e-05,0.018,-0.012,-0.58,0.0068,-0.0049,-1.2,-0.0011,-0.005,3.4e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,3.4e-07,0.0011,0.0011,4.8e-05,0.51,0.51,2.7,0.19,0.19,36,0.00078,0.00079,2.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4690000,1,-0.01,-0.012,-4.2e-05,0.014,-0.01,-0.6,0.0049,-0.0034,-1.3,-0.0012,-0.0052,3.6e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,2.7e-07,0.00091,0.00092,4.3e-05,0.39,0.39,2.7,0.14,0.14,38,0.00064,0.00064,2.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4790000,1,-0.01,-0.012,-5.7e-05,0.016,-0.012,-0.61,0.0064,-0.0045,-1.4,-0.0012,-0.0052,3.6e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,2.9e-07,0.00099,0.00099,4.5e-05,0.47,0.47,2.7,0.18,0.18,40,0.00064,0.00064,2.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4890000,1,-0.01,-0.012,-7.9e-05,0.013,-0.01,-0.63,0.0046,-0.0033,-1.4,-0.0013,-0.0053,3.8e-05,0,0,-7.8e-05,0,0,0,0,0,0,0,0,2.3e-07,0.0008,0.0008,4.1e-05,0.36,0.36,2.8,0.13,0.13,42,0.00052,0.00052,2e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -4990000,1,-0.01,-0.012,-0.00011,0.015,-0.011,-0.64,0.006,-0.0043,-1.5,-0.0013,-0.0053,3.8e-05,0,0,-7.8e-05,0,0,0,0,0,0,0,0,2.4e-07,0.00086,0.00086,4.3e-05,0.43,0.43,2.8,0.17,0.17,44,0.00052,0.00052,2e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5090000,1,-0.01,-0.011,-6.8e-05,0.012,-0.0086,-0.66,0.0043,-0.0031,-1.6,-0.0013,-0.0054,4e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,1.9e-07,0.00069,0.00069,3.9e-05,0.33,0.33,2.8,0.12,0.12,46,0.00042,0.00042,1.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5190000,1,-0.01,-0.011,-4.6e-05,0.013,-0.01,-0.67,0.0056,-0.004,-1.6,-0.0013,-0.0054,4e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,2e-07,0.00075,0.00075,4.1e-05,0.39,0.39,2.9,0.16,0.16,49,0.00042,0.00042,1.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5290000,1,-0.01,-0.011,-6.5e-05,0.0093,-0.0074,-0.68,0.0039,-0.0029,-1.7,-0.0014,-0.0055,4.2e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,1.6e-07,0.0006,0.0006,3.7e-05,0.3,0.3,2.9,0.12,0.12,51,0.00034,0.00034,1.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5390000,1,-0.0099,-0.011,-1.2e-05,0.0088,-0.0083,-0.7,0.0048,-0.0037,-1.8,-0.0014,-0.0055,4.2e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,1.7e-07,0.00064,0.00064,3.9e-05,0.36,0.36,2.9,0.16,0.16,53,0.00034,0.00034,1.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5490000,1,-0.0098,-0.011,-8.7e-06,0.0061,-0.0064,-0.71,0.0032,-0.0026,-1.8,-0.0014,-0.0055,4.3e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,1.3e-07,0.00052,0.00052,3.6e-05,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,1.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5590000,1,-0.0098,-0.011,-3.4e-05,0.0069,-0.0068,-0.73,0.0039,-0.0032,-1.9,-0.0014,-0.0055,4.3e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,1.4e-07,0.00055,0.00055,3.8e-05,0.33,0.33,3,0.15,0.15,58,0.00028,0.00028,1.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5690000,1,-0.0096,-0.011,3.2e-05,0.0048,-0.004,-0.74,0.0027,-0.0022,-2,-0.0015,-0.0056,4.4e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,1.1e-07,0.00045,0.00045,3.4e-05,0.25,0.25,3.1,0.11,0.11,61,0.00022,0.00022,1.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5790000,1,-0.0096,-0.011,2.4e-05,0.0052,-0.003,-0.75,0.0031,-0.0026,-2,-0.0015,-0.0056,4.4e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,1.2e-07,0.00048,0.00048,3.6e-05,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,1.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5890000,1,-0.0095,-0.011,-5.9e-06,0.0045,-0.0011,0.0028,0.0022,-0.0017,-3.7e+02,-0.0015,-0.0056,4.5e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,9e-08,0.00038,0.00038,3.3e-05,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,1.2e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -5990000,1,-0.0094,-0.011,1.2e-05,0.0048,0.00032,0.015,0.0026,-0.0017,-3.7e+02,-0.0015,-0.0056,4.5e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,9.6e-08,0.00041,0.00041,3.5e-05,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,1.2e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6090000,1,-0.0094,-0.011,-7.2e-06,0.0059,0.0015,-0.011,0.0031,-0.0016,-3.7e+02,-0.0015,-0.0056,4.5e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,1e-07,0.00044,0.00044,3.7e-05,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,1.2e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6190000,0.71,0.0013,-0.015,0.71,-0.004,0.0045,-0.005,0.0017,-0.00054,-3.7e+02,-0.0015,-0.0057,4.5e-05,0,0,-8.8e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0016,0.00034,0.00034,0.0016,0.22,0.22,4.9,0.13,0.13,0.32,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6290000,0.71,0.0013,-0.015,0.71,-0.0041,0.0058,-0.012,0.0013,-8.7e-06,-3.7e+02,-0.0015,-0.0057,4.5e-05,0,0,-9.6e-05,0.37,0.0037,0.025,0,0,0,0,0,0.00078,0.00034,0.00034,0.00075,0.22,0.22,3.2,0.16,0.16,0.3,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6390000,0.71,0.0013,-0.014,0.71,-0.0051,0.0063,-0.05,0.00079,0.00061,-3.7e+02,-0.0015,-0.0057,4.6e-05,0,0,-3.7e-05,0.37,0.0037,0.025,0,0,0,0,0,0.00054,0.00034,0.00034,0.00051,0.23,0.23,2.3,0.19,0.19,0.29,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6490000,0.71,0.0014,-0.014,0.71,-0.0053,0.0071,-0.052,0.00026,0.0013,-3.7e+02,-0.0015,-0.0057,4.5e-05,0,0,-8.6e-05,0.37,0.0037,0.025,0,0,0,0,0,0.00041,0.00034,0.00034,0.00037,0.24,0.24,1.5,0.23,0.23,0.26,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6590000,0.71,0.0014,-0.014,0.71,-0.0058,0.0072,-0.099,-0.00026,0.002,-3.7e+02,-0.0015,-0.0057,4.4e-05,0,0,9.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.00033,0.00034,0.00034,0.00029,0.25,0.25,1.1,0.28,0.28,0.23,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6690000,0.7,0.0014,-0.014,0.71,-0.0054,0.0083,-0.076,-0.00083,0.0028,-3.7e+02,-0.0015,-0.0057,4.2e-05,0,0,-0.00022,0.37,0.0037,0.025,0,0,0,0,0,0.00028,0.00035,0.00035,0.00024,0.26,0.26,0.78,0.33,0.33,0.21,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6790000,0.71,0.0014,-0.014,0.71,-0.0067,0.0082,-0.11,-0.0014,0.0036,-3.7e+02,-0.0015,-0.0057,4.3e-05,0,0,8.1e-06,0.37,0.0037,0.025,0,0,0,0,0,0.00025,0.00035,0.00035,0.00021,0.28,0.28,0.6,0.38,0.38,0.2,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6890000,0.71,0.0014,-0.014,0.71,-0.0088,0.0088,-0.12,-0.0022,0.0045,-3.7e+02,-0.0015,-0.0057,4.3e-05,0,0,-4.3e-05,0.37,0.0037,0.025,0,0,0,0,0,0.00022,0.00036,0.00036,0.00018,0.3,0.3,0.46,0.44,0.44,0.18,0.00015,0.00015,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -6990000,0.7,0.0015,-0.014,0.71,-0.0088,0.0097,-0.12,-0.0031,0.0054,-3.7e+02,-0.0015,-0.0057,3.8e-05,0,0,-0.00034,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00036,0.00036,0.00016,0.32,0.32,0.36,0.51,0.51,0.16,0.00014,0.00014,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -7090000,0.7,0.0015,-0.014,0.71,-0.0094,0.0093,-0.13,-0.004,0.0063,-3.7e+02,-0.0015,-0.0057,3.3e-05,0,0,-0.0007,0.37,0.0037,0.025,0,0,0,0,0,0.00018,0.00037,0.00037,0.00015,0.35,0.35,0.29,0.58,0.58,0.16,0.00014,0.00014,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -7190000,0.7,0.0015,-0.014,0.71,-0.011,0.0095,-0.15,-0.005,0.0072,-3.7e+02,-0.0015,-0.0057,3.1e-05,0,0,-0.00049,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00038,0.00038,0.00013,0.37,0.37,0.24,0.66,0.66,0.15,0.00014,0.00014,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -7290000,0.7,0.0015,-0.014,0.71,-0.012,0.0097,-0.15,-0.0062,0.0081,-3.7e+02,-0.0015,-0.0057,3e-05,0,0,-0.0012,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00039,0.00039,0.00012,0.41,0.41,0.2,0.76,0.76,0.14,0.00014,0.00014,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -7390000,0.7,0.0016,-0.014,0.71,-0.013,0.011,-0.16,-0.0074,0.0092,-3.7e+02,-0.0015,-0.0056,3.1e-05,0,0,-0.0013,0.37,0.0037,0.025,0,0,0,0,0,0.00015,0.00039,0.00039,0.00011,0.44,0.44,0.18,0.85,0.85,0.13,0.00014,0.00014,1.1e-05,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -7490000,0.7,0.0016,-0.014,0.71,-0.014,0.012,-0.16,-0.0087,0.01,-3.7e+02,-0.0015,-0.0056,3e-05,0,0,-0.0021,0.37,0.0037,0.025,0,0,0,0,0,0.00014,0.0004,0.0004,0.00011,0.48,0.48,0.15,0.96,0.96,0.12,0.00014,0.00014,1.1e-05,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -7590000,0.7,0.0016,-0.014,0.71,-0.016,0.013,-0.17,-0.01,0.011,-3.7e+02,-0.0015,-0.0056,3.3e-05,0,0,-0.003,0.37,0.0037,0.025,0,0,0,0,0,0.00013,0.00041,0.00041,9.8e-05,0.52,0.52,0.14,1.1,1.1,0.12,0.00014,0.00014,1.1e-05,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -7690000,0.7,0.0017,-0.014,0.71,-0.018,0.014,-0.16,-0.012,0.013,-3.7e+02,-0.0015,-0.0056,3.2e-05,0,0,-0.005,0.37,0.0037,0.025,0,0,0,0,0,0.00013,0.00042,0.00042,9.3e-05,0.56,0.56,0.13,1.2,1.2,0.11,0.00014,0.00014,1.1e-05,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -7790000,0.7,0.0017,-0.014,0.71,-0.019,0.015,-0.16,-0.014,0.014,-3.7e+02,-0.0015,-0.0056,2.4e-05,0,0,-0.007,0.37,0.0037,0.025,0,0,0,0,0,0.00012,0.00043,0.00043,8.7e-05,0.6,0.6,0.12,1.3,1.3,0.11,0.00014,0.00014,1.1e-05,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -7890000,0.7,0.0017,-0.014,0.71,-0.022,0.016,-0.16,-0.016,0.015,-3.7e+02,-0.0015,-0.0056,2.6e-05,0,0,-0.0096,0.37,0.0037,0.025,0,0,0,0,0,0.00011,0.00045,0.00045,8.2e-05,0.66,0.66,0.11,1.5,1.5,0.1,0.00014,0.00014,1.1e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 -7990000,0.7,0.0018,-0.014,0.71,-0.024,0.018,-0.16,-0.018,0.017,-3.7e+02,-0.0015,-0.0056,2.9e-05,0,0,-0.011,0.37,0.0037,0.025,0,0,0,0,0,0.00011,0.00046,0.00046,7.8e-05,0.7,0.7,0.1,1.7,1.7,0.099,0.00014,0.00014,1.1e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 -8090000,0.7,0.0018,-0.014,0.71,-0.025,0.019,-0.17,-0.02,0.018,-3.7e+02,-0.0015,-0.0056,3.4e-05,0,0,-0.011,0.37,0.0037,0.025,0,0,0,0,0,0.00011,0.00047,0.00047,7.5e-05,0.76,0.76,0.1,1.9,1.9,0.097,0.00014,0.00014,1e-05,0.04,0.04,0.037,0,0,0,0,0,0,0,0 -8190000,0.7,0.0018,-0.014,0.71,-0.028,0.021,-0.18,-0.023,0.02,-3.7e+02,-0.0015,-0.0056,2.7e-05,0,0,-0.013,0.37,0.0037,0.025,0,0,0,0,0,0.0001,0.00049,0.00049,7.1e-05,0.83,0.83,0.099,2.1,2.1,0.094,0.00014,0.00014,1e-05,0.04,0.04,0.037,0,0,0,0,0,0,0,0 -8290000,0.7,0.0018,-0.014,0.71,-0.029,0.021,-0.17,-0.025,0.022,-3.7e+02,-0.0015,-0.0056,2.2e-05,0,0,-0.017,0.37,0.0037,0.025,0,0,0,0,0,0.0001,0.0005,0.0005,6.8e-05,0.88,0.88,0.097,2.3,2.3,0.091,0.00014,0.00014,1e-05,0.04,0.04,0.036,0,0,0,0,0,0,0,0 -8390000,0.7,0.0019,-0.014,0.71,-0.031,0.022,-0.17,-0.028,0.024,-3.7e+02,-0.0015,-0.0056,2.8e-05,0,0,-0.021,0.37,0.0037,0.025,0,0,0,0,0,9.8e-05,0.00051,0.00051,6.5e-05,0.95,0.95,0.097,2.5,2.5,0.091,0.00014,0.00014,1e-05,0.04,0.04,0.035,0,0,0,0,0,0,0,0 -8490000,0.7,0.0018,-0.014,0.71,-0.033,0.024,-0.17,-0.031,0.025,-3.7e+02,-0.0015,-0.0056,2.4e-05,0,0,-0.025,0.37,0.0037,0.025,0,0,0,0,0,9.5e-05,0.00052,0.00052,6.3e-05,1,1,0.096,2.8,2.8,0.089,0.00014,0.00014,1e-05,0.04,0.04,0.034,0,0,0,0,0,0,0,0 -8590000,0.7,0.0019,-0.014,0.71,-0.035,0.026,-0.17,-0.035,0.028,-3.7e+02,-0.0015,-0.0056,1.6e-05,0,0,-0.029,0.37,0.0037,0.025,0,0,0,0,0,9.3e-05,0.00054,0.00054,6e-05,1.1,1.1,0.095,3.1,3.1,0.088,0.00014,0.00014,1e-05,0.04,0.04,0.034,0,0,0,0,0,0,0,0 -8690000,0.7,0.0019,-0.014,0.71,-0.038,0.027,-0.16,-0.038,0.029,-3.7e+02,-0.0015,-0.0056,2.5e-05,0,0,-0.035,0.37,0.0037,0.025,0,0,0,0,0,9.1e-05,0.00055,0.00055,5.9e-05,1.1,1.1,0.096,3.3,3.3,0.088,0.00014,0.00014,1e-05,0.04,0.04,0.033,0,0,0,0,0,0,0,0 -8790000,0.7,0.0019,-0.014,0.71,-0.041,0.029,-0.15,-0.042,0.031,-3.7e+02,-0.0015,-0.0056,1.9e-05,0,0,-0.041,0.37,0.0037,0.025,0,0,0,0,0,8.9e-05,0.00056,0.00056,5.7e-05,1.2,1.2,0.095,3.7,3.7,0.087,0.00014,0.00014,1e-05,0.04,0.04,0.032,0,0,0,0,0,0,0,0 -8890000,0.7,0.0019,-0.014,0.71,-0.042,0.029,-0.15,-0.045,0.033,-3.7e+02,-0.0015,-0.0056,1.5e-05,0,0,-0.045,0.37,0.0037,0.025,0,0,0,0,0,8.7e-05,0.00057,0.00057,5.5e-05,1.3,1.3,0.095,3.9,3.9,0.086,0.00013,0.00013,1e-05,0.04,0.04,0.03,0,0,0,0,0,0,0,0 -8990000,0.7,0.002,-0.014,0.71,-0.044,0.03,-0.14,-0.049,0.035,-3.7e+02,-0.0015,-0.0056,6.5e-06,0,0,-0.051,0.37,0.0037,0.025,0,0,0,0,0,8.6e-05,0.00059,0.00059,5.3e-05,1.4,1.4,0.096,4.3,4.3,0.087,0.00013,0.00013,1e-05,0.04,0.04,0.029,0,0,0,0,0,0,0,0 -9090000,0.7,0.002,-0.014,0.71,-0.046,0.03,-0.14,-0.052,0.037,-3.7e+02,-0.0015,-0.0056,1.7e-07,0,0,-0.053,0.37,0.0037,0.025,0,0,0,0,0,8.4e-05,0.0006,0.0006,5.2e-05,1.4,1.4,0.095,4.6,4.6,0.086,0.00013,0.00013,1e-05,0.04,0.04,0.028,0,0,0,0,0,0,0,0 -9190000,0.7,0.002,-0.014,0.71,-0.047,0.031,-0.14,-0.057,0.04,-3.7e+02,-0.0015,-0.0056,1.6e-05,0,0,-0.057,0.37,0.0037,0.025,0,0,0,0,0,8.3e-05,0.00062,0.00062,5e-05,1.5,1.5,0.094,5.1,5.1,0.085,0.00013,0.00013,1e-05,0.04,0.04,0.027,0,0,0,0,0,0,0,0 -9290000,0.7,0.002,-0.014,0.71,-0.047,0.032,-0.14,-0.059,0.04,-3.7e+02,-0.0015,-0.0056,1.7e-05,0,0,-0.061,0.37,0.0037,0.025,0,0,0,0,0,8.2e-05,0.00062,0.00062,4.9e-05,1.6,1.6,0.093,5.3,5.3,0.085,0.00013,0.00013,1e-05,0.04,0.04,0.025,0,0,0,0,0,0,0,0 -9390000,0.7,0.002,-0.014,0.71,-0.049,0.034,-0.14,-0.064,0.043,-3.7e+02,-0.0015,-0.0056,1.5e-05,0,0,-0.065,0.37,0.0037,0.025,0,0,0,0,0,8e-05,0.00064,0.00064,4.8e-05,1.7,1.7,0.093,5.9,5.9,0.086,0.00013,0.00013,1e-05,0.04,0.04,0.024,0,0,0,0,0,0,0,0 -9490000,0.7,0.002,-0.014,0.71,-0.049,0.034,-0.13,-0.065,0.044,-3.7e+02,-0.0015,-0.0056,2.8e-05,0,0,-0.068,0.37,0.0037,0.025,0,0,0,0,0,7.9e-05,0.00064,0.00064,4.7e-05,1.7,1.7,0.091,6.1,6.1,0.085,0.00013,0.00013,1e-05,0.04,0.04,0.023,0,0,0,0,0,0,0,0 -9590000,0.7,0.002,-0.014,0.71,-0.052,0.036,-0.13,-0.07,0.047,-3.7e+02,-0.0015,-0.0056,3e-05,0,0,-0.072,0.37,0.0037,0.025,0,0,0,0,0,7.8e-05,0.00066,0.00066,4.6e-05,1.8,1.8,0.09,6.7,6.7,0.085,0.00013,0.00013,1e-05,0.04,0.04,0.021,0,0,0,0,0,0,0,0 -9690000,0.7,0.002,-0.014,0.71,-0.051,0.037,-0.12,-0.071,0.047,-3.7e+02,-0.0015,-0.0056,1.5e-05,0,0,-0.077,0.37,0.0037,0.025,0,0,0,0,0,7.7e-05,0.00065,0.00065,4.5e-05,1.9,1.9,0.089,6.8,6.8,0.086,0.00012,0.00012,1e-05,0.04,0.04,0.02,0,0,0,0,0,0,0,0 -9790000,0.7,0.002,-0.014,0.71,-0.052,0.039,-0.11,-0.077,0.05,-3.7e+02,-0.0015,-0.0056,2.3e-05,0,0,-0.082,0.37,0.0037,0.025,0,0,0,0,0,7.7e-05,0.00067,0.00067,4.4e-05,2,2,0.087,7.5,7.5,0.085,0.00012,0.00012,1e-05,0.04,0.04,0.019,0,0,0,0,0,0,0,0 -9890000,0.7,0.002,-0.014,0.71,-0.051,0.039,-0.11,-0.076,0.05,-3.7e+02,-0.0015,-0.0056,2.1e-05,0,0,-0.085,0.37,0.0037,0.025,0,0,0,0,0,7.6e-05,0.00067,0.00067,4.3e-05,2,2,0.084,7.6,7.6,0.085,0.00012,0.00012,1e-05,0.04,0.04,0.018,0,0,0,0,0,0,0,0 -9990000,0.7,0.0021,-0.014,0.71,-0.053,0.04,-0.1,-0.082,0.054,-3.7e+02,-0.0015,-0.0056,1.3e-05,0,0,-0.089,0.37,0.0037,0.025,0,0,0,0,0,7.5e-05,0.00069,0.00069,4.2e-05,2.1,2.1,0.083,8.4,8.4,0.086,0.00012,0.00012,1e-05,0.04,0.04,0.017,0,0,0,0,0,0,0,0 -10090000,0.7,0.0021,-0.014,0.71,-0.05,0.039,-0.096,-0.08,0.053,-3.7e+02,-0.0014,-0.0056,1.4e-05,0,0,-0.091,0.37,0.0037,0.025,0,0,0,0,0,7.4e-05,0.00068,0.00068,4.2e-05,2.1,2.1,0.08,8.4,8.4,0.085,0.00012,0.00012,1e-05,0.04,0.04,0.016,0,0,0,0,0,0,0,0 -10190000,0.7,0.0021,-0.014,0.71,-0.052,0.042,-0.096,-0.085,0.057,-3.7e+02,-0.0014,-0.0056,-7.6e-06,0,0,-0.093,0.37,0.0037,0.025,0,0,0,0,0,7.4e-05,0.0007,0.0007,4.1e-05,2.2,2.2,0.078,9.2,9.2,0.084,0.00012,0.00012,1e-05,0.04,0.04,0.014,0,0,0,0,0,0,0,0 -10290000,0.7,0.0021,-0.014,0.71,-0.053,0.043,-0.084,-0.091,0.061,-3.7e+02,-0.0014,-0.0056,-2.2e-05,0,0,-0.098,0.37,0.0037,0.025,0,0,0,0,0,7.3e-05,0.00072,0.00072,4e-05,2.4,2.4,0.076,10,10,0.085,0.00012,0.00012,1e-05,0.04,0.04,0.014,0,0,0,0,0,0,0,0 -10390000,0.7,0.002,-0.014,0.71,0.0096,-0.019,0.0096,0.00087,-0.0017,-3.7e+02,-0.0014,-0.0056,-2.3e-05,-6.6e-08,4.9e-08,-0.1,0.37,0.0037,0.025,0,0,0,0,0,7.3e-05,0.00075,0.00075,4e-05,0.25,0.25,0.56,0.25,0.25,0.078,0.00012,0.00012,1e-05,0.04,0.04,0.013,0,0,0,0,0,0,0,0 -10490000,0.7,0.0021,-0.014,0.71,0.0086,-0.017,0.023,0.0018,-0.0035,-3.7e+02,-0.0014,-0.0056,-3.8e-05,-1.7e-06,1.3e-06,-0.1,0.37,0.0037,0.025,0,0,0,0,0,7.2e-05,0.00077,0.00077,3.9e-05,0.26,0.26,0.55,0.26,0.26,0.08,0.00012,0.00012,9.9e-06,0.04,0.04,0.012,0,0,0,0,0,0,0,0 -10590000,0.7,0.0022,-0.014,0.71,0.0081,-0.0063,0.026,0.0018,-0.00081,-3.7e+02,-0.0014,-0.0056,-4e-05,-0.00026,2.6e-05,-0.11,0.37,0.0037,0.025,0,0,0,0,0,7.1e-05,0.00078,0.00078,3.9e-05,0.14,0.14,0.27,0.13,0.13,0.073,0.00012,0.00012,9.9e-06,0.04,0.04,0.012,0,0,0,0,0,0,0,0 -10690000,0.7,0.0023,-0.014,0.71,0.0062,-0.0056,0.03,0.0026,-0.0014,-3.7e+02,-0.0014,-0.0056,-4.6e-05,-0.00026,2.7e-05,-0.11,0.37,0.0037,0.025,0,0,0,0,0,7.1e-05,0.0008,0.0008,3.8e-05,0.15,0.15,0.26,0.14,0.14,0.078,0.00012,0.00012,9.9e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 -10790000,0.7,0.0023,-0.014,0.71,0.0057,-0.0028,0.024,0.0027,-0.00071,-3.7e+02,-0.0014,-0.0055,-4.7e-05,-0.00041,0.00014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,7.1e-05,0.00077,0.00077,3.8e-05,0.11,0.11,0.17,0.091,0.091,0.072,0.00011,0.00011,9.8e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 -10890000,0.7,0.0022,-0.014,0.71,0.0046,-0.0014,0.02,0.0032,-0.00089,-3.7e+02,-0.0014,-0.0055,-5e-05,-0.00041,0.00014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,7e-05,0.0008,0.0008,3.8e-05,0.13,0.13,0.16,0.098,0.098,0.075,0.00011,0.00011,9.8e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 -10990000,0.7,0.0022,-0.014,0.71,0.0073,0.0036,0.014,0.0048,-0.0022,-3.7e+02,-0.0013,-0.0055,-4e-05,-0.00079,0.00072,-0.11,0.37,0.0037,0.025,0,0,0,0,0,7e-05,0.00073,0.00073,3.7e-05,0.1,0.1,0.12,0.073,0.073,0.071,0.0001,0.0001,9.7e-06,0.039,0.039,0.011,0,0,0,0,0,0,0,0 -11090000,0.7,0.0022,-0.014,0.71,0.0062,0.0062,0.019,0.0054,-0.0018,-3.7e+02,-0.0013,-0.0055,-2.8e-05,-0.0008,0.00072,-0.11,0.37,0.0037,0.025,0,0,0,0,0,7e-05,0.00075,0.00075,3.7e-05,0.13,0.13,0.11,0.08,0.08,0.074,0.0001,0.0001,9.7e-06,0.039,0.039,0.011,0,0,0,0,0,0,0,0 -11190000,0.7,0.0021,-0.014,0.71,0.011,0.0086,0.0077,0.0068,-0.0027,-3.7e+02,-0.0013,-0.0055,-4.3e-05,-0.00088,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.9e-05,0.00065,0.00065,3.7e-05,0.1,0.1,0.084,0.063,0.063,0.069,9.3e-05,9.3e-05,9.6e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 -11290000,0.7,0.0021,-0.014,0.71,0.011,0.011,0.0074,0.0079,-0.0017,-3.7e+02,-0.0013,-0.0055,-6.5e-05,-0.00089,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.9e-05,0.00067,0.00067,3.6e-05,0.13,0.13,0.078,0.071,0.071,0.072,9.3e-05,9.3e-05,9.6e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 -11390000,0.7,0.0022,-0.014,0.71,0.0058,0.0094,0.0017,0.0056,-0.0019,-3.7e+02,-0.0013,-0.0055,-8.1e-05,-0.00045,0.00078,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.9e-05,0.00055,0.00055,3.6e-05,0.1,0.1,0.063,0.058,0.058,0.068,8.2e-05,8.2e-05,9.5e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 -11490000,0.7,0.0022,-0.014,0.71,0.0035,0.012,0.0025,0.0061,-0.00084,-3.7e+02,-0.0013,-0.0055,-0.00011,-0.00046,0.00078,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.8e-05,0.00057,0.00057,3.6e-05,0.13,0.13,0.058,0.066,0.066,0.069,8.2e-05,8.2e-05,9.5e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 -11590000,0.7,0.0021,-0.014,0.71,-0.0011,0.01,-0.0034,0.0045,-0.0014,-3.7e+02,-0.0014,-0.0056,-0.00012,-2.2e-05,0.00063,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.8e-05,0.00047,0.00047,3.6e-05,0.1,0.1,0.049,0.054,0.054,0.066,7.2e-05,7.2e-05,9.4e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -11690000,0.7,0.0021,-0.014,0.71,-0.0038,0.014,-0.0079,0.0043,-0.00022,-3.7e+02,-0.0014,-0.0056,-0.00013,-1.3e-05,0.00062,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.8e-05,0.00048,0.00048,3.5e-05,0.12,0.12,0.046,0.063,0.063,0.066,7.2e-05,7.2e-05,9.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -11790000,0.7,0.0021,-0.014,0.71,-0.0097,0.013,-0.0098,0.0019,0.00058,-3.7e+02,-0.0014,-0.0056,-0.00014,0.0001,0.00041,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.8e-05,0.0004,0.0004,3.5e-05,0.097,0.097,0.039,0.053,0.053,0.063,6.3e-05,6.4e-05,9.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -11890000,0.7,0.0021,-0.014,0.71,-0.011,0.014,-0.011,0.00086,0.002,-3.7e+02,-0.0014,-0.0056,-0.00015,9.7e-05,0.00041,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.8e-05,0.00041,0.00041,3.5e-05,0.12,0.12,0.037,0.061,0.061,0.063,6.3e-05,6.4e-05,9.2e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -11990000,0.7,0.0021,-0.014,0.71,-0.013,0.014,-0.016,-0.00025,0.0023,-3.7e+02,-0.0014,-0.0056,-0.00015,0.00024,0.00042,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.8e-05,0.00034,0.00034,3.5e-05,0.091,0.091,0.033,0.051,0.051,0.061,5.7e-05,5.7e-05,9.2e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12090000,0.7,0.0022,-0.014,0.71,-0.014,0.016,-0.022,-0.0016,0.0038,-3.7e+02,-0.0014,-0.0056,-0.00014,0.00026,0.00041,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00035,0.00035,3.5e-05,0.11,0.11,0.031,0.06,0.06,0.061,5.7e-05,5.7e-05,9.1e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12190000,0.7,0.0018,-0.014,0.71,-0.0077,0.013,-0.017,0.0015,0.0019,-3.7e+02,-0.0013,-0.0057,-0.00014,0.00052,0.0009,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.0003,0.0003,3.5e-05,0.085,0.085,0.028,0.051,0.051,0.059,5.2e-05,5.2e-05,9e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12290000,0.7,0.0018,-0.014,0.71,-0.01,0.015,-0.016,0.00061,0.0033,-3.7e+02,-0.0013,-0.0057,-0.00013,0.00049,0.00092,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00031,0.00031,3.4e-05,0.1,0.1,0.028,0.06,0.06,0.059,5.2e-05,5.2e-05,9e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12390000,0.7,0.0014,-0.014,0.71,-0.0049,0.011,-0.015,0.0031,0.0017,-3.7e+02,-0.0013,-0.0058,-0.00015,0.00065,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00027,0.00027,3.4e-05,0.079,0.079,0.026,0.05,0.05,0.057,4.8e-05,4.8e-05,8.9e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12490000,0.7,0.0014,-0.014,0.71,-0.006,0.013,-0.018,0.0025,0.0029,-3.7e+02,-0.0013,-0.0058,-0.00016,0.00065,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00028,0.00028,3.4e-05,0.093,0.093,0.026,0.059,0.059,0.057,4.8e-05,4.8e-05,8.8e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 -12590000,0.7,0.0015,-0.014,0.71,-0.014,0.011,-0.023,-0.0027,0.0015,-3.7e+02,-0.0013,-0.0058,-0.00016,0.00079,0.00096,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00025,0.00025,3.4e-05,0.074,0.074,0.025,0.05,0.05,0.055,4.5e-05,4.5e-05,8.8e-06,0.037,0.037,0.0099,0,0,0,0,0,0,0,0 -12690000,0.7,0.0015,-0.014,0.71,-0.014,0.013,-0.027,-0.0041,0.0026,-3.7e+02,-0.0013,-0.0058,-0.00018,0.00081,0.00094,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00026,0.00026,3.4e-05,0.085,0.085,0.025,0.058,0.058,0.055,4.5e-05,4.5e-05,8.7e-06,0.037,0.037,0.0098,0,0,0,0,0,0,0,0 -12790000,0.7,0.0016,-0.013,0.71,-0.02,0.0093,-0.03,-0.0076,0.0013,-3.7e+02,-0.0014,-0.0058,-0.00017,0.00085,0.00088,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00023,0.00023,3.4e-05,0.068,0.068,0.024,0.049,0.049,0.053,4.2e-05,4.2e-05,8.6e-06,0.037,0.037,0.0097,0,0,0,0,0,0,0,0 -12890000,0.7,0.0015,-0.014,0.71,-0.021,0.0093,-0.029,-0.0096,0.0022,-3.7e+02,-0.0014,-0.0058,-0.00017,0.00078,0.00094,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00024,0.00024,3.4e-05,0.079,0.079,0.025,0.058,0.058,0.054,4.2e-05,4.2e-05,8.6e-06,0.037,0.037,0.0096,0,0,0,0,0,0,0,0 -12990000,0.7,0.0012,-0.014,0.71,-0.0082,0.0067,-0.03,-0.00099,0.0012,-3.7e+02,-0.0013,-0.0059,-0.00015,0.00077,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00022,0.00022,3.4e-05,0.064,0.064,0.025,0.049,0.049,0.052,4e-05,4e-05,8.5e-06,0.037,0.037,0.0094,0,0,0,0,0,0,0,0 -13090000,0.7,0.0012,-0.014,0.71,-0.0089,0.0069,-0.03,-0.0018,0.0018,-3.7e+02,-0.0013,-0.0059,-0.00017,0.00073,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00023,0.00023,3.4e-05,0.073,0.073,0.025,0.058,0.058,0.052,4e-05,4e-05,8.4e-06,0.037,0.037,0.0094,0,0,0,0,0,0,0,0 -13190000,0.7,0.00096,-0.014,0.71,0.00012,0.0063,-0.027,0.0044,0.0012,-3.7e+02,-0.0012,-0.0059,-0.00017,0.00058,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00021,0.00021,3.4e-05,0.06,0.06,0.025,0.049,0.049,0.051,3.8e-05,3.8e-05,8.3e-06,0.037,0.037,0.0091,0,0,0,0,0,0,0,0 -13290000,0.7,0.00097,-0.014,0.71,-0.00014,0.0071,-0.024,0.0045,0.0018,-3.7e+02,-0.0012,-0.0059,-0.00015,0.00046,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00022,0.00022,3.4e-05,0.068,0.068,0.027,0.057,0.057,0.051,3.8e-05,3.8e-05,8.3e-06,0.037,0.037,0.0091,0,0,0,0,0,0,0,0 -13390000,0.7,0.00082,-0.014,0.71,0.00065,0.0061,-0.02,0.0033,0.0011,-3.7e+02,-0.0012,-0.0059,-0.00013,0.00029,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00021,0.00021,3.4e-05,0.056,0.056,0.026,0.049,0.049,0.05,3.6e-05,3.6e-05,8.2e-06,0.037,0.037,0.0088,0,0,0,0,0,0,0,0 -13490000,0.7,0.00085,-0.014,0.71,0.00015,0.006,-0.019,0.0034,0.0017,-3.7e+02,-0.0012,-0.0059,-0.00012,0.0002,0.0014,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00021,0.00021,3.4e-05,0.064,0.064,0.028,0.057,0.057,0.05,3.6e-05,3.6e-05,8.1e-06,0.037,0.037,0.0087,0,0,0,0,0,0,0,0 -13590000,0.7,0.00079,-0.014,0.71,0.00039,0.0062,-0.021,0.0024,0.001,-3.7e+02,-0.0012,-0.0059,-0.00013,0.00017,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.053,0.053,0.028,0.049,0.049,0.05,3.4e-05,3.4e-05,8.1e-06,0.036,0.036,0.0084,0,0,0,0,0,0,0,0 -13690000,0.7,0.00077,-0.014,0.71,0.0011,0.008,-0.025,0.0025,0.0017,-3.7e+02,-0.0012,-0.0059,-0.00011,0.0002,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00021,0.00021,3.4e-05,0.06,0.06,0.029,0.056,0.056,0.05,3.4e-05,3.4e-05,8e-06,0.036,0.036,0.0083,0,0,0,0,0,0,0,0 -13790000,0.7,0.00065,-0.014,0.71,0.0016,0.0038,-0.027,0.0035,-0.00062,-3.7e+02,-0.0011,-0.006,-0.00011,-1.6e-05,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.051,0.051,0.029,0.048,0.048,0.049,3.3e-05,3.3e-05,7.9e-06,0.036,0.036,0.0079,0,0,0,0,0,0,0,0 -13890000,0.7,0.00062,-0.014,0.71,0.0021,0.0037,-0.031,0.0037,-0.00027,-3.7e+02,-0.0011,-0.006,-9.1e-05,1.6e-05,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00021,0.00021,3.4e-05,0.057,0.057,0.03,0.056,0.056,0.05,3.3e-05,3.3e-05,7.8e-06,0.036,0.036,0.0078,0,0,0,0,0,0,0,0 -13990000,0.7,0.00055,-0.014,0.71,0.0024,0.0012,-0.03,0.0045,-0.002,-3.7e+02,-0.0011,-0.006,-8.7e-05,-0.00025,0.001,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.048,0.048,0.03,0.048,0.048,0.05,3.1e-05,3.1e-05,7.8e-06,0.036,0.036,0.0074,0,0,0,0,0,0,0,0 -14090000,0.7,0.00053,-0.014,0.71,0.0024,0.0014,-0.031,0.0047,-0.0019,-3.7e+02,-0.0011,-0.006,-5.9e-05,-0.00024,0.001,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.055,0.055,0.031,0.055,0.055,0.05,3.1e-05,3.1e-05,7.7e-06,0.036,0.036,0.0073,0,0,0,0,0,0,0,0 -14190000,0.7,0.00043,-0.014,0.71,0.0058,0.00081,-0.033,0.0068,-0.0016,-3.7e+02,-0.0011,-0.006,-4.4e-05,-0.00022,0.00074,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.046,0.046,0.031,0.048,0.048,0.05,2.9e-05,2.9e-05,7.6e-06,0.036,0.036,0.0069,0,0,0,0,0,0,0,0 -14290000,0.7,0.00044,-0.014,0.71,0.0065,0.0016,-0.032,0.0074,-0.0015,-3.7e+02,-0.0011,-0.006,-3.2e-05,-0.00029,0.0008,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.052,0.052,0.032,0.055,0.055,0.051,2.9e-05,2.9e-05,7.5e-06,0.036,0.036,0.0067,0,0,0,0,0,0,0,0 -14390000,0.7,0.00036,-0.014,0.71,0.0083,0.0025,-0.034,0.0087,-0.0013,-3.7e+02,-0.0011,-0.006,-3.1e-06,-0.0003,0.00056,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.045,0.045,0.031,0.048,0.048,0.05,2.8e-05,2.8e-05,7.4e-06,0.036,0.036,0.0063,0,0,0,0,0,0,0,0 -14490000,0.7,0.00034,-0.014,0.71,0.0083,0.0037,-0.037,0.0096,-0.001,-3.7e+02,-0.0011,-0.006,7e-06,-0.00024,0.00052,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.05,0.05,0.032,0.055,0.055,0.051,2.8e-05,2.8e-05,7.4e-06,0.036,0.036,0.0062,0,0,0,0,0,0,0,0 -14590000,0.7,0.00033,-0.013,0.71,0.0048,0.0021,-0.037,0.006,-0.0024,-3.7e+02,-0.0011,-0.006,1e-05,-0.00062,0.00092,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.043,0.043,0.031,0.047,0.047,0.051,2.6e-05,2.6e-05,7.3e-06,0.036,0.036,0.0058,0,0,0,0,0,0,0,0 -14690000,0.7,0.00029,-0.013,0.71,0.0062,-0.00082,-0.034,0.0066,-0.0024,-3.7e+02,-0.0011,-0.006,3.3e-05,-0.00072,0.001,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.0002,3.4e-05,0.049,0.049,0.032,0.054,0.054,0.051,2.6e-05,2.6e-05,7.2e-06,0.036,0.036,0.0056,0,0,0,0,0,0,0,0 -14790000,0.7,0.00031,-0.013,0.71,0.0029,-0.0024,-0.03,0.0037,-0.0034,-3.7e+02,-0.0012,-0.0061,4.1e-05,-0.0011,0.0015,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.042,0.042,0.031,0.047,0.047,0.051,2.5e-05,2.5e-05,7.1e-06,0.036,0.036,0.0053,0,0,0,0,0,0,0,0 -14890000,0.7,0.00031,-0.013,0.71,0.0044,-0.0014,-0.033,0.0041,-0.0036,-3.7e+02,-0.0012,-0.0061,5.6e-05,-0.0011,0.0015,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.0002,0.00019,3.4e-05,0.048,0.047,0.031,0.054,0.054,0.052,2.5e-05,2.5e-05,7.1e-06,0.036,0.036,0.0051,0,0,0,0,0,0,0,0 -14990000,0.7,0.0003,-0.013,0.71,0.0033,-0.0016,-0.029,0.0032,-0.0029,-3.7e+02,-0.0012,-0.0061,5.4e-05,-0.0012,0.0017,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.041,0.041,0.03,0.047,0.047,0.051,2.3e-05,2.3e-05,7e-06,0.036,0.036,0.0048,0,0,0,0,0,0,0,0 -15090000,0.7,0.00023,-0.013,0.71,0.0036,-0.0018,-0.032,0.0035,-0.003,-3.7e+02,-0.0012,-0.0061,5.7e-05,-0.0011,0.0017,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.046,0.046,0.031,0.054,0.054,0.052,2.3e-05,2.3e-05,6.9e-06,0.036,0.036,0.0046,0,0,0,0,0,0,0,0 -15190000,0.7,0.00024,-0.013,0.71,0.003,-0.00058,-0.029,0.0028,-0.0024,-3.7e+02,-0.0012,-0.0061,5.6e-05,-0.0011,0.0019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.04,0.04,0.03,0.047,0.047,0.052,2.2e-05,2.2e-05,6.8e-06,0.035,0.035,0.0043,0,0,0,0,0,0,0,0 -15290000,0.7,0.00021,-0.013,0.71,0.0036,-0.0004,-0.027,0.0031,-0.0025,-3.7e+02,-0.0012,-0.0061,7.2e-05,-0.0012,0.002,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00019,0.00019,3.4e-05,0.045,0.045,0.03,0.054,0.054,0.052,2.2e-05,2.2e-05,6.7e-06,0.035,0.035,0.0042,0,0,0,0,0,0,0,0 -15390000,0.7,0.00021,-0.013,0.71,0.0028,-7.6e-05,-0.024,0.00054,-0.002,-3.7e+02,-0.0012,-0.0061,0.00011,-0.0013,0.0022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00018,0.00018,3.4e-05,0.04,0.04,0.029,0.047,0.047,0.051,2e-05,2e-05,6.7e-06,0.035,0.035,0.0039,0,0,0,0,0,0,0,0 -15490000,0.7,0.00023,-0.013,0.71,0.0041,-0.00044,-0.024,0.00089,-0.002,-3.7e+02,-0.0012,-0.0061,9.1e-05,-0.0012,0.0022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.7e-05,0.00019,0.00019,3.4e-05,0.045,0.045,0.029,0.053,0.053,0.053,2e-05,2e-05,6.6e-06,0.035,0.035,0.0037,0,0,0,0,0,0,0,0 -15590000,0.7,0.00024,-0.013,0.71,0.0022,-0.00045,-0.023,-0.0013,-0.0017,-3.7e+02,-0.0012,-0.0061,8.2e-05,-0.0013,0.0024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00018,0.00018,3.4e-05,0.039,0.039,0.028,0.046,0.046,0.052,1.9e-05,1.9e-05,6.5e-06,0.035,0.035,0.0035,0,0,0,0,0,0,0,0 -15690000,0.7,0.00025,-0.013,0.71,0.0025,-0.00061,-0.023,-0.0011,-0.0017,-3.7e+02,-0.0012,-0.0061,8.4e-05,-0.0013,0.0024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00018,0.00018,3.4e-05,0.044,0.044,0.028,0.053,0.053,0.052,1.9e-05,1.9e-05,6.4e-06,0.035,0.035,0.0033,0,0,0,0,0,0,0,0 -15790000,0.7,0.00021,-0.013,0.71,0.003,-0.0023,-0.026,-0.00099,-0.0028,-3.7e+02,-0.0012,-0.0061,8.7e-05,-0.0015,0.0024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00018,0.00018,3.4e-05,0.038,0.038,0.027,0.046,0.046,0.051,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.0031,0,0,0,0,0,0,0,0 -15890000,0.7,0.00016,-0.013,0.71,0.0039,-0.0028,-0.024,-0.00062,-0.0031,-3.7e+02,-0.0012,-0.0061,9.3e-05,-0.0015,0.0024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00018,0.00018,3.4e-05,0.043,0.043,0.027,0.053,0.053,0.052,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.003,0,0,0,0,0,0,0,0 -15990000,0.7,0.0001,-0.013,0.71,0.0038,-0.0037,-0.019,-0.00068,-0.0038,-3.7e+02,-0.0012,-0.0061,0.00012,-0.0018,0.0026,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00017,0.00017,3.4e-05,0.038,0.038,0.026,0.046,0.046,0.051,1.6e-05,1.6e-05,6.2e-06,0.034,0.034,0.0028,0,0,0,0,0,0,0,0 -16090000,0.7,0.0001,-0.013,0.71,0.0055,-0.0039,-0.016,-0.00024,-0.0042,-3.7e+02,-0.0012,-0.0061,0.00015,-0.0019,0.0027,-0.12,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00018,0.00018,3.4e-05,0.043,0.043,0.025,0.053,0.053,0.052,1.6e-05,1.6e-05,6.1e-06,0.034,0.034,0.0027,0,0,0,0,0,0,0,0 -16190000,0.7,0.00013,-0.013,0.71,0.0055,-0.0031,-0.014,-0.00043,-0.0034,-3.7e+02,-0.0012,-0.0061,0.00016,-0.0018,0.0029,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00017,0.00017,3.4e-05,0.037,0.037,0.025,0.046,0.046,0.051,1.5e-05,1.5e-05,6e-06,0.034,0.034,0.0025,0,0,0,0,0,0,0,0 -16290000,0.7,0.00015,-0.013,0.71,0.0071,-0.0039,-0.016,0.0002,-0.0038,-3.7e+02,-0.0012,-0.0061,0.00019,-0.0018,0.0029,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00017,0.00017,3.4e-05,0.042,0.042,0.024,0.053,0.053,0.052,1.5e-05,1.5e-05,6e-06,0.034,0.034,0.0024,0,0,0,0,0,0,0,0 -16390000,0.7,0.00013,-0.013,0.71,0.006,-0.0042,-0.015,-0.0001,-0.003,-3.7e+02,-0.0013,-0.0061,0.00019,-0.0016,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00017,0.00017,3.4e-05,0.037,0.037,0.023,0.046,0.046,0.051,1.4e-05,1.4e-05,5.9e-06,0.034,0.034,0.0022,0,0,0,0,0,0,0,0 -16490000,0.7,0.00015,-0.013,0.71,0.0052,-0.0037,-0.018,0.00043,-0.0034,-3.7e+02,-0.0013,-0.0061,0.0002,-0.0016,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00017,0.00017,3.4e-05,0.041,0.041,0.023,0.052,0.052,0.052,1.4e-05,1.4e-05,5.8e-06,0.034,0.034,0.0021,0,0,0,0,0,0,0,0 -16590000,0.7,0.00041,-0.013,0.71,0.0016,-0.001,-0.018,-0.0025,-2.7e-05,-3.7e+02,-0.0013,-0.006,0.0002,-0.00093,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00016,0.00016,3.4e-05,0.036,0.036,0.022,0.046,0.046,0.051,1.2e-05,1.2e-05,5.7e-06,0.033,0.033,0.002,0,0,0,0,0,0,0,0 -16690000,0.7,0.0004,-0.013,0.71,0.0018,-0.00052,-0.015,-0.0023,-0.0001,-3.7e+02,-0.0013,-0.006,0.00019,-0.00099,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00017,0.00016,3.4e-05,0.041,0.041,0.022,0.052,0.052,0.051,1.2e-05,1.2e-05,5.7e-06,0.033,0.033,0.0019,0,0,0,0,0,0,0,0 -16790000,0.7,0.00054,-0.013,0.71,-0.0016,0.0017,-0.014,-0.0047,0.0026,-3.7e+02,-0.0013,-0.006,0.0002,-0.00042,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00016,0.00016,3.4e-05,0.036,0.036,0.021,0.046,0.046,0.05,1.1e-05,1.1e-05,5.6e-06,0.033,0.033,0.0018,0,0,0,0,0,0,0,0 -16890000,0.7,0.00056,-0.013,0.71,-0.0019,0.0025,-0.011,-0.0048,0.0028,-3.7e+02,-0.0013,-0.006,0.00019,-0.00046,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00016,0.00016,3.4e-05,0.04,0.04,0.021,0.052,0.052,0.051,1.1e-05,1.1e-05,5.5e-06,0.033,0.033,0.0017,0,0,0,0,0,0,0,0 -16990000,0.7,0.00049,-0.013,0.71,-0.0018,0.00051,-0.01,-0.0053,0.0009,-3.7e+02,-0.0013,-0.006,0.00018,-0.00088,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00015,0.00015,3.3e-05,0.035,0.035,0.02,0.046,0.046,0.05,1e-05,1e-05,5.5e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 -17090000,0.7,0.00046,-0.013,0.71,-0.001,0.0015,-0.01,-0.0054,0.00098,-3.7e+02,-0.0013,-0.006,0.00018,-0.00087,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00016,0.00016,3.3e-05,0.039,0.039,0.02,0.052,0.052,0.05,1e-05,1e-05,5.4e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 -17190000,0.71,0.00045,-0.013,0.71,-0.00056,0.0014,-0.011,-0.0057,-0.00048,-3.7e+02,-0.0014,-0.006,0.0002,-0.0012,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00015,0.00015,3.3e-05,0.035,0.035,0.019,0.046,0.046,0.049,9.4e-06,9.4e-06,5.3e-06,0.032,0.032,0.0015,0,0,0,0,0,0,0,0 -17290000,0.7,0.00042,-0.013,0.71,0.0015,0.0025,-0.0066,-0.0056,-0.0003,-3.7e+02,-0.0014,-0.006,0.00019,-0.0013,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00015,0.00015,3.3e-05,0.039,0.039,0.019,0.052,0.052,0.049,9.4e-06,9.4e-06,5.2e-06,0.032,0.032,0.0014,0,0,0,0,0,0,0,0 -17390000,0.71,0.00039,-0.013,0.71,0.0022,0.0017,-0.0047,-0.0047,-0.0016,-3.7e+02,-0.0014,-0.006,0.00021,-0.0016,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00015,0.00015,3.3e-05,0.034,0.034,0.018,0.046,0.046,0.048,8.5e-06,8.5e-06,5.2e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 -17490000,0.71,0.00038,-0.013,0.71,0.0027,0.0012,-0.003,-0.0045,-0.0014,-3.7e+02,-0.0014,-0.006,0.00021,-0.0016,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00015,0.00015,3.3e-05,0.038,0.038,0.018,0.052,0.052,0.049,8.5e-06,8.5e-06,5.1e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 -17590000,0.71,0.00029,-0.013,0.71,0.004,5.6e-05,0.0025,-0.0038,-0.0026,-3.7e+02,-0.0014,-0.0061,0.00022,-0.002,0.0052,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00014,0.00014,3.3e-05,0.034,0.034,0.017,0.045,0.045,0.048,7.7e-06,7.7e-06,5e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 -17690000,0.71,0.00026,-0.013,0.71,0.0048,0.00078,0.0019,-0.0033,-0.0025,-3.7e+02,-0.0014,-0.0061,0.00023,-0.002,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00014,0.00014,3.3e-05,0.037,0.037,0.017,0.052,0.052,0.048,7.7e-06,7.7e-06,5e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 -17790000,0.71,0.00017,-0.013,0.71,0.0074,0.00048,0.00059,-0.0021,-0.0022,-3.7e+02,-0.0013,-0.0061,0.00027,-0.0019,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00014,0.00014,3.3e-05,0.033,0.033,0.016,0.045,0.045,0.048,7e-06,7e-06,4.9e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 -17890000,0.71,0.00018,-0.013,0.71,0.0089,-0.00027,0.00069,-0.0013,-0.0021,-3.7e+02,-0.0013,-0.0061,0.00029,-0.0019,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00014,0.00014,3.3e-05,0.037,0.037,0.016,0.052,0.052,0.048,7e-06,7e-06,4.8e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 -17990000,0.71,0.00012,-0.013,0.71,0.011,-0.002,0.0019,-0.0006,-0.0018,-3.7e+02,-0.0013,-0.0061,0.00028,-0.0019,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00013,0.00013,3.3e-05,0.032,0.032,0.016,0.045,0.045,0.047,6.4e-06,6.4e-06,4.8e-06,0.031,0.031,0.001,0,0,0,0,0,0,0,0 -18090000,0.71,0.00013,-0.013,0.71,0.011,-0.0022,0.0043,0.00051,-0.0021,-3.7e+02,-0.0013,-0.0061,0.00026,-0.0019,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00014,0.00014,3.3e-05,0.036,0.036,0.016,0.051,0.051,0.047,6.4e-06,6.4e-06,4.7e-06,0.031,0.031,0.00097,0,0,0,0,0,0,0,0 -18190000,0.71,9.6e-05,-0.013,0.71,0.012,-0.0011,0.0056,0.0014,-0.0016,-3.7e+02,-0.0013,-0.0061,0.00028,-0.0018,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00013,0.00013,3.3e-05,0.032,0.032,0.015,0.045,0.045,0.047,5.8e-06,5.8e-06,4.7e-06,0.031,0.031,0.00092,0,0,0,0,0,0,0,0 -18290000,0.71,3.7e-05,-0.012,0.71,0.012,-0.0017,0.0068,0.0026,-0.0018,-3.7e+02,-0.0013,-0.0061,0.00027,-0.0018,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00013,0.00013,3.3e-05,0.035,0.035,0.015,0.051,0.051,0.046,5.8e-06,5.8e-06,4.6e-06,0.031,0.031,0.00089,0,0,0,0,0,0,0,0 -18390000,0.71,5.2e-05,-0.013,0.71,0.013,-4.9e-05,0.008,0.0031,-0.0013,-3.7e+02,-0.0013,-0.0061,0.0003,-0.0018,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00013,0.00013,3.3e-05,0.031,0.031,0.014,0.045,0.045,0.046,5.2e-06,5.2e-06,4.5e-06,0.031,0.031,0.00084,0,0,0,0,0,0,0,0 -18490000,0.71,6.7e-05,-0.012,0.71,0.014,0.00037,0.0076,0.0046,-0.0013,-3.7e+02,-0.0013,-0.0061,0.0003,-0.0018,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00013,0.00013,3.3e-05,0.034,0.034,0.014,0.051,0.051,0.046,5.2e-06,5.2e-06,4.5e-06,0.031,0.031,0.00082,0,0,0,0,0,0,0,0 -18590000,0.71,7.1e-05,-0.012,0.71,0.013,0.0006,0.0058,0.0035,-0.0011,-3.7e+02,-0.0014,-0.0061,0.00033,-0.0018,0.0052,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00012,0.00012,3.3e-05,0.03,0.03,0.014,0.045,0.045,0.045,4.8e-06,4.8e-06,4.4e-06,0.031,0.031,0.00078,0,0,0,0,0,0,0,0 -18690000,0.71,4e-05,-0.012,0.71,0.014,-8.7e-05,0.0039,0.0048,-0.0011,-3.7e+02,-0.0014,-0.0061,0.00033,-0.0018,0.0052,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00013,0.00013,3.3e-05,0.033,0.033,0.013,0.051,0.051,0.045,4.8e-06,4.8e-06,4.3e-06,0.031,0.031,0.00075,0,0,0,0,0,0,0,0 -18790000,0.71,6.9e-05,-0.012,0.71,0.012,0.0002,0.0035,0.0037,-0.00085,-3.7e+02,-0.0014,-0.006,0.00032,-0.0018,0.0055,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.6e-05,0.00012,0.00012,3.3e-05,0.03,0.03,0.013,0.045,0.045,0.045,4.3e-06,4.3e-06,4.3e-06,0.031,0.031,0.00072,0,0,0,0,0,0,0,0 -18890000,0.71,9.3e-05,-0.012,0.71,0.013,0.00071,0.0042,0.0049,-0.00077,-3.7e+02,-0.0014,-0.006,0.00035,-0.0018,0.0055,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00012,0.00012,3.3e-05,0.033,0.033,0.013,0.051,0.051,0.045,4.3e-06,4.3e-06,4.2e-06,0.031,0.031,0.0007,0,0,0,0,0,0,0,0 -18990000,0.71,8e-05,-0.012,0.71,0.014,0.0016,0.0028,0.0063,-0.00065,-3.7e+02,-0.0014,-0.0061,0.00036,-0.0018,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00012,0.00012,3.3e-05,0.029,0.029,0.012,0.045,0.045,0.044,3.9e-06,3.9e-06,4.2e-06,0.031,0.031,0.00066,0,0,0,0,0,0,0,0 -19090000,0.71,6.4e-05,-0.012,0.71,0.014,0.0022,0.0059,0.0077,-0.00043,-3.7e+02,-0.0014,-0.0061,0.00036,-0.0018,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00012,0.00012,3.3e-05,0.032,0.032,0.012,0.051,0.051,0.044,3.9e-06,3.9e-06,4.1e-06,0.031,0.031,0.00065,0,0,0,0,0,0,0,0 -19190000,0.71,6.6e-05,-0.012,0.71,0.014,0.0022,0.0059,0.0085,-0.00039,-3.7e+02,-0.0014,-0.0061,0.00037,-0.0018,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00012,0.00012,3.3e-05,0.028,0.028,0.012,0.045,0.045,0.044,3.6e-06,3.6e-06,4.1e-06,0.03,0.03,0.00062,0,0,0,0,0,0,0,0 -19290000,0.71,8.8e-05,-0.012,0.71,0.015,0.0014,0.0086,0.0099,-0.00019,-3.7e+02,-0.0014,-0.0061,0.00037,-0.0018,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00012,0.00012,3.2e-05,0.031,0.031,0.012,0.05,0.05,0.044,3.6e-06,3.6e-06,4e-06,0.03,0.03,0.0006,0,0,0,0,0,0,0,0 -19390000,0.71,9.9e-05,-0.012,0.71,0.012,0.00051,0.012,0.008,-0.00022,-3.7e+02,-0.0014,-0.0061,0.00038,-0.0019,0.0058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.027,0.027,0.012,0.044,0.044,0.043,3.3e-06,3.3e-06,4e-06,0.03,0.03,0.00058,0,0,0,0,0,0,0,0 -19490000,0.71,0.00012,-0.012,0.71,0.011,-0.0002,0.0088,0.0091,-0.00021,-3.7e+02,-0.0014,-0.0061,0.0004,-0.0019,0.0058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.03,0.03,0.011,0.05,0.05,0.043,3.3e-06,3.3e-06,3.9e-06,0.03,0.03,0.00056,0,0,0,0,0,0,0,0 -19590000,0.71,0.00017,-0.012,0.71,0.0095,-0.0012,0.0081,0.0074,-0.00024,-3.7e+02,-0.0014,-0.0061,0.00043,-0.0019,0.0061,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.027,0.027,0.011,0.044,0.044,0.042,3e-06,3e-06,3.9e-06,0.03,0.03,0.00054,0,0,0,0,0,0,0,0 -19690000,0.71,0.00017,-0.012,0.71,0.0099,-0.0034,0.0096,0.0083,-0.00048,-3.7e+02,-0.0014,-0.0061,0.00042,-0.0019,0.0061,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.029,0.029,0.011,0.05,0.05,0.042,3e-06,3e-06,3.8e-06,0.03,0.03,0.00052,0,0,0,0,0,0,0,0 -19790000,0.71,0.00024,-0.012,0.71,0.0075,-0.0043,0.01,0.0068,-0.00039,-3.7e+02,-0.0014,-0.0061,0.00042,-0.0019,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.026,0.026,0.011,0.044,0.044,0.042,2.7e-06,2.7e-06,3.7e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19890000,0.71,0.00019,-0.012,0.71,0.0063,-0.0045,0.011,0.0075,-0.00084,-3.7e+02,-0.0014,-0.0061,0.00045,-0.0019,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.029,0.029,0.011,0.05,0.05,0.042,2.7e-06,2.7e-06,3.7e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19990000,0.71,0.00017,-0.012,0.71,0.0039,-0.0053,0.014,0.0061,-0.00071,-3.7e+02,-0.0014,-0.006,0.00049,-0.0018,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.026,0.026,0.01,0.044,0.044,0.041,2.5e-06,2.5e-06,3.6e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20090000,0.71,0.00017,-0.012,0.71,0.0036,-0.0072,0.014,0.0064,-0.0013,-3.7e+02,-0.0014,-0.006,0.00052,-0.0018,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.00011,0.00011,3.2e-05,0.028,0.028,0.01,0.049,0.049,0.042,2.5e-06,2.5e-06,3.6e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20190000,0.71,0.00027,-0.012,0.71,0.0013,-0.0079,0.017,0.0042,-0.001,-3.7e+02,-0.0014,-0.006,0.00054,-0.0017,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-05,0.0001,0.0001,3.2e-05,0.025,0.025,0.01,0.044,0.044,0.041,2.3e-06,2.3e-06,3.6e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20290000,0.71,0.00023,-0.012,0.71,0.00017,-0.0095,0.015,0.0042,-0.0019,-3.7e+02,-0.0014,-0.006,0.00055,-0.0017,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.00011,0.00011,3.2e-05,0.027,0.027,0.0099,0.049,0.049,0.041,2.3e-06,2.3e-06,3.5e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20390000,0.71,0.00025,-0.012,0.71,-0.0023,-0.01,0.017,0.0024,-0.0015,-3.7e+02,-0.0014,-0.006,0.00055,-0.0015,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.0001,0.0001,3.2e-05,0.024,0.024,0.0097,0.044,0.044,0.041,2.1e-06,2.1e-06,3.5e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20490000,0.71,0.00031,-0.012,0.71,-0.0027,-0.011,0.016,0.0021,-0.0025,-3.7e+02,-0.0014,-0.006,0.00055,-0.0015,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.0001,0.0001,3.2e-05,0.026,0.026,0.0096,0.049,0.049,0.041,2.1e-06,2.1e-06,3.4e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20590000,0.71,0.00033,-0.012,0.71,-0.0024,-0.011,0.013,0.0018,-0.002,-3.7e+02,-0.0014,-0.006,0.00054,-0.0014,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.0001,0.0001,3.2e-05,0.024,0.024,0.0094,0.044,0.044,0.04,1.9e-06,1.9e-06,3.4e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20690000,0.71,0.00035,-0.012,0.71,-0.0024,-0.012,0.015,0.0015,-0.0032,-3.7e+02,-0.0014,-0.006,0.00055,-0.0014,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.0001,0.0001,3.2e-05,0.026,0.026,0.0093,0.049,0.049,0.04,1.9e-06,1.9e-06,3.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20790000,0.71,0.00038,-0.012,0.71,-0.0035,-0.011,0.015,0.0013,-0.0025,-3.7e+02,-0.0014,-0.006,0.00056,-0.0012,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.0001,0.0001,3.1e-05,0.023,0.023,0.0091,0.043,0.043,0.04,1.8e-06,1.8e-06,3.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20890000,0.71,0.00037,-0.012,0.71,-0.0039,-0.014,0.014,0.00092,-0.0037,-3.7e+02,-0.0014,-0.006,0.00058,-0.0012,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,0.0001,0.0001,3.1e-05,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-06,1.8e-06,3.2e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -20990000,0.71,0.00037,-0.012,0.71,-0.0041,-0.014,0.015,0.0026,-0.0031,-3.7e+02,-0.0014,-0.006,0.00058,-0.00095,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,9.9e-05,9.8e-05,3.1e-05,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-06,1.7e-06,3.2e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -21090000,0.71,0.00037,-0.012,0.71,-0.0043,-0.017,0.015,0.0022,-0.0046,-3.7e+02,-0.0014,-0.006,0.00059,-0.00096,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,9.9e-05,9.9e-05,3.1e-05,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-06,1.7e-06,3.1e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -21190000,0.71,0.00041,-0.012,0.71,-0.0035,-0.016,0.014,0.0037,-0.0038,-3.7e+02,-0.0014,-0.006,0.00059,-0.00071,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,9.7e-05,9.7e-05,3.1e-05,0.022,0.022,0.0087,0.043,0.043,0.039,1.5e-06,1.5e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21290000,0.71,0.00045,-0.012,0.71,-0.0041,-0.018,0.016,0.0033,-0.0054,-3.7e+02,-0.0014,-0.006,0.00061,-0.00071,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,9.8e-05,9.8e-05,3.1e-05,0.024,0.024,0.0086,0.048,0.048,0.039,1.5e-06,1.5e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21390000,0.71,0.00049,-0.012,0.71,-0.0049,-0.017,0.016,0.0028,-0.0034,-3.7e+02,-0.0014,-0.006,0.0006,-0.00033,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,9.6e-05,9.6e-05,3.1e-05,0.022,0.022,0.0085,0.043,0.043,0.039,1.4e-06,1.4e-06,3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21490000,0.71,0.0005,-0.012,0.71,-0.0054,-0.018,0.015,0.0022,-0.0051,-3.7e+02,-0.0014,-0.006,0.00061,-0.00034,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.4e-05,9.7e-05,9.7e-05,3.1e-05,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-06,1.4e-06,3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21590000,0.71,0.00052,-0.012,0.71,-0.0059,-0.015,0.015,0.0018,-0.0031,-3.7e+02,-0.0014,-0.006,0.00061,1.1e-05,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.5e-05,9.5e-05,3.1e-05,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-06,1.3e-06,2.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21690000,0.71,0.00053,-0.012,0.71,-0.0058,-0.016,0.017,0.0012,-0.0047,-3.7e+02,-0.0014,-0.006,0.00062,8.7e-06,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.6e-05,9.5e-05,3.1e-05,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-06,1.3e-06,2.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21790000,0.71,0.00055,-0.012,0.71,-0.0064,-0.011,0.015,7.3e-06,-0.00073,-3.7e+02,-0.0014,-0.0059,0.0006,0.00054,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.4e-05,9.4e-05,3.1e-05,0.021,0.021,0.0082,0.042,0.042,0.038,1.2e-06,1.2e-06,2.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21890000,0.71,0.00055,-0.012,0.71,-0.0064,-0.012,0.016,-0.00064,-0.0019,-3.7e+02,-0.0014,-0.0059,0.0006,0.00053,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.5e-05,9.4e-05,3.1e-05,0.022,0.022,0.0082,0.047,0.047,0.038,1.3e-06,1.3e-06,2.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21990000,0.71,0.0006,-0.012,0.71,-0.0069,-0.0091,0.016,-0.0015,0.0015,-3.7e+02,-0.0014,-0.0059,0.0006,0.00097,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.3e-05,9.3e-05,3.1e-05,0.02,0.02,0.0081,0.042,0.042,0.038,1.2e-06,1.2e-06,2.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22090000,0.71,0.00062,-0.012,0.71,-0.0072,-0.0082,0.015,-0.0022,0.00065,-3.7e+02,-0.0014,-0.0059,0.0006,0.00096,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.4e-05,9.3e-05,3e-05,0.022,0.022,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,2.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22190000,0.71,0.00059,-0.012,0.71,-0.007,-0.0073,0.015,-0.0019,0.0006,-3.7e+02,-0.0014,-0.0059,0.0006,0.001,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.2e-05,9.2e-05,3e-05,0.02,0.02,0.008,0.042,0.042,0.037,1.1e-06,1.1e-06,2.7e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22290000,0.71,0.00063,-0.012,0.71,-0.0084,-0.008,0.015,-0.0026,-0.00017,-3.7e+02,-0.0014,-0.0059,0.00059,0.001,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.3e-05,9.3e-05,3e-05,0.021,0.021,0.008,0.047,0.047,0.037,1.1e-06,1.1e-06,2.7e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22390000,0.71,0.0006,-0.012,0.71,-0.0089,-0.0075,0.017,-0.0023,-0.00016,-3.7e+02,-0.0014,-0.0059,0.0006,0.0011,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.1e-05,9.1e-05,3e-05,0.019,0.019,0.0079,0.042,0.042,0.037,1e-06,1e-06,2.7e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22490000,0.71,0.00061,-0.012,0.71,-0.0096,-0.0074,0.018,-0.0032,-0.00093,-3.7e+02,-0.0014,-0.0059,0.00059,0.0011,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.2e-05,9.2e-05,3e-05,0.021,0.021,0.0079,0.047,0.047,0.037,1e-06,1e-06,2.6e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22590000,0.71,0.00059,-0.012,0.71,-0.0093,-0.007,0.017,-0.0035,0.00015,-3.7e+02,-0.0014,-0.0059,0.0006,0.0013,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,9.1e-05,9.1e-05,3e-05,0.019,0.019,0.0078,0.042,0.042,0.036,9.7e-07,9.7e-07,2.6e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22690000,0.71,0.00062,-0.012,0.71,-0.011,-0.0067,0.018,-0.0044,-0.00053,-3.7e+02,-0.0014,-0.0059,0.0006,0.0013,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,9.1e-05,9.1e-05,3e-05,0.02,0.02,0.0079,0.046,0.046,0.037,9.7e-07,9.7e-07,2.6e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22790000,0.71,0.00061,-0.012,0.71,-0.011,-0.0055,0.019,-0.0055,-0.00043,-3.7e+02,-0.0014,-0.0059,0.00057,0.0013,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,9e-05,9e-05,3e-05,0.019,0.019,0.0078,0.042,0.042,0.036,9.2e-07,9.2e-07,2.5e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22890000,0.71,0.00062,-0.012,0.71,-0.013,-0.0051,0.021,-0.0067,-0.00096,-3.7e+02,-0.0014,-0.0059,0.00057,0.0013,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,9e-05,9e-05,3e-05,0.02,0.02,0.0078,0.046,0.046,0.036,9.2e-07,9.2e-07,2.5e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -22990000,0.71,0.0006,-0.012,0.71,-0.012,-0.0056,0.022,-0.0074,-0.00085,-3.7e+02,-0.0014,-0.0059,0.00058,0.0014,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.9e-05,8.9e-05,3e-05,0.018,0.018,0.0078,0.041,0.041,0.036,8.7e-07,8.7e-07,2.5e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23090000,0.71,0.00056,-0.012,0.71,-0.013,-0.0056,0.022,-0.0087,-0.0014,-3.7e+02,-0.0014,-0.0059,0.00056,0.0014,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,9e-05,9e-05,3e-05,0.02,0.02,0.0078,0.046,0.046,0.036,8.7e-07,8.7e-07,2.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23190000,0.71,0.00063,-0.012,0.71,-0.015,-0.0065,0.024,-0.012,-0.0013,-3.7e+02,-0.0014,-0.0059,0.00056,0.0015,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.9e-05,8.9e-05,2.9e-05,0.018,0.018,0.0077,0.041,0.041,0.036,8.2e-07,8.2e-07,2.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23290000,0.71,0.00057,-0.012,0.71,-0.015,-0.0078,0.024,-0.014,-0.002,-3.7e+02,-0.0014,-0.0059,0.00056,0.0015,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.9e-05,8.9e-05,2.9e-05,0.019,0.019,0.0078,0.046,0.046,0.036,8.2e-07,8.2e-07,2.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23390000,0.71,0.00066,-0.012,0.71,-0.016,-0.008,0.022,-0.016,-0.0017,-3.7e+02,-0.0014,-0.0059,0.00055,0.0015,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.8e-05,8.8e-05,2.9e-05,0.018,0.018,0.0077,0.041,0.041,0.036,7.8e-07,7.8e-07,2.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23490000,0.71,0.0031,-0.0096,0.71,-0.023,-0.0089,-0.012,-0.018,-0.0026,-3.7e+02,-0.0014,-0.0059,0.00056,0.0015,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.9e-05,8.8e-05,2.9e-05,0.019,0.019,0.0078,0.045,0.045,0.036,7.8e-07,7.8e-07,2.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23590000,0.71,0.0083,-0.0018,0.71,-0.034,-0.0076,-0.044,-0.017,-0.0013,-3.7e+02,-0.0014,-0.0059,0.00055,0.0017,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.8e-05,8.8e-05,2.9e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.4e-07,7.4e-07,2.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23690000,0.71,0.0079,0.004,0.71,-0.065,-0.016,-0.094,-0.021,-0.0024,-3.7e+02,-0.0014,-0.0059,0.00055,0.0017,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.8e-05,8.8e-05,2.9e-05,0.019,0.019,0.0078,0.045,0.045,0.036,7.4e-07,7.4e-07,2.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23790000,0.71,0.005,0.00064,0.71,-0.089,-0.027,-0.15,-0.021,-0.0017,-3.7e+02,-0.0014,-0.0059,0.00055,0.002,0.0059,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.2e-05,8.7e-05,8.7e-05,2.9e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.1e-07,7.1e-07,2.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23890000,0.71,0.0023,-0.0054,0.71,-0.11,-0.036,-0.2,-0.031,-0.005,-3.7e+02,-0.0014,-0.0059,0.00055,0.002,0.0059,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.7e-05,8.7e-05,2.9e-05,0.019,0.019,0.0078,0.045,0.045,0.035,7.1e-07,7.1e-07,2.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -23990000,0.71,0.00095,-0.01,0.71,-0.11,-0.04,-0.25,-0.034,-0.0082,-3.7e+02,-0.0014,-0.0059,0.00055,0.0021,0.0055,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.7e-05,8.6e-05,2.9e-05,0.017,0.017,0.0077,0.041,0.041,0.035,6.7e-07,6.7e-07,2.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24090000,0.71,0.0022,-0.0088,0.71,-0.11,-0.04,-0.3,-0.045,-0.012,-3.7e+02,-0.0014,-0.0059,0.00056,0.0021,0.0055,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.7e-05,8.7e-05,2.9e-05,0.018,0.018,0.0078,0.045,0.045,0.035,6.7e-07,6.7e-07,2.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24190000,0.71,0.0033,-0.0065,0.71,-0.11,-0.041,-0.35,-0.046,-0.014,-3.7e+02,-0.0014,-0.0059,0.00056,0.0023,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.6e-05,8.6e-05,2.8e-05,0.017,0.017,0.0077,0.04,0.04,0.035,6.4e-07,6.4e-07,2.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24290000,0.71,0.0038,-0.0057,0.71,-0.12,-0.045,-0.41,-0.058,-0.018,-3.7e+02,-0.0014,-0.0059,0.00056,0.0024,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.6e-05,8.6e-05,2.8e-05,0.018,0.018,0.0078,0.045,0.045,0.036,6.4e-07,6.4e-07,2.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24390000,0.71,0.0038,-0.0059,0.71,-0.13,-0.052,-0.46,-0.064,-0.03,-3.7e+02,-0.0013,-0.0059,0.00054,0.002,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.5e-05,8.5e-05,2.8e-05,0.017,0.017,0.0078,0.04,0.04,0.035,6.2e-07,6.2e-07,2.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24490000,0.71,0.0047,-0.0018,0.71,-0.14,-0.057,-0.51,-0.077,-0.035,-3.7e+02,-0.0013,-0.0059,0.00054,0.0021,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.6e-05,8.5e-05,2.8e-05,0.018,0.018,0.0078,0.045,0.045,0.035,6.2e-07,6.2e-07,2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24590000,0.71,0.0051,0.0018,0.71,-0.16,-0.069,-0.56,-0.081,-0.045,-3.7e+02,-0.0013,-0.0059,0.00055,0.0019,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.5e-05,8.5e-05,2.8e-05,0.017,0.017,0.0078,0.04,0.04,0.035,5.9e-07,5.9e-07,2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24690000,0.71,0.0052,0.0028,0.71,-0.18,-0.082,-0.64,-0.098,-0.052,-3.7e+02,-0.0013,-0.0059,0.00056,0.0019,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.5e-05,8.5e-05,2.8e-05,0.018,0.018,0.0078,0.044,0.044,0.035,5.9e-07,5.9e-07,2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24790000,0.71,0.0049,0.0014,0.71,-0.2,-0.095,-0.72,-0.1,-0.063,-3.7e+02,-0.0013,-0.0059,0.00055,0.0023,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.4e-05,8.4e-05,2.8e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.7e-07,5.7e-07,2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24890000,0.71,0.0066,0.0031,0.71,-0.22,-0.11,-0.75,-0.13,-0.073,-3.7e+02,-0.0013,-0.0059,0.00054,0.0024,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.4e-05,8.4e-05,2.8e-05,0.018,0.018,0.0078,0.044,0.044,0.035,5.7e-07,5.7e-07,1.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -24990000,0.71,0.0084,0.0047,0.71,-0.24,-0.11,-0.81,-0.13,-0.081,-3.7e+02,-0.0013,-0.0059,0.00052,0.0033,0.0016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.1e-05,8.3e-05,8.3e-05,2.8e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.4e-07,5.4e-07,1.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25090000,0.71,0.0088,0.0041,0.71,-0.27,-0.12,-0.85,-0.15,-0.093,-3.7e+02,-0.0013,-0.0059,0.00051,0.0033,0.0015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.4e-05,8.3e-05,2.8e-05,0.018,0.017,0.0079,0.044,0.044,0.035,5.5e-07,5.5e-07,1.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25190000,0.71,0.0082,0.0027,0.71,-0.29,-0.14,-0.91,-0.17,-0.12,-3.7e+02,-0.0013,-0.0059,0.00052,0.0029,0.00098,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.3e-05,8.2e-05,2.8e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.2e-07,5.2e-07,1.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25290000,0.71,0.01,0.0095,0.71,-0.32,-0.15,-0.96,-0.2,-0.13,-3.7e+02,-0.0013,-0.0059,0.00052,0.0029,0.00098,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.3e-05,8.3e-05,2.8e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.3e-07,5.3e-07,1.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25390000,0.71,0.011,0.016,0.71,-0.35,-0.17,-1,-0.22,-0.15,-3.7e+02,-0.0012,-0.0059,0.00052,0.0031,-0.00058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.2e-05,8.2e-05,2.8e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.1e-07,5.1e-07,1.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25490000,0.71,0.012,0.017,0.71,-0.4,-0.19,-1.1,-0.25,-0.17,-3.7e+02,-0.0012,-0.0059,0.00052,0.0031,-0.00057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.2e-05,8.2e-05,2.7e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.1e-07,5.1e-07,1.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25590000,0.71,0.011,0.015,0.71,-0.44,-0.22,-1.1,-0.28,-0.21,-3.7e+02,-0.0012,-0.0059,0.00052,0.0028,-0.0015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.1e-05,8.1e-05,2.7e-05,0.016,0.016,0.0079,0.04,0.04,0.035,4.9e-07,4.9e-07,1.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25690000,0.71,0.015,0.022,0.71,-0.49,-0.24,-1.2,-0.33,-0.23,-3.7e+02,-0.0012,-0.0059,0.00052,0.0028,-0.0015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.1e-05,8.1e-05,2.7e-05,0.017,0.017,0.0079,0.044,0.044,0.035,4.9e-07,4.9e-07,1.8e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -25790000,0.71,0.017,0.028,0.71,-0.53,-0.27,-1.2,-0.34,-0.26,-3.7e+02,-0.0012,-0.0059,0.00053,0.0036,-0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8e-05,8e-05,2.7e-05,0.016,0.016,0.0079,0.04,0.04,0.035,4.7e-07,4.7e-07,1.7e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 -25890000,0.71,0.017,0.028,0.71,-0.6,-0.3,-1.3,-0.4,-0.29,-3.7e+02,-0.0012,-0.0059,0.00054,0.0036,-0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8.1e-05,8e-05,2.7e-05,0.017,0.017,0.008,0.044,0.044,0.035,4.7e-07,4.7e-07,1.7e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 -25990000,0.71,0.016,0.025,0.71,-0.66,-0.33,-1.3,-0.44,-0.34,-3.7e+02,-0.0011,-0.0059,0.00054,0.0032,-0.0058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8e-05,8e-05,2.7e-05,0.015,0.015,0.0079,0.039,0.039,0.035,4.6e-07,4.6e-07,1.7e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26090000,0.7,0.021,0.035,0.71,-0.72,-0.36,-1.3,-0.51,-0.38,-3.7e+02,-0.0011,-0.0059,0.00053,0.0032,-0.0058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,8e-05,8e-05,2.7e-05,0.017,0.016,0.008,0.043,0.043,0.035,4.6e-07,4.6e-07,1.7e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26190000,0.7,0.023,0.045,0.71,-0.78,-0.39,-1.3,-0.53,-0.42,-3.7e+02,-0.0011,-0.0059,0.00053,0.0044,-0.0096,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.9e-05,7.9e-05,2.7e-05,0.015,0.015,0.0079,0.039,0.039,0.035,4.4e-07,4.4e-07,1.7e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26290000,0.7,0.024,0.047,0.71,-0.87,-0.44,-1.3,-0.62,-0.46,-3.7e+02,-0.0011,-0.0059,0.00052,0.0044,-0.0096,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.9e-05,7.9e-05,2.7e-05,0.016,0.016,0.008,0.043,0.043,0.035,4.5e-07,4.5e-07,1.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26390000,0.7,0.023,0.043,0.71,-0.95,-0.49,-1.3,-0.68,-0.55,-3.7e+02,-0.001,-0.0059,0.00052,0.0031,-0.011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.8e-05,7.8e-05,2.7e-05,0.015,0.015,0.0079,0.039,0.039,0.035,4.3e-07,4.3e-07,1.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26490000,0.7,0.031,0.059,0.71,-1,-0.53,-1.3,-0.78,-0.6,-3.7e+02,-0.001,-0.0059,0.00052,0.003,-0.011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.8e-05,7.8e-05,2.7e-05,0.016,0.016,0.008,0.043,0.043,0.035,4.3e-07,4.3e-07,1.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26590000,0.7,0.037,0.075,0.71,-1.1,-0.59,-1.3,-0.82,-0.67,-3.7e+02,-0.00095,-0.0059,0.00049,0.004,-0.015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.8e-05,7.7e-05,2.7e-05,0.015,0.015,0.008,0.039,0.039,0.035,4.2e-07,4.2e-07,1.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26690000,0.7,0.038,0.078,0.71,-1.3,-0.65,-1.3,-0.94,-0.73,-3.7e+02,-0.00095,-0.0059,0.0005,0.004,-0.015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-05,7.8e-05,7.7e-05,2.7e-05,0.016,0.016,0.008,0.043,0.043,0.035,4.2e-07,4.2e-07,1.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26790000,0.7,0.036,0.072,0.71,-1.4,-0.73,-1.3,-1,-0.85,-3.7e+02,-0.0009,-0.006,0.00049,0.0017,-0.017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-05,7.7e-05,7.7e-05,2.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.1e-07,4.1e-07,1.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26890000,0.7,0.045,0.094,0.7,-1.5,-0.79,-1.3,-1.2,-0.93,-3.7e+02,-0.0009,-0.006,0.00049,0.0017,-0.017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-05,7.7e-05,7.7e-05,2.7e-05,0.015,0.015,0.0081,0.043,0.043,0.035,4.1e-07,4.1e-07,1.5e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -26990000,0.7,0.051,0.12,0.7,-1.7,-0.87,-1.3,-1.2,-1,-3.7e+02,-0.00079,-0.006,0.00049,0.0025,-0.022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-05,7.6e-05,7.6e-05,2.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4e-07,4e-07,1.5e-06,0.028,0.028,0.0005,0.0025,0.0023,0.0025,0.0025,0.0025,0.0025,0,0 -27090000,0.7,0.052,0.12,0.7,-1.9,-0.96,-1.3,-1.4,-1.1,-3.7e+02,-0.00079,-0.006,0.00048,0.0025,-0.022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-05,7.7e-05,7.6e-05,2.7e-05,0.015,0.015,0.0081,0.043,0.043,0.035,4e-07,4e-07,1.5e-06,0.028,0.028,0.0005,0.0025,0.0015,0.0025,0.0025,0.0025,0.0025,0,0 -27190000,0.71,0.048,0.11,0.7,-2.1,-1,-1.2,-1.6,-1.2,-3.7e+02,-0.00078,-0.0059,0.00049,0.0036,-0.022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-05,7.6e-05,7.6e-05,2.7e-05,0.015,0.015,0.0081,0.045,0.045,0.035,4e-07,4e-07,1.5e-06,0.028,0.028,0.0005,0.0025,0.0011,0.0025,0.0025,0.0025,0.0025,0,0 -27290000,0.71,0.043,0.094,0.7,-2.2,-1.1,-1.2,-1.8,-1.3,-3.7e+02,-0.00078,-0.0059,0.00049,0.0036,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.8e-05,7.7e-05,7.6e-05,2.7e-05,0.017,0.017,0.0081,0.049,0.049,0.035,4e-07,4e-07,1.5e-06,0.028,0.028,0.0005,0.0025,0.0009,0.0025,0.0025,0.0025,0.0025,0,0 -27390000,0.71,0.036,0.078,0.7,-2.3,-1.1,-1.2,-2,-1.4,-3.7e+02,-0.00072,-0.0059,0.0005,0.0056,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.8e-05,7.7e-05,7.6e-05,2.7e-05,0.017,0.017,0.0081,0.052,0.052,0.035,3.9e-07,3.9e-07,1.5e-06,0.028,0.028,0.0005,0.0025,0.00074,0.0025,0.0025,0.0025,0.0025,0,0 -27490000,0.71,0.031,0.062,0.7,-2.4,-1.2,-1.2,-2.3,-1.5,-3.7e+02,-0.00072,-0.0059,0.0005,0.0055,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.8e-05,7.7e-05,7.6e-05,2.6e-05,0.018,0.018,0.0082,0.057,0.057,0.035,3.9e-07,3.9e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.00063,0.0025,0.0025,0.0025,0.0025,0,0 -27590000,0.71,0.026,0.05,0.7,-2.5,-1.2,-1.2,-2.5,-1.6,-3.7e+02,-0.00075,-0.0059,0.0005,0.0055,-0.021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.6e-05,2.6e-05,0.018,0.018,0.0082,0.059,0.059,0.035,3.9e-07,3.9e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.00056,0.0025,0.0025,0.0025,0.0025,0,0 -27690000,0.71,0.025,0.048,0.7,-2.5,-1.2,-1.2,-2.8,-1.7,-3.7e+02,-0.00075,-0.0059,0.00049,0.0054,-0.021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.7e-05,2.6e-05,0.019,0.019,0.0082,0.065,0.065,0.035,3.9e-07,3.9e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.00049,0.0025,0.0025,0.0025,0.0025,0,0 -27790000,0.71,0.026,0.05,0.7,-2.6,-1.2,-1.2,-3,-1.8,-3.7e+02,-0.00075,-0.0058,0.00048,0.0056,-0.02,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.6e-05,2.6e-05,0.018,0.018,0.0082,0.067,0.067,0.035,3.8e-07,3.8e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.00044,0.0025,0.0025,0.0025,0.0025,0,0 -27890000,0.71,0.025,0.048,0.7,-2.6,-1.2,-1.2,-3.3,-2,-3.7e+02,-0.00075,-0.0058,0.00048,0.0054,-0.02,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.6e-05,2.6e-05,0.02,0.02,0.0083,0.074,0.073,0.035,3.8e-07,3.8e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.0004,0.0025,0.0025,0.0025,0.0025,0,0 -27990000,0.71,0.024,0.045,0.7,-2.7,-1.2,-1.2,-3.6,-2.1,-3.7e+02,-0.00079,-0.0058,0.00049,0.005,-0.018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.6e-05,2.6e-05,0.019,0.019,0.0083,0.076,0.076,0.035,3.8e-07,3.8e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.00037,0.0025,0.0025,0.0025,0.0025,0,0 -28090000,0.71,0.03,0.058,0.7,-2.7,-1.3,-1.2,-3.9,-2.2,-3.7e+02,-0.00079,-0.0058,0.00047,0.0049,-0.018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.6e-05,2.6e-05,0.02,0.02,0.0084,0.083,0.082,0.035,3.8e-07,3.8e-07,1.4e-06,0.028,0.028,0.0005,0.0025,0.00034,0.0025,0.0025,0.0025,0.0025,0,0 -28190000,0.71,0.035,0.071,0.7,-2.8,-1.3,-0.95,-4.2,-2.3,-3.7e+02,-0.00082,-0.0058,0.00047,0.0046,-0.016,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.7e-05,7.7e-05,7.6e-05,2.6e-05,0.02,0.02,0.0084,0.085,0.085,0.035,3.7e-07,3.7e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.00032,0.0025,0.0025,0.0025,0.0025,0,0 -28290000,0.71,0.028,0.054,0.7,-2.8,-1.3,-0.089,-4.4,-2.4,-3.7e+02,-0.00082,-0.0058,0.00045,0.0044,-0.016,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.7e-05,7.6e-05,2.6e-05,0.02,0.02,0.0086,0.092,0.092,0.036,3.7e-07,3.7e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.0003,0.0025,0.0025,0.0025,0.0025,0,0 -28390000,0.71,0.011,0.023,0.7,-2.8,-1.3,0.77,-4.7,-2.6,-3.7e+02,-0.00082,-0.0058,0.00044,0.0042,-0.015,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.7e-05,7.7e-05,2.5e-05,0.021,0.021,0.0087,0.099,0.099,0.036,3.7e-07,3.8e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.00028,0.0025,0.0025,0.0025,0.0025,0,0 -28490000,0.71,0.0026,0.0044,0.7,-2.7,-1.3,1.1,-5,-2.7,-3.7e+02,-0.00082,-0.0058,0.00044,0.0039,-0.014,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.8e-05,7.7e-05,2.5e-05,0.022,0.022,0.0088,0.11,0.11,0.036,3.8e-07,3.8e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.00026,0.0025,0.0025,0.0025,0.0025,0,0 -28590000,0.71,0.00079,0.00091,0.7,-2.6,-1.3,0.96,-5.3,-2.8,-3.7e+02,-0.00082,-0.0058,0.00044,0.0036,-0.013,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.8e-05,7.7e-05,2.5e-05,0.023,0.024,0.0089,0.12,0.12,0.036,3.8e-07,3.8e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.00025,0.0025,0.0025,0.0025,0.0025,0,0 -28690000,0.71,8.9e-05,-2.1e-06,0.7,-2.6,-1.2,0.96,-5.5,-2.9,-3.7e+02,-0.00082,-0.0058,0.00043,0.0031,-0.012,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.8e-05,7.7e-05,2.5e-05,0.025,0.025,0.009,0.12,0.12,0.036,3.8e-07,3.8e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.00023,0.0025,0.0025,0.0025,0.0025,0,0 -28790000,0.71,-0.0002,-0.00024,0.71,-2.5,-1.2,0.97,-5.8,-3,-3.7e+02,-0.00089,-0.0058,0.00043,8.2e-05,-0.019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.8e-05,7.7e-05,2.5e-05,0.024,0.024,0.0089,0.13,0.13,0.036,3.7e-07,3.7e-07,1.3e-06,0.028,0.028,0.0005,0.0025,0.00022,0.0025,0.0025,0.0025,0.0025,0,0 -28890000,0.71,-0.00022,-2.2e-05,0.71,-2.5,-1.2,0.96,-6.1,-3.2,-3.7e+02,-0.00089,-0.0058,0.00042,-0.00035,-0.018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,7.8e-05,7.7e-05,2.4e-05,0.025,0.025,0.009,0.14,0.14,0.036,3.7e-07,3.7e-07,1.2e-06,0.028,0.028,0.0005,0.0025,0.00021,0.0025,0.0025,0.0025,0.0025,0,0 -28990000,0.71,-2.5e-05,0.00044,0.71,-2.4,-1.2,0.95,-6.4,-3.3,-3.7e+02,-0.00098,-0.0058,0.00041,-0.0024,-0.026,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.8e-05,7.7e-05,2.4e-05,0.024,0.024,0.009,0.14,0.14,0.036,3.6e-07,3.7e-07,1.2e-06,0.028,0.028,0.0005,0.0025,0.0002,0.0025,0.0025,0.0025,0.0025,0,0 -29090000,0.71,0.00013,0.00084,0.71,-2.4,-1.2,0.94,-6.7,-3.4,-3.7e+02,-0.00098,-0.0058,0.0004,-0.0029,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.025,0.025,0.009,0.15,0.15,0.036,3.7e-07,3.7e-07,1.2e-06,0.028,0.028,0.0005,0.0025,0.00019,0.0025,0.0025,0.0025,0.0025,0,0 -29190000,0.71,0.00035,0.0013,0.71,-2.3,-1.1,0.93,-7,-3.5,-3.7e+02,-0.001,-0.0058,0.00041,-0.0048,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.024,0.024,0.009,0.15,0.15,0.036,3.6e-07,3.6e-07,1.2e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 -29290000,0.71,0.0007,0.0021,0.71,-2.3,-1.1,0.96,-7.2,-3.6,-3.7e+02,-0.001,-0.0058,0.0004,-0.0054,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.025,0.025,0.0091,0.16,0.16,0.036,3.6e-07,3.6e-07,1.2e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 -29390000,0.71,0.0013,0.0036,0.71,-2.3,-1.1,0.97,-7.5,-3.7,-3.7e+02,-0.0011,-0.0058,0.00037,-0.0069,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.024,0.024,0.009,0.16,0.16,0.036,3.5e-07,3.5e-07,1.2e-06,0.028,0.027,0.0005,0.0025,0.00017,0.0025,0.0025,0.0025,0.0025,0,0 -29490000,0.71,0.0018,0.0048,0.71,-2.2,-1.1,0.97,-7.7,-3.8,-3.7e+02,-0.0011,-0.0058,0.00037,-0.0073,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.026,0.026,0.0091,0.17,0.17,0.037,3.5e-07,3.5e-07,1.2e-06,0.028,0.027,0.0005,0.0025,0.00016,0.0025,0.0025,0.0025,0.0025,0,0 -29590000,0.71,0.0022,0.0059,0.71,-2.2,-1.1,0.96,-8,-3.9,-3.7e+02,-0.0011,-0.0057,0.00037,-0.0097,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.024,0.025,0.009,0.17,0.17,0.036,3.5e-07,3.5e-07,1.2e-06,0.028,0.027,0.0005,0.0025,0.00016,0.0025,0.0025,0.0025,0.0025,0,0 -29690000,0.71,0.0025,0.0066,0.71,-2.2,-1.1,0.95,-8.2,-4,-3.7e+02,-0.0011,-0.0057,0.00036,-0.01,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,7.9e-05,7.8e-05,2.4e-05,0.026,0.026,0.0091,0.18,0.18,0.036,3.5e-07,3.5e-07,1.2e-06,0.028,0.027,0.0005,0.0025,0.00015,0.0025,0.0025,0.0025,0.0025,0,0 -29790000,0.71,0.0028,0.0071,0.71,-2.1,-1.1,0.94,-8.5,-4.1,-3.7e+02,-0.0012,-0.0057,0.00035,-0.012,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.8e-05,2.4e-05,0.025,0.025,0.0091,0.18,0.17,0.037,3.4e-07,3.4e-07,1.1e-06,0.028,0.027,0.0005,0.0025,0.00015,0.0025,0.0025,0.0025,0.0025,0,0 -29890000,0.71,0.0028,0.0073,0.71,-2.1,-1.1,0.92,-8.7,-4.2,-3.7e+02,-0.0012,-0.0057,0.00034,-0.013,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.8e-05,2.4e-05,0.026,0.026,0.0091,0.19,0.19,0.037,3.4e-07,3.4e-07,1.1e-06,0.028,0.027,0.0005,0.0025,0.00014,0.0025,0.0025,0.0025,0.0025,0,0 -29990000,0.71,0.003,0.0075,0.71,-2.1,-1.1,0.9,-9,-4.3,-3.7e+02,-0.0012,-0.0057,0.00033,-0.015,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.8e-05,2.4e-05,0.025,0.025,0.009,0.18,0.18,0.036,3.4e-07,3.4e-07,1.1e-06,0.028,0.027,0.0005,0.0025,0.00014,0.0025,0.0025,0.0025,0.0025,0,0 -30090000,0.71,0.003,0.0075,0.71,-2.1,-1,0.89,-9.2,-4.4,-3.7e+02,-0.0012,-0.0057,0.00032,-0.015,-0.026,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.8e-05,2.4e-05,0.026,0.026,0.0091,0.2,0.2,0.036,3.4e-07,3.4e-07,1.1e-06,0.028,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 -30190000,0.71,0.0031,0.0073,0.71,-2,-1,0.88,-9.4,-4.5,-3.7e+02,-0.0013,-0.0057,0.00032,-0.017,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.8e-05,2.4e-05,0.025,0.025,0.009,0.19,0.19,0.037,3.3e-07,3.3e-07,1.1e-06,0.027,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 -30290000,0.71,0.003,0.0072,0.71,-2,-1,0.87,-9.6,-4.6,-3.7e+02,-0.0013,-0.0057,0.00031,-0.017,-0.026,-0.11,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.8e-05,2.4e-05,0.026,0.026,0.0091,0.21,0.21,0.037,3.3e-07,3.3e-07,1.1e-06,0.027,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 -30390000,0.71,0.003,0.007,0.71,-2,-1,0.85,-9.9,-4.7,-3.7e+02,-0.0013,-0.0057,0.0003,-0.018,-0.025,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8e-05,7.7e-05,2.3e-05,0.025,0.025,0.009,0.2,0.2,0.036,3.3e-07,3.3e-07,1.1e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 -30490000,0.71,0.0029,0.0068,0.71,-2,-1,0.83,-10,-4.8,-3.7e+02,-0.0013,-0.0057,0.0003,-0.018,-0.024,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8e-05,7.7e-05,2.3e-05,0.026,0.026,0.0091,0.22,0.22,0.037,3.3e-07,3.3e-07,1.1e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 -30590000,0.71,0.0028,0.0065,0.71,-1.9,-1,0.79,-10,-4.9,-3.7e+02,-0.0013,-0.0057,0.0003,-0.019,-0.023,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8e-05,7.7e-05,2.3e-05,0.025,0.025,0.009,0.21,0.21,0.037,3.2e-07,3.2e-07,1.1e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 -30690000,0.71,0.0027,0.0062,0.71,-1.9,-0.99,0.79,-11,-5,-3.7e+02,-0.0013,-0.0057,0.0003,-0.02,-0.022,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8e-05,7.7e-05,2.3e-05,0.026,0.026,0.009,0.23,0.23,0.037,3.2e-07,3.2e-07,1.1e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 -30790000,0.71,0.0026,0.0058,0.71,-1.9,-0.98,0.78,-11,-5.1,-3.7e+02,-0.0013,-0.0057,0.00029,-0.021,-0.022,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8e-05,7.7e-05,2.3e-05,0.025,0.025,0.009,0.22,0.22,0.037,3.2e-07,3.2e-07,1e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 -30890000,0.71,0.0024,0.0054,0.71,-1.9,-0.97,0.76,-11,-5.2,-3.7e+02,-0.0013,-0.0057,0.00029,-0.021,-0.021,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.7e-05,2.3e-05,0.026,0.026,0.009,0.24,0.24,0.037,3.2e-07,3.2e-07,1e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 -30990000,0.71,0.0024,0.005,0.71,-1.9,-0.96,0.76,-11,-5.3,-3.7e+02,-0.0013,-0.0057,0.00028,-0.022,-0.02,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.7e-05,2.3e-05,0.025,0.025,0.0089,0.23,0.23,0.037,3.1e-07,3.1e-07,1e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 -31090000,0.71,0.0022,0.0045,0.71,-1.8,-0.96,0.74,-11,-5.4,-3.7e+02,-0.0013,-0.0057,0.00027,-0.022,-0.018,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.7e-05,2.3e-05,0.026,0.026,0.009,0.25,0.25,0.037,3.1e-07,3.2e-07,1e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 -31190000,0.71,0.0022,0.0042,0.71,-1.8,-0.95,0.73,-12,-5.5,-3.7e+02,-0.0014,-0.0057,0.00027,-0.024,-0.016,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.7e-05,2.3e-05,0.025,0.025,0.0089,0.24,0.24,0.037,3.1e-07,3.1e-07,1e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 -31290000,0.71,0.0019,0.0037,0.71,-1.8,-0.94,0.73,-12,-5.6,-3.7e+02,-0.0014,-0.0057,0.00027,-0.024,-0.014,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.7e-05,2.3e-05,0.026,0.026,0.0089,0.25,0.25,0.037,3.1e-07,3.1e-07,9.9e-07,0.027,0.026,0.0005,0.0025,9.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 -31390000,0.71,0.0018,0.0032,0.71,-1.8,-0.93,0.73,-12,-5.7,-3.7e+02,-0.0014,-0.0057,0.00026,-0.025,-0.013,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.6e-05,2.3e-05,0.025,0.025,0.0088,0.25,0.25,0.036,3.1e-07,3.1e-07,9.9e-07,0.027,0.026,0.0005,0.0025,9.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -31490000,0.71,0.0016,0.0026,0.71,-1.7,-0.92,0.73,-12,-5.8,-3.7e+02,-0.0014,-0.0057,0.00025,-0.026,-0.011,-0.1,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.6e-05,2.3e-05,0.026,0.026,0.0088,0.26,0.26,0.037,3.1e-07,3.1e-07,9.8e-07,0.027,0.026,0.0005,0.0025,9.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 -31590000,0.71,0.0015,0.0023,0.71,-1.7,-0.91,0.72,-12,-5.8,-3.7e+02,-0.0014,-0.0057,0.00025,-0.026,-0.0098,-0.099,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.6e-05,2.3e-05,0.025,0.025,0.0087,0.26,0.26,0.037,3e-07,3e-07,9.7e-07,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 -31690000,0.71,0.0013,0.0016,0.71,-1.7,-0.9,0.72,-12,-5.9,-3.7e+02,-0.0014,-0.0057,0.00025,-0.027,-0.0086,-0.099,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,8.1e-05,7.6e-05,2.3e-05,0.026,0.026,0.0088,0.27,0.27,0.037,3e-07,3e-07,9.6e-07,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 -31790000,0.71,0.0011,0.001,0.71,-1.7,-0.89,0.72,-13,-6,-3.7e+02,-0.0014,-0.0057,0.00025,-0.028,-0.0067,-0.098,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.6e-05,2.3e-05,0.025,0.025,0.0087,0.27,0.27,0.037,3e-07,3e-07,9.5e-07,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 -31890000,0.71,0.00086,0.00032,0.71,-1.6,-0.88,0.72,-13,-6.1,-3.7e+02,-0.0014,-0.0057,0.00024,-0.028,-0.0054,-0.097,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.6e-05,2.2e-05,0.026,0.026,0.0087,0.28,0.28,0.037,3e-07,3e-07,9.4e-07,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 -31990000,0.71,0.00072,-0.00013,0.71,-1.6,-0.87,0.71,-13,-6.2,-3.7e+02,-0.0014,-0.0057,0.00024,-0.029,-0.0035,-0.097,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.025,0.025,0.0086,0.28,0.28,0.036,3e-07,3e-07,9.3e-07,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 -32090000,0.71,0.00044,-0.00085,0.71,-1.6,-0.86,0.72,-13,-6.3,-3.7e+02,-0.0014,-0.0057,0.00023,-0.03,-0.002,-0.096,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.026,0.026,0.0087,0.29,0.29,0.037,3e-07,3e-07,9.3e-07,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 -32190000,0.71,0.00023,-0.0016,0.71,-1.5,-0.85,0.72,-13,-6.4,-3.7e+02,-0.0014,-0.0057,0.00021,-0.031,7.9e-05,-0.096,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.025,0.025,0.0086,0.29,0.29,0.036,2.9e-07,2.9e-07,9.2e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32290000,0.71,-5.1e-06,-0.0024,0.71,-1.5,-0.84,0.71,-13,-6.4,-3.7e+02,-0.0014,-0.0057,0.00021,-0.032,0.0017,-0.095,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.026,0.026,0.0086,0.3,0.3,0.037,2.9e-07,2.9e-07,9.1e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32390000,0.71,-0.00019,-0.003,0.71,-1.5,-0.83,0.71,-14,-6.5,-3.7e+02,-0.0014,-0.0057,0.00021,-0.032,0.0026,-0.095,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.025,0.025,0.0085,0.3,0.3,0.037,2.9e-07,2.9e-07,9e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32490000,0.71,-0.00031,-0.0032,0.71,-1.4,-0.81,0.72,-14,-6.6,-3.7e+02,-0.0014,-0.0057,0.00021,-0.032,0.0037,-0.094,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.026,0.026,0.0086,0.31,0.31,0.037,2.9e-07,2.9e-07,9e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32590000,0.71,-0.00031,-0.0034,0.71,-1.4,-0.8,0.71,-14,-6.7,-3.7e+02,-0.0015,-0.0057,0.00021,-0.033,0.0045,-0.094,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.024,0.025,0.0084,0.31,0.31,0.036,2.9e-07,2.9e-07,8.9e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32690000,0.71,-0.00035,-0.0035,0.71,-1.4,-0.79,0.71,-14,-6.8,-3.7e+02,-0.0015,-0.0057,0.0002,-0.033,0.005,-0.094,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.5e-05,2.2e-05,0.026,0.026,0.0085,0.32,0.32,0.036,2.9e-07,2.9e-07,8.8e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32790000,0.71,-0.00031,-0.0035,0.71,-1.4,-0.78,0.71,-14,-6.8,-3.7e+02,-0.0015,-0.0057,0.0002,-0.033,0.006,-0.094,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.4e-05,2.2e-05,0.024,0.025,0.0084,0.31,0.31,0.036,2.8e-07,2.8e-07,8.7e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32890000,0.71,-0.00022,-0.0034,0.71,-1.3,-0.77,0.71,-14,-6.9,-3.7e+02,-0.0015,-0.0057,0.00019,-0.034,0.0071,-0.093,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.4e-05,2.2e-05,0.026,0.026,0.0084,0.33,0.33,0.036,2.8e-07,2.9e-07,8.7e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -32990000,0.71,-8.9e-05,-0.0034,0.71,-1.3,-0.76,0.7,-15,-7,-3.7e+02,-0.0015,-0.0057,0.0002,-0.034,0.0084,-0.093,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.4e-05,2.2e-05,0.024,0.024,0.0083,0.32,0.32,0.036,2.8e-07,2.8e-07,8.6e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -33090000,0.71,-0.00012,-0.0034,0.71,-1.3,-0.76,0.69,-15,-7.1,-3.7e+02,-0.0015,-0.0057,0.0002,-0.035,0.0089,-0.093,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.4e-05,2.2e-05,0.026,0.026,0.0084,0.34,0.34,0.036,2.8e-07,2.8e-07,8.5e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -33190000,0.7,0.0033,-0.0024,0.71,-1.3,-0.75,0.64,-15,-7.1,-3.7e+02,-0.0015,-0.0057,0.00021,-0.035,0.0097,-0.092,0.37,0.0037,0.025,0,0,0,0,0,5.3e-05,8.1e-05,7.4e-05,2.1e-05,0.024,0.024,0.0083,0.33,0.33,0.036,2.8e-07,2.8e-07,8.5e-07,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 -33290000,0.65,0.015,-0.0015,0.76,-1.3,-0.73,0.62,-15,-7.2,-3.7e+02,-0.0015,-0.0057,0.00021,-0.035,0.01,-0.092,0.37,0.0037,0.025,0,0,0,0,0,5.5e-05,8e-05,7.4e-05,1.8e-05,0.026,0.026,0.0083,0.35,0.35,0.036,2.8e-07,2.8e-07,8.4e-07,0.027,0.025,0.0005,0.0025,9.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33390000,0.55,0.013,-0.0018,0.84,-1.3,-0.72,0.81,-15,-7.3,-3.7e+02,-0.0015,-0.0057,0.00021,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.9e-05,7.5e-05,1.4e-05,0.024,0.024,0.0083,0.34,0.34,0.036,2.8e-07,2.8e-07,8.3e-07,0.027,0.025,0.0005,0.0025,9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33490000,0.41,0.0068,0.00068,0.91,-1.2,-0.71,0.83,-15,-7.4,-3.7e+02,-0.0015,-0.0057,0.00021,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,7.8e-05,7.7e-05,1.1e-05,0.026,0.026,0.0081,0.35,0.35,0.036,2.8e-07,2.8e-07,8.3e-07,0.027,0.025,0.0005,0.0025,8.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33590000,0.25,0.00094,-0.0018,0.97,-1.2,-0.71,0.79,-15,-7.4,-3.7e+02,-0.0015,-0.0057,0.00021,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,6.3e-05,7.6e-05,7.8e-05,1.1e-05,0.025,0.025,0.0078,0.35,0.35,0.036,2.7e-07,2.8e-07,8.2e-07,0.027,0.025,0.0005,0.0025,8.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33690000,0.087,-0.0023,-0.0049,1,-1.2,-0.71,0.8,-15,-7.5,-3.7e+02,-0.0015,-0.0057,0.00022,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,6e-05,7.5e-05,8e-05,1.3e-05,0.028,0.028,0.0076,0.36,0.36,0.036,2.7e-07,2.8e-07,8.1e-07,0.027,0.025,0.0005,0.0025,8.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33790000,-0.082,-0.0038,-0.0067,1,-1.1,-0.69,0.78,-16,-7.6,-3.7e+02,-0.0015,-0.0057,0.00022,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,5.4e-05,7.2e-05,8e-05,1.8e-05,0.028,0.028,0.0074,0.36,0.36,0.036,2.7e-07,2.7e-07,8.1e-07,0.027,0.025,0.0005,0.0025,8.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33890000,-0.25,-0.005,-0.0075,0.97,-1,-0.67,0.77,-16,-7.6,-3.7e+02,-0.0015,-0.0057,0.00022,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,4.7e-05,7.2e-05,8.1e-05,2.5e-05,0.031,0.032,0.0072,0.37,0.37,0.036,2.7e-07,2.7e-07,8e-07,0.027,0.025,0.0005,0.0025,8.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 -33990000,-0.39,-0.0032,-0.011,0.92,-0.98,-0.63,0.74,-16,-7.7,-3.7e+02,-0.0015,-0.0056,0.00022,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,3.9e-05,6.9e-05,7.8e-05,3.2e-05,0.031,0.032,0.007,0.37,0.37,0.035,2.7e-07,2.7e-07,8e-07,0.027,0.025,0.0005,0.0025,8e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34090000,-0.5,-0.0021,-0.013,0.87,-0.93,-0.58,0.74,-16,-7.7,-3.7e+02,-0.0015,-0.0056,0.00023,-0.036,0.011,-0.092,0.37,0.0037,0.025,0,0,0,0,0,3.3e-05,6.9e-05,7.8e-05,3.7e-05,0.036,0.037,0.0069,0.38,0.38,0.036,2.7e-07,2.7e-07,7.9e-07,0.027,0.025,0.0005,0.0025,7.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34190000,-0.57,-0.0014,-0.011,0.82,-0.91,-0.54,0.74,-16,-7.8,-3.7e+02,-0.0015,-0.0057,0.00023,-0.038,0.013,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.9e-05,6.5e-05,7.3e-05,4e-05,0.036,0.037,0.0067,0.38,0.38,0.035,2.7e-07,2.7e-07,7.9e-07,0.027,0.025,0.0005,0.0025,7.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34290000,-0.61,-0.0024,-0.0085,0.79,-0.87,-0.49,0.74,-16,-7.9,-3.7e+02,-0.0015,-0.0057,0.00023,-0.038,0.013,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.7e-05,6.5e-05,7.3e-05,4.1e-05,0.042,0.043,0.0066,0.39,0.39,0.035,2.7e-07,2.7e-07,7.8e-07,0.027,0.025,0.0005,0.0025,7.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34390000,-0.64,-0.0025,-0.006,0.77,-0.85,-0.46,0.73,-16,-7.9,-3.7e+02,-0.0016,-0.0057,0.00023,-0.043,0.017,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,6e-05,6.7e-05,4.1e-05,0.041,0.043,0.0065,0.39,0.39,0.035,2.6e-07,2.7e-07,7.8e-07,0.027,0.025,0.0005,0.0025,7.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34490000,-0.65,-0.0035,-0.0039,0.76,-0.8,-0.42,0.73,-16,-8,-3.7e+02,-0.0016,-0.0057,0.00024,-0.043,0.017,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,6.1e-05,6.7e-05,4.1e-05,0.048,0.05,0.0064,0.4,0.4,0.035,2.7e-07,2.7e-07,7.7e-07,0.027,0.025,0.0005,0.0025,7.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34590000,-0.66,-0.0028,-0.0028,0.75,-0.8,-0.4,0.73,-17,-8.1,-3.7e+02,-0.0016,-0.0057,0.00025,-0.053,0.026,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,5.5e-05,6.1e-05,4e-05,0.047,0.048,0.0063,0.4,0.4,0.034,2.6e-07,2.6e-07,7.7e-07,0.026,0.024,0.0005,0.0025,7.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34690000,-0.67,-0.0032,-0.002,0.74,-0.75,-0.36,0.73,-17,-8.1,-3.7e+02,-0.0016,-0.0057,0.00025,-0.053,0.026,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,5.5e-05,6.1e-05,4e-05,0.054,0.056,0.0063,0.41,0.41,0.034,2.6e-07,2.7e-07,7.7e-07,0.026,0.024,0.0005,0.0025,7.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34790000,-0.67,-0.0021,-0.0017,0.74,-0.75,-0.36,0.72,-17,-8.2,-3.7e+02,-0.0016,-0.0057,0.00024,-0.064,0.036,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,5.1e-05,5.5e-05,3.9e-05,0.051,0.053,0.0062,0.41,0.41,0.034,2.6e-07,2.6e-07,7.6e-07,0.026,0.024,0.0005,0.0025,7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34890000,-0.68,-0.0021,-0.0016,0.74,-0.7,-0.32,0.72,-17,-8.2,-3.7e+02,-0.0016,-0.0057,0.00025,-0.064,0.037,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,5.1e-05,5.5e-05,3.8e-05,0.058,0.061,0.0062,0.42,0.42,0.034,2.6e-07,2.7e-07,7.6e-07,0.026,0.024,0.0005,0.0025,6.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -34990000,-0.68,-0.0085,-0.0044,0.73,0.31,0.29,-0.13,-17,-8.2,-3.7e+02,-0.0016,-0.0057,0.00025,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,4.6e-05,5e-05,3.8e-05,0.056,0.058,0.0068,0.42,0.42,0.034,2.6e-07,2.6e-07,7.5e-07,0.025,0.023,0.0005,0.0025,6.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35090000,-0.68,-0.0085,-0.0045,0.73,0.43,0.31,-0.19,-17,-8.2,-3.7e+02,-0.0016,-0.0057,0.00025,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,4.6e-05,5e-05,3.7e-05,0.061,0.063,0.0068,0.43,0.43,0.034,2.6e-07,2.7e-07,7.5e-07,0.025,0.023,0.0005,0.0025,6.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35190000,-0.68,-0.0085,-0.0045,0.73,0.46,0.34,-0.18,-17,-8.2,-3.7e+02,-0.0016,-0.0057,0.00025,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,4.6e-05,5e-05,3.6e-05,0.066,0.068,0.0067,0.44,0.44,0.034,2.7e-07,2.7e-07,7.5e-07,0.025,0.023,0.0005,0.0025,6.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35290000,-0.68,-0.0086,-0.0045,0.73,0.48,0.37,-0.18,-17,-8.1,-3.7e+02,-0.0016,-0.0057,0.00025,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,4.6e-05,5e-05,3.5e-05,0.071,0.074,0.0066,0.45,0.45,0.033,2.7e-07,2.7e-07,7.5e-07,0.025,0.023,0.0005,0.0025,6.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35390000,-0.68,-0.0086,-0.0045,0.73,0.5,0.41,-0.18,-17,-8.1,-3.7e+02,-0.0016,-0.0057,0.00025,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,4.6e-05,5e-05,3.5e-05,0.077,0.08,0.0066,0.47,0.47,0.034,2.7e-07,2.7e-07,7.4e-07,0.025,0.023,0.0005,0.0025,6.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35490000,-0.68,-0.0087,-0.0045,0.73,0.53,0.44,-0.18,-17,-8,-3.7e+02,-0.0016,-0.0057,0.00024,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,4.6e-05,5e-05,3.4e-05,0.083,0.087,0.0065,0.49,0.49,0.034,2.7e-07,2.7e-07,7.4e-07,0.025,0.023,0.0005,0.0025,6.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35590000,-0.68,-0.0056,-0.0045,0.73,0.42,0.36,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,0.00026,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,3.9e-05,4.2e-05,3.3e-05,0.067,0.069,0.0062,0.48,0.48,0.033,2.7e-07,2.7e-07,7.4e-07,0.025,0.023,0.0005,0.0025,6.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35690000,-0.68,-0.0056,-0.0045,0.73,0.44,0.38,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,0.00027,-0.078,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,3.9e-05,4.2e-05,3.3e-05,0.072,0.075,0.0061,0.49,0.49,0.033,2.7e-07,2.7e-07,7.4e-07,0.025,0.023,0.0005,0.0025,6.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35790000,-0.68,-0.0034,-0.0044,0.73,0.36,0.32,-0.19,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00028,-0.079,0.052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,3.3e-05,3.6e-05,3.2e-05,0.06,0.062,0.0059,0.48,0.48,0.033,2.7e-07,2.7e-07,7.3e-07,0.025,0.023,0.0005,0.0025,6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35890000,-0.68,-0.0034,-0.0045,0.73,0.38,0.35,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,0.00029,-0.079,0.052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,3.3e-05,3.6e-05,3.1e-05,0.066,0.068,0.0058,0.5,0.5,0.033,2.7e-07,2.7e-07,7.3e-07,0.025,0.023,0.0005,0.0025,6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -35990000,-0.68,-0.0015,-0.0044,0.73,0.31,0.29,-0.19,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00031,-0.087,0.059,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.9e-05,3.1e-05,3.1e-05,0.057,0.058,0.0057,0.49,0.49,0.033,2.7e-07,2.7e-07,7.3e-07,0.025,0.023,0.0005,0.0025,5.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36090000,-0.68,-0.0016,-0.0044,0.73,0.32,0.31,-0.19,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00032,-0.087,0.059,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.9e-05,3.1e-05,3e-05,0.062,0.064,0.0057,0.51,0.51,0.032,2.7e-07,2.7e-07,7.3e-07,0.025,0.023,0.0005,0.0025,5.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36190000,-0.68,-0.00018,-0.0043,0.73,0.27,0.27,-0.19,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00032,-0.098,0.068,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.6e-05,2.8e-05,3e-05,0.055,0.056,0.0055,0.5,0.5,0.032,2.7e-07,2.7e-07,7.2e-07,0.024,0.022,0.0005,0.0025,5.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36290000,-0.68,-0.0003,-0.0042,0.73,0.28,0.29,-0.18,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00033,-0.098,0.068,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.6e-05,2.8e-05,2.9e-05,0.06,0.062,0.0056,0.51,0.51,0.032,2.7e-07,2.8e-07,7.2e-07,0.024,0.022,0.0005,0.0025,5.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36390000,-0.68,0.00075,-0.0041,0.73,0.23,0.24,-0.18,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00035,-0.11,0.077,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.4e-05,2.5e-05,2.9e-05,0.053,0.055,0.0055,0.51,0.51,0.032,2.8e-07,2.8e-07,7.2e-07,0.024,0.022,0.0005,0.0025,5.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36490000,-0.68,0.00065,-0.0042,0.73,0.24,0.26,-0.18,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00037,-0.11,0.078,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.4e-05,2.5e-05,2.9e-05,0.059,0.061,0.0055,0.52,0.52,0.032,2.8e-07,2.8e-07,7.2e-07,0.024,0.022,0.0005,0.0025,5.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36590000,-0.68,0.0014,-0.004,0.73,0.2,0.22,-0.18,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00038,-0.12,0.087,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.2e-05,2.3e-05,2.8e-05,0.053,0.054,0.0055,0.52,0.52,0.031,2.8e-07,2.8e-07,7.2e-07,0.023,0.021,0.0005,0.0025,5.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36690000,-0.68,0.0014,-0.004,0.73,0.2,0.24,-0.17,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00039,-0.12,0.088,-0.092,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2.2e-05,2.3e-05,2.8e-05,0.058,0.06,0.0055,0.53,0.53,0.031,2.8e-07,2.8e-07,7.1e-07,0.023,0.021,0.0005,0.0025,5.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36790000,-0.68,0.002,-0.0039,0.74,0.17,0.21,-0.17,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.0004,-0.14,0.097,-0.093,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2e-05,2.1e-05,2.7e-05,0.052,0.053,0.0055,0.53,0.53,0.031,2.8e-07,2.8e-07,7.1e-07,0.022,0.021,0.0005,0.0025,5.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36890000,-0.68,0.0019,-0.0039,0.74,0.17,0.22,-0.16,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00041,-0.14,0.097,-0.093,0.37,0.0037,0.025,0,0,0,0,0,2.4e-05,2e-05,2.1e-05,2.7e-05,0.057,0.059,0.0056,0.54,0.54,0.031,2.8e-07,2.8e-07,7.1e-07,0.022,0.021,0.0005,0.0025,5.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -36990000,-0.68,0.0024,-0.0037,0.74,0.14,0.19,-0.16,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00042,-0.15,0.11,-0.093,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.9e-05,2e-05,2.7e-05,0.051,0.052,0.0056,0.54,0.54,0.031,2.8e-07,2.8e-07,7.1e-07,0.021,0.02,0.0005,0.0025,5.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37090000,-0.68,0.0023,-0.0037,0.74,0.15,0.19,-0.15,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00043,-0.15,0.11,-0.093,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.9e-05,2e-05,2.6e-05,0.056,0.058,0.0057,0.55,0.55,0.031,2.8e-07,2.8e-07,7.1e-07,0.021,0.02,0.0005,0.0025,5.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37190000,-0.68,0.0026,-0.0036,0.74,0.12,0.17,-0.14,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00045,-0.16,0.11,-0.094,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.8e-05,1.9e-05,2.6e-05,0.05,0.051,0.0057,0.55,0.55,0.031,2.8e-07,2.8e-07,7e-07,0.021,0.019,0.0005,0.0025,5.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37290000,-0.68,0.0026,-0.0036,0.74,0.12,0.17,-0.14,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00046,-0.16,0.11,-0.094,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.8e-05,1.9e-05,2.6e-05,0.055,0.057,0.0058,0.56,0.56,0.031,2.8e-07,2.9e-07,7e-07,0.021,0.019,0.0005,0.0025,5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37390000,-0.68,0.0028,-0.0035,0.74,0.097,0.15,-0.13,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00047,-0.17,0.12,-0.094,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.8e-05,1.9e-05,2.5e-05,0.05,0.051,0.0059,0.56,0.56,0.031,2.8e-07,2.9e-07,7e-07,0.02,0.018,0.0005,0.0025,5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37490000,-0.68,0.0028,-0.0034,0.74,0.097,0.15,-0.12,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00048,-0.17,0.12,-0.094,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.8e-05,1.9e-05,2.5e-05,0.054,0.055,0.006,0.57,0.57,0.031,2.9e-07,2.9e-07,7e-07,0.02,0.018,0.0005,0.0025,4.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37590000,-0.68,0.0029,-0.0033,0.74,0.077,0.13,-0.12,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00049,-0.18,0.13,-0.095,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.8e-05,1.8e-05,2.5e-05,0.049,0.049,0.0061,0.57,0.57,0.031,2.9e-07,2.9e-07,7e-07,0.019,0.018,0.0005,0.0025,4.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37690000,-0.68,0.0029,-0.0034,0.74,0.075,0.14,-0.11,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.0005,-0.18,0.13,-0.095,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.8e-05,1.8e-05,2.4e-05,0.053,0.054,0.0062,0.58,0.58,0.031,2.9e-07,2.9e-07,7e-07,0.019,0.018,0.0005,0.0025,4.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37790000,-0.68,0.003,-0.0033,0.74,0.059,0.12,-0.1,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00051,-0.19,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,2.5e-05,1.7e-05,1.8e-05,2.4e-05,0.048,0.048,0.0063,0.58,0.58,0.03,2.9e-07,2.9e-07,7e-07,0.018,0.017,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37890000,-0.68,0.003,-0.0033,0.74,0.056,0.12,-0.093,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00052,-0.19,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.4e-05,0.052,0.053,0.0064,0.59,0.59,0.03,2.9e-07,2.9e-07,6.9e-07,0.018,0.017,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -37990000,-0.68,0.003,-0.0033,0.74,0.042,0.1,-0.084,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00053,-0.2,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.4e-05,0.046,0.047,0.0065,0.59,0.59,0.031,2.9e-07,2.9e-07,6.9e-07,0.017,0.016,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38090000,-0.68,0.003,-0.0033,0.74,0.038,0.11,-0.074,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00054,-0.2,0.13,-0.097,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.3e-05,0.05,0.051,0.0066,0.6,0.6,0.031,2.9e-07,2.9e-07,6.9e-07,0.017,0.016,0.0005,0.0025,4.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38190000,-0.68,0.003,-0.0032,0.74,0.025,0.089,-0.066,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00056,-0.2,0.14,-0.097,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.3e-05,0.045,0.046,0.0067,0.6,0.6,0.031,2.9e-07,2.9e-07,6.9e-07,0.016,0.015,0.0005,0.0025,4.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38290000,-0.68,0.003,-0.0032,0.74,0.022,0.09,-0.059,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00057,-0.2,0.14,-0.097,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.3e-05,0.049,0.05,0.0068,0.61,0.61,0.031,2.9e-07,2.9e-07,6.9e-07,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38390000,-0.68,0.003,-0.0032,0.74,0.014,0.078,-0.051,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00058,-0.21,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.3e-05,0.044,0.045,0.0069,0.61,0.61,0.031,2.9e-07,3e-07,6.9e-07,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38490000,-0.68,0.003,-0.0032,0.74,0.01,0.079,-0.044,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00059,-0.21,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.2e-05,0.048,0.048,0.007,0.62,0.62,0.031,3e-07,3e-07,6.8e-07,0.016,0.015,0.0005,0.0025,4.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38590000,-0.68,0.003,-0.0031,0.74,0.0055,0.068,-0.037,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.0006,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,2.6e-05,1.7e-05,1.8e-05,2.2e-05,0.043,0.044,0.0071,0.62,0.62,0.031,3e-07,3e-07,6.8e-07,0.015,0.014,0.0005,0.0025,4.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38690000,-0.68,0.0029,-0.0031,0.74,0.0011,0.068,-0.03,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00061,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,2.7e-05,1.7e-05,1.8e-05,2.2e-05,0.046,0.047,0.0072,0.63,0.63,0.031,3e-07,3e-07,6.8e-07,0.015,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38790000,-0.68,0.0029,-0.003,0.74,-0.0041,0.056,-0.022,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00062,-0.22,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,2.7e-05,1.7e-05,1.8e-05,2.2e-05,0.042,0.042,0.0073,0.63,0.63,0.031,3e-07,3e-07,6.8e-07,0.014,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 -38890000,-0.68,0.0027,-0.0031,0.74,-0.014,0.045,0.48,-17,-8.2,-3.7e+02,-0.0017,-0.0057,0.00063,-0.22,0.14,-0.1,0.37,0.0037,0.025,0,0,0,0,0,2.7e-05,1.8e-05,1.8e-05,2.2e-05,0.045,0.045,0.0075,0.64,0.64,0.032,3e-07,3e-07,6.8e-07,0.014,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,4.3e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.00011,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +190000,1,-0.0094,-0.011,2.9e-05,6.9e-05,0.0039,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.9e-13,0,0,-2e-09,0,0,0,0,0,0,0,0,5.6e-07,0.0027,0.0027,0.00023,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +290000,1,-0.0094,-0.011,6.4e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-2.8e-11,0,0,-4.8e-07,0,0,0,0,0,0,0,0,6.1e-07,0.0029,0.0029,0.00019,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +390000,1,-0.0095,-0.011,7.2e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,7.6e-11,0,0,-4.5e-06,0,0,0,0,0,0,0,0,6.8e-07,0.0031,0.0031,0.0003,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +490000,1,-0.0095,-0.012,2.2e-05,0.0039,0.0048,-0.06,0.00024,0.00048,-0.011,1.6e-06,-3.7e-07,1.4e-08,0,0,-1.6e-05,0,0,0,0,0,0,0,0,7.7e-07,0.0034,0.0033,0.00018,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +590000,1,-0.0095,-0.012,2.9e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,1.4e-08,0,0,-3.4e-05,0,0,0,0,0,0,0,0,8.7e-07,0.0037,0.0037,0.00025,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +690000,1,-0.0097,-0.012,8.9e-05,0.0063,0.0051,-0.06,0.0005,0.00054,-0.013,5.5e-06,-3.2e-06,1.2e-07,0,0,-6.3e-05,0,0,0,0,0,0,0,0,9.9e-07,0.004,0.004,0.00015,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +790000,1,-0.0098,-0.013,0.0001,0.0086,0.0072,-0.063,0.0012,0.0011,-0.014,5.4e-06,-3.1e-06,1.1e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.1e-06,0.0044,0.0044,0.0002,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.0012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +890000,1,-0.0099,-0.013,0.00012,0.01,0.0059,-0.077,0.00096,0.00071,-0.021,1.6e-05,-1.5e-05,3.4e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.3e-06,0.0048,0.0048,0.00013,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +990000,1,-0.0099,-0.013,0.00013,0.015,0.0062,-0.092,0.0022,0.0013,-0.029,1.6e-05,-1.5e-05,3.4e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.5e-06,0.0053,0.0053,0.00016,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1090000,1,-0.01,-0.014,0.00013,0.016,0.0048,-0.11,0.0018,0.00084,-0.039,4.1e-05,-6.2e-05,9.6e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.6e-06,0.0057,0.0057,0.00011,0.92,0.92,2,0.17,0.17,0.84,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1190000,1,-0.01,-0.014,0.0001,0.02,0.005,-0.12,0.0035,0.0013,-0.051,4.1e-05,-6.2e-05,9.6e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.8e-06,0.0063,0.0063,0.00013,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1290000,1,-0.01,-0.014,0.00016,0.02,0.0041,-0.14,0.0027,0.00085,-0.064,8.2e-05,-0.00019,2.3e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,1.9e-06,0.0064,0.0064,9.9e-05,0.88,0.88,2,0.15,0.15,1.4,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1390000,1,-0.01,-0.014,0.00017,0.026,0.0038,-0.15,0.0051,0.0013,-0.078,8.2e-05,-0.00019,2.3e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,2.2e-06,0.0071,0.0071,0.00011,1.1,1.1,2,0.21,0.21,1.7,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1490000,1,-0.01,-0.014,0.00015,0.024,0.0026,-0.16,0.0038,0.00078,-0.093,0.00014,-0.00046,4.8e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,2.2e-06,0.0067,0.0067,8.8e-05,0.95,0.95,2,0.14,0.14,2.1,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1590000,1,-0.011,-0.014,0.00014,0.031,0.003,-0.18,0.0065,0.0011,-0.11,0.00014,-0.00046,4.8e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,2.4e-06,0.0074,0.0074,0.0001,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1690000,1,-0.011,-0.014,0.0001,0.028,-0.00047,-0.19,0.0045,0.00057,-0.13,0.00019,-0.00088,8.2e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.2e-06,0.0064,0.0064,7.9e-05,1,1,2,0.14,0.14,3,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1790000,1,-0.011,-0.014,7.5e-05,0.035,-0.0024,-0.2,0.0076,0.00044,-0.15,0.00019,-0.00088,8.2e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.4e-06,0.007,0.007,8.9e-05,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1890000,1,-0.011,-0.015,5.4e-05,0.043,-0.0037,-0.22,0.011,0.00014,-0.17,0.00019,-0.00088,8.2e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.7e-06,0.0076,0.0076,0.0001,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1990000,1,-0.011,-0.014,4.7e-05,0.036,-0.0051,-0.23,0.0082,-0.00038,-0.19,0.0002,-0.0014,1.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,2.2e-06,0.0061,0.0061,8e-05,1.3,1.3,2,0.2,0.2,4.7,0.0066,0.0066,0.00011,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2090000,1,-0.011,-0.014,6.8e-06,0.041,-0.0077,-0.24,0.012,-0.001,-0.22,0.0002,-0.0014,1.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,2.4e-06,0.0066,0.0066,8.9e-05,1.7,1.7,2.1,0.31,0.31,5.3,0.0066,0.0066,0.00011,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2190000,1,-0.011,-0.014,-1.2e-06,0.033,-0.0072,-0.26,0.0079,-0.0011,-0.24,0.00015,-0.002,1.8e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.8e-06,0.005,0.005,7.2e-05,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,8.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2290000,1,-0.011,-0.014,-1.7e-05,0.039,-0.0098,-0.27,0.011,-0.0019,-0.27,0.00015,-0.002,1.8e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,2e-06,0.0054,0.0054,8e-05,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,8.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2390000,1,-0.011,-0.013,-1.9e-05,0.03,-0.009,-0.29,0.0074,-0.0016,-0.3,5.8e-05,-0.0025,2.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.5e-06,0.004,0.004,6.6e-05,1,1,2.1,0.19,0.19,7.4,0.0045,0.0045,7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2490000,1,-0.011,-0.013,-4e-05,0.034,-0.011,-0.3,0.011,-0.0025,-0.32,5.8e-05,-0.0025,2.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.6e-06,0.0044,0.0044,7.2e-05,1.3,1.3,2.1,0.28,0.28,8.2,0.0045,0.0045,7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2590000,1,-0.011,-0.013,-4.3e-05,0.026,-0.0093,-0.31,0.0068,-0.0018,-0.36,-5.2e-05,-0.003,2.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.2e-06,0.0032,0.0032,6.1e-05,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,5.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2690000,1,-0.011,-0.013,-5e-05,0.03,-0.011,-0.33,0.0096,-0.0029,-0.39,-5.2e-05,-0.003,2.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.3e-06,0.0035,0.0035,6.6e-05,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,5.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2790000,1,-0.011,-0.013,-7.1e-05,0.023,-0.0096,-0.34,0.0061,-0.002,-0.42,-0.00017,-0.0033,3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1e-06,0.0027,0.0027,5.6e-05,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,4.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2890000,1,-0.011,-0.013,-0.00012,0.027,-0.011,-0.35,0.0086,-0.003,-0.46,-0.00017,-0.0033,3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.1e-06,0.0029,0.0029,6.1e-05,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,4.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2990000,1,-0.011,-0.013,-9.1e-05,0.022,-0.0097,-0.36,0.0056,-0.0021,-0.49,-0.00028,-0.0036,3.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,8.4e-07,0.0023,0.0023,5.3e-05,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,3.9e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3090000,1,-0.011,-0.013,-9.2e-05,0.024,-0.011,-0.38,0.0079,-0.0032,-0.53,-0.00028,-0.0036,3.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,9.3e-07,0.0025,0.0025,5.7e-05,0.82,0.82,2.2,0.22,0.22,14,0.0027,0.0027,3.9e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3190000,1,-0.011,-0.013,-0.00016,0.019,-0.0088,-0.39,0.0052,-0.0022,-0.57,-0.00039,-0.0039,3.4e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,7.2e-07,0.002,0.002,4.9e-05,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,3.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3290000,1,-0.011,-0.013,-0.00013,0.023,-0.011,-0.4,0.0073,-0.0031,-0.61,-0.00039,-0.0039,3.4e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,7.9e-07,0.0022,0.0022,5.3e-05,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,3.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3390000,1,-0.011,-0.012,-0.00017,0.018,-0.0094,-0.42,0.0049,-0.0022,-0.65,-0.00049,-0.0041,3.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,6.2e-07,0.0017,0.0017,4.6e-05,0.53,0.53,2.3,0.14,0.14,17,0.0019,0.0019,2.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3490000,1,-0.011,-0.013,-0.00018,0.021,-0.012,-0.43,0.0068,-0.0032,-0.69,-0.00049,-0.0041,3.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,6.8e-07,0.0019,0.0019,5e-05,0.65,0.65,2.3,0.19,0.19,19,0.0019,0.0019,2.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3590000,1,-0.011,-0.012,-0.00017,0.016,-0.011,-0.44,0.0046,-0.0023,-0.73,-0.0006,-0.0044,3.8e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.3e-07,0.0015,0.0015,4.4e-05,0.48,0.48,2.4,0.13,0.13,20,0.0016,0.0016,2.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3690000,1,-0.011,-0.012,-4.8e-05,0.019,-0.014,-0.46,0.0064,-0.0036,-0.78,-0.0006,-0.0044,3.8e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.9e-07,0.0017,0.0017,4.7e-05,0.59,0.59,2.4,0.18,0.18,21,0.0016,0.0016,2.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3790000,1,-0.011,-0.012,-1.3e-05,0.015,-0.014,-0.47,0.0043,-0.0026,-0.83,-0.00072,-0.0045,4e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,4.7e-07,0.0014,0.0014,4.1e-05,0.44,0.45,2.4,0.12,0.12,23,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3890000,1,-0.011,-0.012,-5.1e-05,0.017,-0.015,-0.48,0.006,-0.004,-0.87,-0.00072,-0.0045,4e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.1e-07,0.0015,0.0015,4.4e-05,0.54,0.54,2.4,0.17,0.17,24,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3990000,1,-0.011,-0.012,-5.2e-05,0.02,-0.016,-0.5,0.0078,-0.0056,-0.92,-0.00072,-0.0045,4e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.5e-07,0.0016,0.0016,4.7e-05,0.66,0.66,2.5,0.22,0.23,26,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4090000,1,-0.011,-0.012,-6.7e-05,0.017,-0.014,-0.51,0.0056,-0.0042,-0.97,-0.00084,-0.0047,4.2e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,4.3e-07,0.0013,0.0013,4.2e-05,0.5,0.5,2.5,0.16,0.16,27,0.0012,0.0011,1.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4190000,1,-0.011,-0.012,-9.6e-05,0.02,-0.017,-0.53,0.0075,-0.0057,-1,-0.00084,-0.0047,4.2e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,4.7e-07,0.0015,0.0015,4.4e-05,0.6,0.61,2.5,0.21,0.21,29,0.0012,0.0011,1.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4290000,1,-0.01,-0.012,-0.00015,0.017,-0.012,-0.54,0.0054,-0.0042,-1.1,-0.00097,-0.0049,4.4e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,3.7e-07,0.0012,0.0012,4e-05,0.46,0.46,2.6,0.15,0.15,31,0.00095,0.00095,1.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4390000,1,-0.01,-0.012,-0.00014,0.018,-0.013,-0.55,0.0072,-0.0055,-1.1,-0.00097,-0.0049,4.4e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,4e-07,0.0013,0.0013,4.2e-05,0.56,0.56,2.6,0.2,0.2,33,0.00095,0.00095,1.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4490000,1,-0.01,-0.012,-8.5e-05,0.015,-0.01,-0.57,0.0051,-0.0038,-1.2,-0.0011,-0.005,4.6e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,3.2e-07,0.001,0.001,3.8e-05,0.43,0.43,2.6,0.14,0.14,34,0.00078,0.00078,1.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4590000,1,-0.01,-0.012,-6.1e-05,0.018,-0.012,-0.58,0.0068,-0.0049,-1.2,-0.0011,-0.005,4.6e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,3.4e-07,0.0011,0.0011,4e-05,0.51,0.51,2.7,0.19,0.19,36,0.00078,0.00078,1.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4690000,1,-0.01,-0.012,-6.8e-05,0.014,-0.01,-0.6,0.0049,-0.0035,-1.3,-0.0012,-0.0052,4.7e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,2.7e-07,0.00091,0.00091,3.6e-05,0.39,0.39,2.7,0.14,0.14,38,0.00064,0.00064,1.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4790000,1,-0.01,-0.012,-8.2e-05,0.016,-0.012,-0.61,0.0064,-0.0045,-1.4,-0.0012,-0.0052,4.7e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,2.9e-07,0.00099,0.00099,3.8e-05,0.47,0.47,2.7,0.18,0.18,40,0.00064,0.00064,1.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4890000,1,-0.01,-0.012,-0.0001,0.013,-0.01,-0.63,0.0046,-0.0033,-1.4,-0.0013,-0.0053,4.8e-05,0,0,-7.7e-05,0,0,0,0,0,0,0,0,2.3e-07,0.0008,0.00079,3.5e-05,0.36,0.36,2.8,0.13,0.13,42,0.00052,0.00052,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4990000,1,-0.01,-0.012,-0.00013,0.015,-0.011,-0.64,0.006,-0.0043,-1.5,-0.0013,-0.0053,4.8e-05,0,0,-7.7e-05,0,0,0,0,0,0,0,0,2.4e-07,0.00086,0.00086,3.7e-05,0.43,0.43,2.8,0.17,0.17,44,0.00052,0.00052,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5090000,1,-0.01,-0.011,-9.4e-05,0.012,-0.0086,-0.66,0.0043,-0.0031,-1.6,-0.0013,-0.0054,4.9e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,1.9e-07,0.00069,0.00069,3.3e-05,0.33,0.33,2.8,0.12,0.12,46,0.00042,0.00042,1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5190000,1,-0.01,-0.011,-7.3e-05,0.013,-0.01,-0.67,0.0056,-0.004,-1.6,-0.0013,-0.0054,4.9e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,2e-07,0.00075,0.00074,3.5e-05,0.39,0.39,2.9,0.16,0.16,49,0.00042,0.00042,1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5290000,1,-0.01,-0.011,-9.2e-05,0.0093,-0.0074,-0.68,0.0039,-0.0029,-1.7,-0.0014,-0.0055,5e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,1.6e-07,0.0006,0.0006,3.2e-05,0.3,0.3,2.9,0.12,0.12,51,0.00034,0.00034,9.1e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5390000,1,-0.0099,-0.011,-3.9e-05,0.0089,-0.0083,-0.7,0.0048,-0.0037,-1.8,-0.0014,-0.0055,5e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,1.7e-07,0.00064,0.00064,3.4e-05,0.36,0.36,2.9,0.16,0.16,53,0.00034,0.00034,9.1e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5490000,1,-0.0098,-0.011,-3.6e-05,0.0062,-0.0064,-0.71,0.0033,-0.0026,-1.8,-0.0014,-0.0055,5.1e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,1.3e-07,0.00052,0.00051,3.1e-05,0.28,0.28,3,0.11,0.11,56,0.00028,0.00027,8.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5590000,1,-0.0098,-0.011,-6.1e-05,0.0069,-0.0068,-0.73,0.0039,-0.0032,-1.9,-0.0014,-0.0055,5.1e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,1.4e-07,0.00055,0.00055,3.2e-05,0.33,0.33,3,0.15,0.15,58,0.00028,0.00027,8.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5690000,1,-0.0096,-0.011,4.4e-06,0.0049,-0.004,-0.74,0.0027,-0.0022,-2,-0.0015,-0.0056,5.2e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,1.1e-07,0.00045,0.00044,3e-05,0.25,0.25,3.1,0.11,0.11,61,0.00022,0.00022,7.5e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5790000,1,-0.0096,-0.011,-3.9e-06,0.0053,-0.003,-0.75,0.0031,-0.0026,-2,-0.0015,-0.0056,5.2e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,1.2e-07,0.00048,0.00048,3.1e-05,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,7.5e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5890000,1,-0.0095,-0.011,-3.4e-05,0.0045,-0.0011,0.0028,0.0022,-0.0017,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,9e-08,0.00038,0.00038,2.9e-05,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5990000,1,-0.0095,-0.011,-1.6e-05,0.0049,0.00031,0.015,0.0026,-0.0017,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,9.6e-08,0.00041,0.00041,3e-05,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6090000,1,-0.0094,-0.011,-3.6e-05,0.006,0.0015,-0.011,0.0032,-0.0016,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,1e-07,0.00044,0.00044,3.1e-05,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6190000,0.71,0.0013,-0.015,0.71,-0.004,0.0045,-0.005,0.0017,-0.00054,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-8.8e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.22,0.22,4.9,0.13,0.13,0.32,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6290000,0.71,0.0013,-0.015,0.71,-0.0041,0.0058,-0.012,0.0013,-1.4e-06,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-9.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.22,0.22,3.2,0.16,0.16,0.3,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6390000,0.71,0.0013,-0.014,0.71,-0.0051,0.0064,-0.05,0.00081,0.00062,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-3.6e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.23,0.23,2.3,0.19,0.19,0.29,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6490000,0.71,0.0014,-0.014,0.71,-0.0053,0.0071,-0.052,0.00028,0.0013,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-8.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.24,0.24,1.5,0.23,0.23,0.26,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6590000,0.71,0.0014,-0.014,0.71,-0.0058,0.0073,-0.099,-0.00024,0.002,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,9.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00035,0.00034,0.0028,0.25,0.25,1.1,0.28,0.28,0.23,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6690000,0.71,0.0014,-0.014,0.71,-0.0054,0.0084,-0.076,-0.00081,0.0028,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.00022,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00035,0.00035,0.0028,0.26,0.26,0.78,0.33,0.33,0.21,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6790000,0.71,0.0014,-0.014,0.71,-0.0067,0.0083,-0.11,-0.0014,0.0037,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,8.4e-06,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00035,0.00035,0.0028,0.28,0.28,0.6,0.38,0.38,0.2,0.00015,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6890000,0.71,0.0014,-0.014,0.71,-0.0087,0.0089,-0.12,-0.0022,0.0045,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-4.3e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00036,0.00036,0.0028,0.3,0.3,0.46,0.44,0.44,0.18,0.00015,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6990000,0.71,0.0015,-0.014,0.71,-0.0088,0.0098,-0.12,-0.0031,0.0055,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.00034,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00036,0.00036,0.0028,0.32,0.32,0.36,0.51,0.51,0.16,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7090000,0.71,0.0015,-0.014,0.71,-0.0093,0.0095,-0.13,-0.004,0.0064,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.0007,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00037,0.00037,0.0028,0.35,0.35,0.29,0.58,0.58,0.16,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7190000,0.71,0.0015,-0.014,0.71,-0.011,0.0096,-0.15,-0.0049,0.0073,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.00049,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00038,0.00038,0.0028,0.37,0.37,0.24,0.66,0.66,0.15,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7290000,0.71,0.0015,-0.014,0.71,-0.012,0.0099,-0.15,-0.0061,0.0083,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.0012,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00039,0.00039,0.0028,0.41,0.41,0.2,0.76,0.76,0.14,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7390000,0.71,0.0015,-0.014,0.71,-0.013,0.011,-0.16,-0.0073,0.0093,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.0013,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.0004,0.00039,0.0028,0.44,0.44,0.18,0.85,0.85,0.13,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7490000,0.71,0.0016,-0.014,0.71,-0.014,0.012,-0.16,-0.0087,0.01,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.0021,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.0004,0.0004,0.0028,0.48,0.48,0.15,0.96,0.96,0.12,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7590000,0.71,0.0016,-0.014,0.71,-0.016,0.013,-0.17,-0.01,0.012,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-0.003,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00041,0.00041,0.0028,0.51,0.52,0.14,1.1,1.1,0.12,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7690000,0.7,0.0017,-0.014,0.71,-0.018,0.014,-0.16,-0.012,0.013,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.005,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00042,0.00042,0.0022,0.56,0.56,0.13,1.2,1.2,0.11,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7790000,0.7,0.0017,-0.014,0.71,-0.019,0.015,-0.16,-0.014,0.014,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.007,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00043,0.00043,0.0022,0.6,0.6,0.12,1.3,1.3,0.11,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7890000,0.7,0.0017,-0.014,0.71,-0.022,0.017,-0.16,-0.016,0.015,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.0096,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00045,0.00045,0.0022,0.66,0.66,0.11,1.5,1.5,0.1,0.00014,0.00014,6.3e-06,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +7990000,0.7,0.0018,-0.014,0.71,-0.024,0.018,-0.16,-0.018,0.017,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.011,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00046,0.00046,0.0022,0.7,0.7,0.1,1.7,1.7,0.099,0.00014,0.00014,6.3e-06,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +8090000,0.7,0.0018,-0.014,0.71,-0.025,0.019,-0.17,-0.02,0.019,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.011,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00047,0.00047,0.0022,0.76,0.76,0.1,1.9,1.9,0.097,0.00014,0.00014,6.3e-06,0.04,0.04,0.037,0,0,0,0,0,0,0,0 +8190000,0.7,0.0018,-0.014,0.71,-0.028,0.021,-0.18,-0.023,0.021,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.013,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00049,0.00048,0.0022,0.83,0.83,0.099,2.1,2.1,0.094,0.00014,0.00014,6.3e-06,0.04,0.04,0.037,0,0,0,0,0,0,0,0 +8290000,0.7,0.0018,-0.014,0.71,-0.029,0.021,-0.17,-0.025,0.022,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.017,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0005,0.0005,0.0022,0.88,0.88,0.097,2.3,2.3,0.091,0.00014,0.00014,6.3e-06,0.04,0.04,0.036,0,0,0,0,0,0,0,0 +8390000,0.7,0.0019,-0.014,0.71,-0.031,0.022,-0.17,-0.028,0.024,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.021,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00051,0.00051,0.0022,0.95,0.95,0.097,2.5,2.5,0.091,0.00014,0.00014,6.3e-06,0.04,0.04,0.035,0,0,0,0,0,0,0,0 +8490000,0.7,0.0018,-0.014,0.71,-0.033,0.024,-0.17,-0.031,0.026,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.025,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00052,0.00052,0.0022,1,1,0.096,2.8,2.8,0.089,0.00014,0.00014,6.3e-06,0.04,0.04,0.034,0,0,0,0,0,0,0,0 +8590000,0.7,0.0019,-0.014,0.71,-0.035,0.026,-0.17,-0.035,0.028,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.029,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00054,0.00054,0.0022,1.1,1.1,0.095,3.1,3.1,0.088,0.00014,0.00014,6.3e-06,0.04,0.04,0.034,0,0,0,0,0,0,0,0 +8690000,0.7,0.0019,-0.014,0.71,-0.038,0.027,-0.16,-0.038,0.029,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.035,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00055,0.00055,0.0022,1.1,1.1,0.096,3.3,3.3,0.088,0.00014,0.00014,6.3e-06,0.04,0.04,0.033,0,0,0,0,0,0,0,0 +8790000,0.7,0.0019,-0.014,0.71,-0.041,0.029,-0.15,-0.042,0.032,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.041,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00056,0.00056,0.0022,1.2,1.2,0.095,3.7,3.7,0.087,0.00014,0.00014,6.3e-06,0.04,0.04,0.032,0,0,0,0,0,0,0,0 +8890000,0.7,0.0019,-0.014,0.71,-0.042,0.029,-0.15,-0.045,0.033,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.045,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00057,0.00057,0.0022,1.3,1.3,0.095,3.9,3.9,0.086,0.00013,0.00013,6.3e-06,0.04,0.04,0.03,0,0,0,0,0,0,0,0 +8990000,0.7,0.002,-0.014,0.71,-0.044,0.03,-0.14,-0.049,0.036,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.051,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00059,0.00059,0.0022,1.4,1.4,0.096,4.3,4.3,0.087,0.00013,0.00013,6.3e-06,0.04,0.04,0.029,0,0,0,0,0,0,0,0 +9090000,0.7,0.002,-0.014,0.71,-0.046,0.03,-0.14,-0.052,0.037,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.053,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0006,0.00059,0.0022,1.4,1.4,0.095,4.6,4.6,0.086,0.00013,0.00013,6.3e-06,0.04,0.04,0.028,0,0,0,0,0,0,0,0 +9190000,0.7,0.002,-0.014,0.71,-0.047,0.032,-0.14,-0.057,0.04,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.057,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00062,0.00061,0.0022,1.5,1.5,0.094,5.1,5.1,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.027,0,0,0,0,0,0,0,0 +9290000,0.7,0.002,-0.014,0.71,-0.047,0.032,-0.14,-0.059,0.041,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.061,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00062,0.00062,0.0022,1.6,1.6,0.093,5.3,5.3,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.025,0,0,0,0,0,0,0,0 +9390000,0.7,0.002,-0.014,0.71,-0.049,0.034,-0.14,-0.064,0.044,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.065,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00064,0.00064,0.0023,1.7,1.7,0.093,5.8,5.9,0.086,0.00013,0.00013,6.3e-06,0.04,0.04,0.024,0,0,0,0,0,0,0,0 +9490000,0.7,0.002,-0.014,0.71,-0.049,0.034,-0.13,-0.065,0.044,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.068,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00064,0.00064,0.0023,1.7,1.7,0.091,6.1,6.1,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.023,0,0,0,0,0,0,0,0 +9590000,0.7,0.002,-0.014,0.71,-0.052,0.036,-0.13,-0.071,0.047,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.072,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00066,0.00066,0.0023,1.8,1.8,0.09,6.7,6.7,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.021,0,0,0,0,0,0,0,0 +9690000,0.7,0.002,-0.014,0.71,-0.051,0.037,-0.12,-0.071,0.047,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.077,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00065,0.00065,0.0023,1.9,1.9,0.089,6.8,6.8,0.086,0.00012,0.00012,6.3e-06,0.04,0.04,0.02,0,0,0,0,0,0,0,0 +9790000,0.7,0.002,-0.014,0.71,-0.052,0.039,-0.11,-0.077,0.051,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.082,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00067,0.00067,0.0023,2,2,0.087,7.5,7.5,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.019,0,0,0,0,0,0,0,0 +9890000,0.7,0.002,-0.014,0.71,-0.051,0.039,-0.11,-0.076,0.051,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.085,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00067,0.00067,0.0023,2,2,0.084,7.6,7.6,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.018,0,0,0,0,0,0,0,0 +9990000,0.7,0.0021,-0.014,0.71,-0.053,0.04,-0.1,-0.082,0.054,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.089,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00069,0.00069,0.0023,2.1,2.1,0.083,8.4,8.4,0.086,0.00012,0.00012,6.3e-06,0.04,0.04,0.017,0,0,0,0,0,0,0,0 +10090000,0.7,0.0021,-0.014,0.71,-0.051,0.039,-0.096,-0.08,0.053,-3.7e+02,-0.0014,-0.0056,4.8e-05,0,0,-0.091,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00068,0.00068,0.0023,2.1,2.1,0.08,8.4,8.4,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.016,0,0,0,0,0,0,0,0 +10190000,0.7,0.0021,-0.014,0.71,-0.052,0.042,-0.096,-0.085,0.057,-3.7e+02,-0.0014,-0.0056,4.8e-05,0,0,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0007,0.0007,0.0023,2.2,2.2,0.078,9.2,9.2,0.084,0.00012,0.00012,6.3e-06,0.04,0.04,0.014,0,0,0,0,0,0,0,0 +10290000,0.7,0.0021,-0.014,0.71,-0.054,0.043,-0.084,-0.091,0.061,-3.7e+02,-0.0014,-0.0056,4.8e-05,0,0,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00072,0.00072,0.0023,2.4,2.4,0.076,10,10,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.014,0,0,0,0,0,0,0,0 +10390000,0.7,0.002,-0.014,0.71,0.0096,-0.019,0.0096,0.00087,-0.0017,-3.7e+02,-0.0014,-0.0056,4.8e-05,-6.6e-08,4.9e-08,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00075,0.00075,0.0023,0.25,0.25,0.56,0.25,0.25,0.078,0.00012,0.00012,6.3e-06,0.04,0.04,0.013,0,0,0,0,0,0,0,0 +10490000,0.7,0.0021,-0.014,0.71,0.0085,-0.017,0.023,0.0018,-0.0035,-3.7e+02,-0.0014,-0.0056,4.8e-05,-1.7e-06,1.3e-06,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00077,0.00077,0.0023,0.26,0.26,0.55,0.26,0.26,0.08,0.00012,0.00012,6.3e-06,0.04,0.04,0.012,0,0,0,0,0,0,0,0 +10590000,0.7,0.0022,-0.014,0.71,0.0081,-0.0063,0.026,0.0018,-0.00081,-3.7e+02,-0.0014,-0.0056,4.8e-05,-0.00026,2.6e-05,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00078,0.00078,0.0023,0.14,0.14,0.27,0.13,0.13,0.073,0.00012,0.00012,6.3e-06,0.04,0.04,0.012,0,0,0,0,0,0,0,0 +10690000,0.7,0.0022,-0.014,0.71,0.0062,-0.0056,0.03,0.0026,-0.0014,-3.7e+02,-0.0014,-0.0056,4.8e-05,-0.00026,2.7e-05,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0008,0.0008,0.0023,0.15,0.15,0.26,0.14,0.14,0.078,0.00012,0.00012,6.3e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +10790000,0.7,0.0022,-0.014,0.71,0.0056,-0.0028,0.024,0.0027,-0.00071,-3.7e+02,-0.0014,-0.0055,4.8e-05,-0.00041,0.00014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00077,0.00077,0.0023,0.11,0.11,0.17,0.091,0.091,0.072,0.00011,0.00011,6.3e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +10890000,0.7,0.0022,-0.014,0.71,0.0045,-0.0014,0.02,0.0032,-0.00089,-3.7e+02,-0.0014,-0.0055,4.8e-05,-0.00041,0.00014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0008,0.0008,0.0023,0.13,0.13,0.16,0.098,0.098,0.075,0.00011,0.00011,6.3e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +10990000,0.7,0.0022,-0.014,0.71,0.0072,0.0036,0.014,0.0047,-0.0022,-3.7e+02,-0.0013,-0.0055,4.6e-05,-0.00079,0.00072,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00073,0.00073,0.0023,0.1,0.1,0.12,0.073,0.073,0.071,0.0001,0.0001,6.3e-06,0.039,0.039,0.011,0,0,0,0,0,0,0,0 +11090000,0.7,0.0022,-0.014,0.71,0.0062,0.0062,0.019,0.0054,-0.0018,-3.7e+02,-0.0013,-0.0055,4.6e-05,-0.0008,0.00072,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00075,0.00075,0.0023,0.13,0.13,0.11,0.08,0.08,0.074,0.0001,0.0001,6.3e-06,0.039,0.039,0.011,0,0,0,0,0,0,0,0 +11190000,0.7,0.002,-0.014,0.71,0.011,0.0086,0.0077,0.0068,-0.0027,-3.7e+02,-0.0013,-0.0055,4.5e-05,-0.00089,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00065,0.00065,0.0023,0.1,0.1,0.084,0.063,0.063,0.069,9.3e-05,9.3e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11290000,0.7,0.0021,-0.014,0.71,0.011,0.011,0.0074,0.0079,-0.0017,-3.7e+02,-0.0013,-0.0055,4.5e-05,-0.00089,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00067,0.00067,0.0023,0.13,0.13,0.078,0.071,0.071,0.072,9.3e-05,9.3e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11390000,0.7,0.0022,-0.014,0.71,0.0058,0.0095,0.0017,0.0056,-0.0019,-3.7e+02,-0.0013,-0.0055,4.6e-05,-0.00045,0.00079,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00055,0.00055,0.0023,0.1,0.1,0.063,0.058,0.058,0.068,8.2e-05,8.2e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11490000,0.7,0.0022,-0.014,0.71,0.0034,0.012,0.0025,0.006,-0.00083,-3.7e+02,-0.0013,-0.0055,4.6e-05,-0.00046,0.0008,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00057,0.00057,0.0023,0.13,0.13,0.058,0.066,0.066,0.069,8.2e-05,8.2e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11590000,0.7,0.0021,-0.014,0.71,-0.0011,0.011,-0.0034,0.0045,-0.0014,-3.7e+02,-0.0014,-0.0056,4.6e-05,-2.1e-05,0.00066,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00047,0.00047,0.0023,0.1,0.1,0.049,0.054,0.054,0.066,7.2e-05,7.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11690000,0.7,0.002,-0.014,0.71,-0.0039,0.014,-0.0079,0.0043,-0.00021,-3.7e+02,-0.0014,-0.0056,4.6e-05,-1.1e-05,0.00065,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00048,0.00048,0.0023,0.12,0.12,0.046,0.063,0.063,0.066,7.2e-05,7.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11790000,0.7,0.0021,-0.014,0.71,-0.0098,0.013,-0.0098,0.0019,0.00058,-3.7e+02,-0.0014,-0.0056,4.7e-05,0.00011,0.00044,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0004,0.0004,0.0023,0.097,0.097,0.039,0.053,0.053,0.063,6.3e-05,6.3e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11890000,0.7,0.0021,-0.014,0.71,-0.011,0.014,-0.011,0.00083,0.002,-3.7e+02,-0.0014,-0.0056,4.7e-05,0.0001,0.00044,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00041,0.00041,0.0023,0.12,0.12,0.037,0.061,0.061,0.063,6.3e-05,6.3e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11990000,0.7,0.0021,-0.014,0.71,-0.013,0.014,-0.016,-0.00027,0.0023,-3.7e+02,-0.0014,-0.0056,4.6e-05,0.00025,0.00045,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00034,0.00034,0.0023,0.091,0.091,0.033,0.051,0.051,0.061,5.7e-05,5.7e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12090000,0.7,0.0021,-0.014,0.71,-0.014,0.016,-0.022,-0.0016,0.0038,-3.7e+02,-0.0014,-0.0056,4.6e-05,0.00026,0.00043,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00035,0.00035,0.0023,0.11,0.11,0.031,0.06,0.06,0.061,5.7e-05,5.7e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12190000,0.7,0.0018,-0.014,0.71,-0.0078,0.013,-0.017,0.0015,0.0019,-3.7e+02,-0.0013,-0.0057,4.3e-05,0.00052,0.00093,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0003,0.0003,0.0023,0.085,0.085,0.028,0.051,0.051,0.059,5.2e-05,5.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12290000,0.7,0.0017,-0.014,0.71,-0.01,0.015,-0.016,0.0006,0.0033,-3.7e+02,-0.0013,-0.0057,4.3e-05,0.00049,0.00095,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00031,0.00031,0.0023,0.1,0.1,0.028,0.06,0.06,0.059,5.2e-05,5.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12390000,0.7,0.0014,-0.014,0.71,-0.0049,0.011,-0.015,0.003,0.0017,-3.7e+02,-0.0013,-0.0058,4.1e-05,0.00066,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00027,0.00027,0.0023,0.079,0.079,0.026,0.05,0.05,0.057,4.8e-05,4.8e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12490000,0.7,0.0014,-0.014,0.71,-0.0061,0.013,-0.018,0.0025,0.0029,-3.7e+02,-0.0013,-0.0058,4.1e-05,0.00066,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00028,0.00028,0.0023,0.093,0.093,0.026,0.059,0.059,0.057,4.8e-05,4.8e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12590000,0.7,0.0015,-0.014,0.71,-0.014,0.011,-0.023,-0.0027,0.0015,-3.7e+02,-0.0013,-0.0058,4.3e-05,0.0008,0.001,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00025,0.00025,0.0023,0.074,0.074,0.025,0.05,0.05,0.055,4.5e-05,4.5e-05,6.3e-06,0.037,0.037,0.0099,0,0,0,0,0,0,0,0 +12690000,0.7,0.0015,-0.014,0.71,-0.014,0.013,-0.027,-0.0042,0.0027,-3.7e+02,-0.0013,-0.0058,4.3e-05,0.00082,0.00098,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00026,0.00026,0.0023,0.085,0.085,0.025,0.058,0.058,0.055,4.5e-05,4.5e-05,6.3e-06,0.037,0.037,0.0098,0,0,0,0,0,0,0,0 +12790000,0.7,0.0016,-0.013,0.71,-0.02,0.0093,-0.03,-0.0076,0.0013,-3.7e+02,-0.0014,-0.0058,4.4e-05,0.00086,0.00093,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00023,0.00023,0.0023,0.068,0.068,0.024,0.049,0.049,0.053,4.2e-05,4.2e-05,6.3e-06,0.037,0.037,0.0097,0,0,0,0,0,0,0,0 +12890000,0.7,0.0015,-0.014,0.71,-0.021,0.0093,-0.029,-0.0096,0.0022,-3.7e+02,-0.0014,-0.0058,4.4e-05,0.0008,0.00098,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00024,0.00024,0.0023,0.079,0.079,0.025,0.058,0.058,0.054,4.2e-05,4.2e-05,6.3e-06,0.037,0.037,0.0096,0,0,0,0,0,0,0,0 +12990000,0.71,0.0012,-0.014,0.71,-0.0083,0.0067,-0.03,-0.00099,0.0012,-3.7e+02,-0.0013,-0.0059,4e-05,0.00077,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00022,0.00022,0.0023,0.064,0.064,0.025,0.049,0.049,0.052,4e-05,4e-05,6.3e-06,0.037,0.037,0.0094,0,0,0,0,0,0,0,0 +13090000,0.7,0.0012,-0.014,0.71,-0.009,0.0069,-0.03,-0.0019,0.0019,-3.7e+02,-0.0013,-0.0059,4e-05,0.00074,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00023,0.00023,0.0023,0.073,0.073,0.025,0.058,0.058,0.052,4e-05,4e-05,6.3e-06,0.037,0.037,0.0094,0,0,0,0,0,0,0,0 +13190000,0.7,0.00094,-0.014,0.71,7.1e-05,0.0063,-0.027,0.0044,0.0012,-3.7e+02,-0.0012,-0.0059,3.8e-05,0.0006,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.06,0.06,0.025,0.049,0.049,0.051,3.8e-05,3.8e-05,6.3e-06,0.037,0.037,0.0091,0,0,0,0,0,0,0,0 +13290000,0.71,0.00095,-0.014,0.71,-0.00018,0.0071,-0.024,0.0044,0.0018,-3.7e+02,-0.0012,-0.0059,3.8e-05,0.00046,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00022,0.00022,0.0023,0.068,0.068,0.027,0.057,0.057,0.051,3.8e-05,3.8e-05,6.3e-06,0.037,0.037,0.0091,0,0,0,0,0,0,0,0 +13390000,0.71,0.0008,-0.014,0.71,0.00063,0.0061,-0.02,0.0033,0.0011,-3.7e+02,-0.0012,-0.0059,3.8e-05,0.00029,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.056,0.056,0.026,0.049,0.049,0.05,3.6e-05,3.6e-05,6.3e-06,0.037,0.037,0.0088,0,0,0,0,0,0,0,0 +13490000,0.71,0.00083,-0.014,0.71,0.00013,0.006,-0.019,0.0034,0.0017,-3.7e+02,-0.0012,-0.0059,3.8e-05,0.0002,0.0015,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.064,0.064,0.028,0.057,0.057,0.05,3.6e-05,3.6e-05,6.3e-06,0.037,0.037,0.0087,0,0,0,0,0,0,0,0 +13590000,0.71,0.00077,-0.014,0.71,0.00037,0.0062,-0.021,0.0024,0.001,-3.7e+02,-0.0012,-0.0059,3.7e-05,0.00018,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.053,0.053,0.028,0.049,0.049,0.05,3.4e-05,3.4e-05,6.3e-06,0.036,0.036,0.0084,0,0,0,0,0,0,0,0 +13690000,0.71,0.00075,-0.014,0.71,0.0011,0.008,-0.025,0.0025,0.0017,-3.7e+02,-0.0012,-0.0059,3.7e-05,0.0002,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.06,0.06,0.029,0.056,0.056,0.05,3.4e-05,3.4e-05,6.3e-06,0.036,0.036,0.0083,0,0,0,0,0,0,0,0 +13790000,0.71,0.00063,-0.014,0.71,0.0016,0.0038,-0.027,0.0035,-0.00062,-3.7e+02,-0.0011,-0.006,3.6e-05,-1.8e-05,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.051,0.051,0.029,0.048,0.048,0.049,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0079,0,0,0,0,0,0,0,0 +13890000,0.71,0.0006,-0.014,0.71,0.0021,0.0037,-0.031,0.0037,-0.00027,-3.7e+02,-0.0011,-0.006,3.6e-05,1.1e-05,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.057,0.057,0.03,0.056,0.056,0.05,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0078,0,0,0,0,0,0,0,0 +13990000,0.71,0.00053,-0.014,0.71,0.0024,0.0012,-0.03,0.0045,-0.002,-3.7e+02,-0.0011,-0.006,3.6e-05,-0.00026,0.001,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.048,0.048,0.03,0.048,0.048,0.05,3.1e-05,3.1e-05,6.3e-06,0.036,0.036,0.0074,0,0,0,0,0,0,0,0 +14090000,0.71,0.00051,-0.014,0.71,0.0024,0.0014,-0.031,0.0047,-0.0019,-3.7e+02,-0.0011,-0.006,3.6e-05,-0.00025,0.001,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.055,0.055,0.031,0.055,0.055,0.05,3.1e-05,3.1e-05,6.3e-06,0.036,0.036,0.0073,0,0,0,0,0,0,0,0 +14190000,0.71,0.00041,-0.014,0.71,0.0058,0.00081,-0.033,0.0068,-0.0016,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00024,0.00073,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.046,0.046,0.031,0.048,0.048,0.05,2.9e-05,2.9e-05,6.3e-06,0.036,0.036,0.0069,0,0,0,0,0,0,0,0 +14290000,0.71,0.00042,-0.014,0.71,0.0065,0.0016,-0.032,0.0074,-0.0015,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00031,0.00079,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.052,0.052,0.032,0.055,0.055,0.051,2.9e-05,2.9e-05,6.3e-06,0.036,0.036,0.0067,0,0,0,0,0,0,0,0 +14390000,0.71,0.00034,-0.014,0.71,0.0083,0.0025,-0.034,0.0088,-0.0013,-3.7e+02,-0.0011,-0.006,3.4e-05,-0.00032,0.00053,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.045,0.045,0.031,0.048,0.048,0.05,2.8e-05,2.8e-05,6.3e-06,0.036,0.036,0.0063,0,0,0,0,0,0,0,0 +14490000,0.71,0.00032,-0.014,0.71,0.0083,0.0037,-0.037,0.0096,-0.001,-3.7e+02,-0.0011,-0.006,3.4e-05,-0.00027,0.00049,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.05,0.05,0.032,0.055,0.055,0.051,2.8e-05,2.8e-05,6.3e-06,0.036,0.036,0.0062,0,0,0,0,0,0,0,0 +14590000,0.71,0.00031,-0.013,0.71,0.0048,0.0021,-0.037,0.006,-0.0024,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00064,0.00089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.043,0.043,0.031,0.047,0.047,0.051,2.6e-05,2.6e-05,6.3e-06,0.036,0.036,0.0058,0,0,0,0,0,0,0,0 +14690000,0.71,0.00027,-0.013,0.71,0.0062,-0.00083,-0.034,0.0066,-0.0024,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00075,0.00099,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.049,0.049,0.032,0.054,0.054,0.051,2.6e-05,2.6e-05,6.3e-06,0.036,0.036,0.0056,0,0,0,0,0,0,0,0 +14790000,0.71,0.0003,-0.013,0.71,0.003,-0.0024,-0.03,0.0037,-0.0034,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0012,0.0015,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.042,0.042,0.031,0.047,0.047,0.051,2.5e-05,2.5e-05,6.3e-06,0.036,0.036,0.0053,0,0,0,0,0,0,0,0 +14890000,0.71,0.00029,-0.013,0.71,0.0045,-0.0014,-0.033,0.0041,-0.0036,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0011,0.0015,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.00019,0.0023,0.047,0.047,0.031,0.054,0.054,0.052,2.5e-05,2.5e-05,6.3e-06,0.036,0.036,0.0051,0,0,0,0,0,0,0,0 +14990000,0.71,0.00029,-0.013,0.71,0.0033,-0.0016,-0.029,0.0032,-0.0029,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0012,0.0017,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.041,0.041,0.03,0.047,0.047,0.051,2.3e-05,2.3e-05,6.3e-06,0.036,0.036,0.0048,0,0,0,0,0,0,0,0 +15090000,0.71,0.00021,-0.013,0.71,0.0037,-0.0018,-0.032,0.0035,-0.003,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0011,0.0017,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.046,0.046,0.031,0.054,0.054,0.052,2.3e-05,2.3e-05,6.3e-06,0.036,0.036,0.0046,0,0,0,0,0,0,0,0 +15190000,0.71,0.00023,-0.013,0.71,0.0031,-0.0006,-0.029,0.0028,-0.0024,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0012,0.0018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.04,0.04,0.03,0.047,0.047,0.052,2.2e-05,2.2e-05,6.3e-06,0.035,0.035,0.0043,0,0,0,0,0,0,0,0 +15290000,0.71,0.00019,-0.013,0.71,0.0036,-0.00042,-0.027,0.0031,-0.0025,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0013,0.0019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.045,0.045,0.03,0.054,0.054,0.052,2.2e-05,2.2e-05,6.3e-06,0.035,0.035,0.0042,0,0,0,0,0,0,0,0 +15390000,0.71,0.0002,-0.013,0.71,0.0029,-9.3e-05,-0.024,0.00056,-0.002,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0013,0.0021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.04,0.04,0.029,0.047,0.047,0.051,2e-05,2e-05,6.3e-06,0.035,0.035,0.0039,0,0,0,0,0,0,0,0 +15490000,0.71,0.00021,-0.013,0.71,0.0041,-0.00046,-0.024,0.00092,-0.0021,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0013,0.0021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.045,0.045,0.029,0.053,0.053,0.053,2e-05,2e-05,6.3e-06,0.035,0.035,0.0037,0,0,0,0,0,0,0,0 +15590000,0.71,0.00023,-0.013,0.71,0.0023,-0.00047,-0.023,-0.0013,-0.0017,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0013,0.0023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.039,0.039,0.028,0.046,0.046,0.052,1.9e-05,1.9e-05,6.3e-06,0.035,0.035,0.0035,0,0,0,0,0,0,0,0 +15690000,0.71,0.00023,-0.013,0.71,0.0025,-0.00062,-0.023,-0.0011,-0.0017,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0013,0.0023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00018,0.0023,0.044,0.044,0.028,0.053,0.053,0.052,1.9e-05,1.9e-05,6.3e-06,0.035,0.035,0.0033,0,0,0,0,0,0,0,0 +15790000,0.71,0.00019,-0.013,0.71,0.003,-0.0023,-0.026,-0.00097,-0.0028,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0015,0.0023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.038,0.038,0.027,0.046,0.046,0.051,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.0031,0,0,0,0,0,0,0,0 +15890000,0.71,0.00014,-0.013,0.71,0.0039,-0.0028,-0.024,-0.0006,-0.0031,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0016,0.0024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.043,0.043,0.027,0.053,0.053,0.052,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.003,0,0,0,0,0,0,0,0 +15990000,0.71,8.6e-05,-0.013,0.71,0.0038,-0.0037,-0.019,-0.00066,-0.0038,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0019,0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.038,0.038,0.026,0.046,0.046,0.051,1.6e-05,1.6e-05,6.3e-06,0.034,0.034,0.0028,0,0,0,0,0,0,0,0 +16090000,0.71,8.9e-05,-0.013,0.71,0.0055,-0.0039,-0.016,-0.0002,-0.0042,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0019,0.0026,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.043,0.043,0.025,0.053,0.053,0.052,1.6e-05,1.6e-05,6.3e-06,0.034,0.034,0.0027,0,0,0,0,0,0,0,0 +16190000,0.71,0.00011,-0.013,0.71,0.0055,-0.0031,-0.014,-0.0004,-0.0034,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0018,0.0028,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.037,0.037,0.025,0.046,0.046,0.051,1.5e-05,1.5e-05,6.3e-06,0.034,0.034,0.0025,0,0,0,0,0,0,0,0 +16290000,0.71,0.00013,-0.013,0.71,0.0071,-0.0039,-0.016,0.00024,-0.0038,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0018,0.0028,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.042,0.042,0.024,0.053,0.053,0.052,1.5e-05,1.5e-05,6.3e-06,0.034,0.034,0.0024,0,0,0,0,0,0,0,0 +16390000,0.71,0.00012,-0.013,0.71,0.006,-0.0042,-0.015,-7.3e-05,-0.003,-3.7e+02,-0.0013,-0.0061,3.9e-05,-0.0017,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.037,0.037,0.023,0.046,0.046,0.051,1.4e-05,1.4e-05,6.3e-06,0.034,0.034,0.0022,0,0,0,0,0,0,0,0 +16490000,0.71,0.00014,-0.013,0.71,0.0052,-0.0037,-0.018,0.00047,-0.0034,-3.7e+02,-0.0013,-0.0061,3.9e-05,-0.0017,0.003,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.041,0.041,0.023,0.052,0.052,0.052,1.4e-05,1.4e-05,6.3e-06,0.034,0.034,0.0021,0,0,0,0,0,0,0,0 +16590000,0.71,0.0004,-0.013,0.71,0.0017,-0.001,-0.018,-0.0025,-3.5e-05,-3.7e+02,-0.0013,-0.006,4e-05,-0.001,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00016,0.00016,0.0023,0.036,0.036,0.022,0.046,0.046,0.051,1.2e-05,1.2e-05,6.3e-06,0.033,0.033,0.002,0,0,0,0,0,0,0,0 +16690000,0.71,0.00038,-0.013,0.71,0.0019,-0.00054,-0.015,-0.0023,-0.00011,-3.7e+02,-0.0013,-0.006,4e-05,-0.0011,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00016,0.0023,0.041,0.041,0.022,0.052,0.052,0.051,1.2e-05,1.2e-05,6.3e-06,0.033,0.033,0.0019,0,0,0,0,0,0,0,0 +16790000,0.71,0.00053,-0.013,0.71,-0.0015,0.0016,-0.014,-0.0046,0.0026,-3.7e+02,-0.0013,-0.006,4.2e-05,-0.00049,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00016,0.00016,0.0023,0.036,0.036,0.021,0.046,0.046,0.05,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0018,0,0,0,0,0,0,0,0 +16890000,0.71,0.00055,-0.013,0.71,-0.0018,0.0025,-0.011,-0.0048,0.0028,-3.7e+02,-0.0013,-0.006,4.2e-05,-0.00053,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00016,0.00016,0.0023,0.04,0.04,0.021,0.052,0.052,0.051,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0017,0,0,0,0,0,0,0,0 +16990000,0.71,0.00048,-0.013,0.71,-0.0017,0.00049,-0.01,-0.0052,0.0009,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.00095,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00015,0.00015,0.0023,0.035,0.035,0.02,0.046,0.046,0.05,1e-05,1e-05,6.3e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 +17090000,0.71,0.00045,-0.013,0.71,-0.00097,0.0015,-0.01,-0.0054,0.00097,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.00094,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00016,0.00016,0.0023,0.039,0.039,0.02,0.052,0.052,0.05,1e-05,1e-05,6.3e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 +17190000,0.71,0.00044,-0.013,0.71,-0.0005,0.0014,-0.011,-0.0057,-0.00049,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0013,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.035,0.035,0.019,0.046,0.046,0.049,9.4e-06,9.4e-06,6.3e-06,0.032,0.032,0.0015,0,0,0,0,0,0,0,0 +17290000,0.71,0.00042,-0.013,0.71,0.0016,0.0025,-0.0066,-0.0056,-0.00031,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0013,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.039,0.039,0.019,0.052,0.052,0.049,9.4e-06,9.4e-06,6.3e-06,0.032,0.032,0.0014,0,0,0,0,0,0,0,0 +17390000,0.71,0.00038,-0.013,0.71,0.0022,0.0016,-0.0047,-0.0047,-0.0016,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0017,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.034,0.034,0.018,0.046,0.046,0.048,8.5e-06,8.5e-06,6.3e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 +17490000,0.71,0.00037,-0.013,0.71,0.0028,0.0012,-0.003,-0.0045,-0.0015,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0017,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.038,0.038,0.018,0.052,0.052,0.049,8.5e-06,8.5e-06,6.3e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 +17590000,0.71,0.00029,-0.013,0.71,0.004,3.6e-05,0.0025,-0.0037,-0.0026,-3.7e+02,-0.0014,-0.0061,4.1e-05,-0.0021,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0023,0.034,0.034,0.017,0.045,0.045,0.048,7.7e-06,7.7e-06,6.3e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 +17690000,0.71,0.00026,-0.013,0.71,0.0049,0.00075,0.0019,-0.0033,-0.0025,-3.7e+02,-0.0014,-0.0061,4.1e-05,-0.002,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0023,0.037,0.037,0.017,0.052,0.052,0.048,7.7e-06,7.7e-06,6.3e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 +17790000,0.71,0.00017,-0.013,0.71,0.0075,0.00046,0.0006,-0.0021,-0.0022,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.002,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0023,0.033,0.033,0.016,0.045,0.045,0.048,7e-06,7e-06,6.3e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 +17890000,0.71,0.00018,-0.013,0.71,0.009,-0.00029,0.00069,-0.0013,-0.0021,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.002,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0024,0.037,0.037,0.016,0.052,0.052,0.048,7e-06,7e-06,6.3e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 +17990000,0.71,0.00012,-0.013,0.71,0.011,-0.002,0.0019,-0.00057,-0.0018,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.002,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.032,0.032,0.016,0.045,0.045,0.047,6.4e-06,6.4e-06,6.3e-06,0.031,0.031,0.001,0,0,0,0,0,0,0,0 +18090000,0.71,0.00012,-0.013,0.71,0.011,-0.0022,0.0043,0.00054,-0.0021,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.002,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0024,0.036,0.036,0.016,0.051,0.051,0.047,6.4e-06,6.4e-06,6.3e-06,0.031,0.031,0.00097,0,0,0,0,0,0,0,0 +18190000,0.71,9.3e-05,-0.013,0.71,0.012,-0.0011,0.0056,0.0014,-0.0016,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0019,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.032,0.032,0.015,0.045,0.045,0.047,5.8e-06,5.8e-06,6.3e-06,0.031,0.031,0.00092,0,0,0,0,0,0,0,0 +18290000,0.71,3.4e-05,-0.012,0.71,0.012,-0.0017,0.0068,0.0026,-0.0018,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0019,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.035,0.035,0.015,0.051,0.051,0.046,5.8e-06,5.8e-06,6.3e-06,0.031,0.031,0.00089,0,0,0,0,0,0,0,0 +18390000,0.71,5e-05,-0.013,0.71,0.013,-6.2e-05,0.008,0.0032,-0.0013,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0018,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.031,0.031,0.014,0.045,0.045,0.046,5.2e-06,5.2e-06,6.3e-06,0.031,0.031,0.00084,0,0,0,0,0,0,0,0 +18490000,0.71,6.6e-05,-0.012,0.71,0.014,0.00036,0.0076,0.0046,-0.0013,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0018,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.034,0.034,0.014,0.051,0.051,0.046,5.2e-06,5.2e-06,6.3e-06,0.031,0.031,0.00082,0,0,0,0,0,0,0,0 +18590000,0.71,7.2e-05,-0.012,0.71,0.013,0.00059,0.0058,0.0035,-0.0011,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.03,0.03,0.014,0.045,0.045,0.045,4.8e-06,4.8e-06,6.3e-06,0.031,0.031,0.00078,0,0,0,0,0,0,0,0 +18690000,0.71,4.1e-05,-0.012,0.71,0.014,-0.0001,0.0039,0.0048,-0.0011,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.033,0.033,0.013,0.051,0.051,0.045,4.8e-06,4.8e-06,6.3e-06,0.031,0.031,0.00075,0,0,0,0,0,0,0,0 +18790000,0.71,7e-05,-0.012,0.71,0.012,0.00019,0.0035,0.0037,-0.00086,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.03,0.03,0.013,0.045,0.045,0.045,4.3e-06,4.3e-06,6.3e-06,0.031,0.031,0.00072,0,0,0,0,0,0,0,0 +18890000,0.71,9.5e-05,-0.012,0.71,0.013,0.00069,0.0042,0.0049,-0.00078,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.033,0.033,0.013,0.051,0.051,0.045,4.3e-06,4.3e-06,6.3e-06,0.031,0.031,0.0007,0,0,0,0,0,0,0,0 +18990000,0.71,8.3e-05,-0.012,0.71,0.014,0.0016,0.0028,0.0063,-0.00065,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.029,0.029,0.012,0.045,0.045,0.044,3.9e-06,3.9e-06,6.3e-06,0.031,0.031,0.00066,0,0,0,0,0,0,0,0 +19090000,0.71,6.8e-05,-0.012,0.71,0.015,0.0022,0.0059,0.0077,-0.00043,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.032,0.032,0.012,0.051,0.051,0.044,3.9e-06,3.9e-06,6.3e-06,0.031,0.031,0.00065,0,0,0,0,0,0,0,0 +19190000,0.71,7e-05,-0.012,0.71,0.015,0.0022,0.0059,0.0085,-0.0004,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.028,0.028,0.012,0.045,0.045,0.044,3.6e-06,3.6e-06,6.3e-06,0.03,0.03,0.00062,0,0,0,0,0,0,0,0 +19290000,0.71,9.2e-05,-0.012,0.71,0.015,0.0014,0.0086,0.01,-0.0002,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.031,0.031,0.012,0.05,0.05,0.044,3.6e-06,3.6e-06,6.3e-06,0.03,0.03,0.0006,0,0,0,0,0,0,0,0 +19390000,0.71,0.0001,-0.012,0.71,0.012,0.00049,0.012,0.008,-0.00023,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.002,0.0057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.027,0.027,0.012,0.044,0.044,0.043,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.00058,0,0,0,0,0,0,0,0 +19490000,0.71,0.00013,-0.012,0.71,0.011,-0.00022,0.0088,0.0092,-0.00022,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.002,0.0057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.03,0.03,0.011,0.05,0.05,0.043,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.00056,0,0,0,0,0,0,0,0 +19590000,0.71,0.00018,-0.012,0.71,0.0095,-0.0013,0.0081,0.0074,-0.00025,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.002,0.006,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.027,0.027,0.011,0.044,0.044,0.042,3e-06,3e-06,6.3e-06,0.03,0.03,0.00054,0,0,0,0,0,0,0,0 +19690000,0.71,0.00018,-0.012,0.71,0.0099,-0.0034,0.0096,0.0084,-0.00049,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.002,0.006,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.029,0.029,0.011,0.05,0.05,0.042,3e-06,3e-06,6.3e-06,0.03,0.03,0.00052,0,0,0,0,0,0,0,0 +19790000,0.71,0.00025,-0.012,0.71,0.0076,-0.0043,0.01,0.0068,-0.00039,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.002,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.026,0.026,0.011,0.044,0.044,0.042,2.7e-06,2.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19890000,0.71,0.0002,-0.012,0.71,0.0063,-0.0046,0.011,0.0075,-0.00085,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.002,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.029,0.029,0.011,0.05,0.05,0.042,2.7e-06,2.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19990000,0.71,0.00019,-0.012,0.71,0.0039,-0.0053,0.014,0.0061,-0.00071,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0019,0.0064,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.025,0.026,0.01,0.044,0.044,0.041,2.5e-06,2.5e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20090000,0.71,0.00018,-0.012,0.71,0.0037,-0.0072,0.014,0.0065,-0.0013,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0019,0.0064,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.028,0.028,0.01,0.049,0.049,0.042,2.5e-06,2.5e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20190000,0.71,0.00029,-0.012,0.71,0.0014,-0.0079,0.017,0.0042,-0.001,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0018,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.0001,0.0024,0.025,0.025,0.01,0.044,0.044,0.041,2.3e-06,2.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20290000,0.71,0.00025,-0.012,0.71,0.00024,-0.0095,0.015,0.0043,-0.0019,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0018,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.027,0.027,0.0099,0.049,0.049,0.041,2.3e-06,2.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20390000,0.71,0.00027,-0.012,0.71,-0.0022,-0.01,0.017,0.0024,-0.0015,-3.7e+02,-0.0015,-0.006,4.4e-05,-0.0016,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.024,0.024,0.0097,0.044,0.044,0.041,2.1e-06,2.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20490000,0.71,0.00032,-0.012,0.71,-0.0027,-0.011,0.016,0.0021,-0.0025,-3.7e+02,-0.0015,-0.006,4.4e-05,-0.0016,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.026,0.026,0.0096,0.049,0.049,0.041,2.1e-06,2.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20590000,0.71,0.00035,-0.012,0.71,-0.0023,-0.011,0.013,0.0018,-0.002,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0014,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.024,0.024,0.0094,0.044,0.044,0.04,1.9e-06,1.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20690000,0.71,0.00037,-0.012,0.71,-0.0023,-0.012,0.015,0.0016,-0.0032,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0014,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.026,0.026,0.0093,0.049,0.049,0.04,1.9e-06,1.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20790000,0.71,0.0004,-0.012,0.71,-0.0034,-0.011,0.015,0.0013,-0.0025,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0013,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.023,0.023,0.0091,0.043,0.043,0.04,1.8e-06,1.8e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20890000,0.71,0.00039,-0.012,0.71,-0.0039,-0.014,0.014,0.00096,-0.0038,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0013,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-06,1.8e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20990000,0.71,0.00039,-0.012,0.71,-0.0041,-0.014,0.015,0.0026,-0.0031,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.001,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.9e-05,9.8e-05,0.0024,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-06,1.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +21090000,0.71,0.00039,-0.012,0.71,-0.0042,-0.017,0.015,0.0022,-0.0046,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.001,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,9.9e-05,0.0024,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-06,1.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +21190000,0.71,0.00043,-0.012,0.71,-0.0034,-0.016,0.014,0.0037,-0.0038,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00079,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.8e-05,9.7e-05,0.0024,0.022,0.022,0.0087,0.043,0.043,0.039,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21290000,0.71,0.00047,-0.012,0.71,-0.004,-0.018,0.016,0.0033,-0.0054,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00079,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.9e-05,9.8e-05,0.0024,0.024,0.024,0.0086,0.048,0.048,0.039,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21390000,0.71,0.00052,-0.012,0.71,-0.0048,-0.017,0.016,0.0028,-0.0034,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00041,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.7e-05,9.6e-05,0.0024,0.022,0.022,0.0085,0.043,0.043,0.039,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21490000,0.71,0.00053,-0.012,0.71,-0.0053,-0.018,0.015,0.0023,-0.0051,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00042,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.7e-05,9.7e-05,0.0024,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21590000,0.71,0.00055,-0.012,0.71,-0.0059,-0.015,0.015,0.0019,-0.0032,-3.7e+02,-0.0014,-0.006,4.5e-05,-6.5e-05,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0024,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21690000,0.71,0.00056,-0.012,0.71,-0.0058,-0.016,0.017,0.0013,-0.0048,-3.7e+02,-0.0014,-0.006,4.5e-05,-6.6e-05,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0024,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21790000,0.71,0.00058,-0.012,0.71,-0.0064,-0.011,0.015,4.7e-05,-0.00075,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00046,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0024,0.021,0.021,0.0082,0.042,0.042,0.038,1.2e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21890000,0.71,0.00058,-0.012,0.71,-0.0064,-0.012,0.016,-0.00059,-0.0019,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00045,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0024,0.022,0.022,0.0082,0.047,0.047,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21990000,0.71,0.00063,-0.012,0.71,-0.0068,-0.0091,0.016,-0.0015,0.0015,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00089,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.4e-05,9.3e-05,0.0024,0.02,0.02,0.0081,0.042,0.042,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22090000,0.71,0.00065,-0.012,0.71,-0.0072,-0.0082,0.015,-0.0022,0.00062,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00089,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.4e-05,9.3e-05,0.0024,0.022,0.022,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22190000,0.71,0.00062,-0.012,0.71,-0.007,-0.0073,0.015,-0.0018,0.00058,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00096,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.3e-05,9.2e-05,0.0024,0.02,0.02,0.008,0.042,0.042,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22290000,0.71,0.00066,-0.012,0.71,-0.0084,-0.0081,0.015,-0.0026,-0.0002,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00096,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.3e-05,9.3e-05,0.0024,0.021,0.021,0.008,0.047,0.047,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22390000,0.71,0.00063,-0.012,0.71,-0.0089,-0.0076,0.017,-0.0022,-0.00019,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.001,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.2e-05,9.1e-05,0.0024,0.019,0.019,0.0079,0.042,0.042,0.037,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22490000,0.71,0.00064,-0.012,0.71,-0.0096,-0.0075,0.018,-0.0031,-0.00096,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.001,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.3e-05,9.2e-05,0.0025,0.021,0.021,0.0079,0.047,0.047,0.037,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22590000,0.71,0.00062,-0.012,0.71,-0.0093,-0.007,0.017,-0.0034,0.00013,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0012,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.1e-05,9.1e-05,0.0025,0.019,0.019,0.0078,0.042,0.042,0.036,9.7e-07,9.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22690000,0.71,0.00066,-0.012,0.71,-0.011,-0.0068,0.018,-0.0044,-0.00056,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0012,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.2e-05,9.1e-05,0.0025,0.02,0.02,0.0079,0.046,0.046,0.037,9.7e-07,9.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22790000,0.71,0.00064,-0.012,0.71,-0.011,-0.0055,0.019,-0.0055,-0.00045,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.1e-05,9e-05,0.0025,0.019,0.019,0.0078,0.042,0.042,0.036,9.2e-07,9.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22890000,0.71,0.00065,-0.012,0.71,-0.012,-0.0052,0.021,-0.0067,-0.00099,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.1e-05,9e-05,0.0025,0.02,0.02,0.0078,0.046,0.046,0.036,9.2e-07,9.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22990000,0.71,0.00063,-0.012,0.71,-0.012,-0.0056,0.022,-0.0074,-0.00087,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9e-05,8.9e-05,0.0025,0.018,0.018,0.0078,0.041,0.041,0.036,8.7e-07,8.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23090000,0.71,0.0006,-0.012,0.71,-0.013,-0.0056,0.022,-0.0087,-0.0014,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9e-05,9e-05,0.0025,0.02,0.02,0.0078,0.046,0.046,0.036,8.7e-07,8.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23190000,0.71,0.00066,-0.012,0.71,-0.015,-0.0066,0.024,-0.012,-0.0013,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0014,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.9e-05,8.9e-05,0.0025,0.018,0.018,0.0077,0.041,0.041,0.036,8.2e-07,8.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23290000,0.71,0.0006,-0.012,0.71,-0.015,-0.0078,0.024,-0.013,-0.002,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0014,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9e-05,8.9e-05,0.0025,0.019,0.019,0.0078,0.046,0.046,0.036,8.2e-07,8.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23390000,0.71,0.00069,-0.012,0.71,-0.016,-0.0081,0.022,-0.016,-0.0018,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0014,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.9e-05,8.8e-05,0.0025,0.018,0.018,0.0077,0.041,0.041,0.036,7.8e-07,7.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23490000,0.71,0.0031,-0.0096,0.71,-0.023,-0.0089,-0.012,-0.018,-0.0026,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0014,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.9e-05,8.8e-05,0.0025,0.019,0.019,0.0078,0.045,0.045,0.036,7.8e-07,7.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23590000,0.71,0.0083,-0.0017,0.71,-0.034,-0.0077,-0.044,-0.017,-0.0013,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0016,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.8e-05,8.8e-05,0.0025,0.017,0.017,0.0077,0.041,0.041,0.035,7.4e-07,7.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23690000,0.7,0.0079,0.004,0.71,-0.065,-0.016,-0.094,-0.021,-0.0025,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0016,0.0066,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.8e-05,8.8e-05,0.0025,0.019,0.019,0.0078,0.045,0.045,0.036,7.4e-07,7.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23790000,0.7,0.005,0.00066,0.71,-0.089,-0.028,-0.15,-0.021,-0.0018,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0019,0.006,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.7e-05,8.7e-05,0.0025,0.017,0.017,0.0077,0.041,0.041,0.035,7.1e-07,7.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23890000,0.7,0.0024,-0.0054,0.71,-0.11,-0.037,-0.2,-0.031,-0.0051,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0019,0.006,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.8e-05,8.7e-05,0.0025,0.019,0.019,0.0078,0.045,0.045,0.035,7.1e-07,7.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23990000,0.7,0.00098,-0.01,0.71,-0.11,-0.04,-0.25,-0.034,-0.0083,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.002,0.0056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.7e-05,8.6e-05,0.0025,0.017,0.017,0.0077,0.041,0.041,0.035,6.8e-07,6.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24090000,0.7,0.0023,-0.0088,0.71,-0.11,-0.04,-0.3,-0.045,-0.012,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.002,0.0056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.7e-05,8.7e-05,0.0025,0.018,0.019,0.0078,0.045,0.045,0.035,6.8e-07,6.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24190000,0.7,0.0033,-0.0065,0.71,-0.11,-0.041,-0.35,-0.046,-0.014,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0023,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.6e-05,0.0025,0.017,0.017,0.0077,0.04,0.04,0.035,6.5e-07,6.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24290000,0.7,0.0038,-0.0057,0.71,-0.12,-0.045,-0.41,-0.058,-0.019,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0023,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.6e-05,0.0025,0.018,0.018,0.0078,0.045,0.045,0.036,6.5e-07,6.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24390000,0.7,0.0039,-0.0059,0.71,-0.13,-0.053,-0.46,-0.064,-0.03,-3.7e+02,-0.0014,-0.0059,4.4e-05,0.002,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.5e-05,0.0025,0.017,0.017,0.0078,0.04,0.04,0.035,6.2e-07,6.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24490000,0.7,0.0047,-0.0018,0.71,-0.14,-0.058,-0.51,-0.077,-0.036,-3.7e+02,-0.0014,-0.0059,4.4e-05,0.002,0.0047,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.6e-05,0.0025,0.018,0.018,0.0078,0.045,0.045,0.035,6.2e-07,6.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24590000,0.7,0.0051,0.0019,0.71,-0.16,-0.069,-0.56,-0.081,-0.045,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0019,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.5e-05,8.5e-05,0.0025,0.017,0.017,0.0078,0.04,0.04,0.035,5.9e-07,5.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24690000,0.7,0.0052,0.0028,0.71,-0.18,-0.083,-0.64,-0.098,-0.053,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0019,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.5e-05,8.5e-05,0.0025,0.018,0.018,0.0078,0.044,0.044,0.035,5.9e-07,5.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24790000,0.7,0.0049,0.0014,0.71,-0.2,-0.095,-0.72,-0.1,-0.064,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0023,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.4e-05,8.4e-05,0.0025,0.017,0.017,0.0078,0.04,0.04,0.035,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24890000,0.7,0.0066,0.0031,0.71,-0.22,-0.11,-0.75,-0.13,-0.074,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0023,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.4e-05,8.4e-05,0.0025,0.018,0.018,0.0078,0.044,0.044,0.035,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24990000,0.7,0.0084,0.0047,0.71,-0.24,-0.11,-0.81,-0.13,-0.082,-3.7e+02,-0.0013,-0.0059,4.5e-05,0.0032,0.0016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.4e-05,0.0025,0.016,0.017,0.0078,0.04,0.04,0.035,5.5e-07,5.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25090000,0.7,0.0088,0.0041,0.71,-0.27,-0.13,-0.85,-0.15,-0.094,-3.7e+02,-0.0013,-0.0059,4.5e-05,0.0032,0.0016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.4e-05,8.4e-05,0.0025,0.018,0.019,0.0079,0.044,0.044,0.035,5.5e-07,5.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25190000,0.7,0.0082,0.0027,0.71,-0.29,-0.14,-0.91,-0.17,-0.12,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0029,0.001,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.3e-05,0.0025,0.016,0.017,0.0078,0.04,0.04,0.035,5.3e-07,5.3e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25290000,0.7,0.01,0.0095,0.71,-0.32,-0.15,-0.96,-0.2,-0.13,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0029,0.001,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.3e-05,0.0025,0.017,0.019,0.0079,0.044,0.044,0.035,5.3e-07,5.3e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25390000,0.7,0.011,0.016,0.71,-0.35,-0.17,-1,-0.22,-0.15,-3.7e+02,-0.0012,-0.0059,4.1e-05,0.0031,-0.00052,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.2e-05,8.2e-05,0.0025,0.016,0.017,0.0078,0.04,0.04,0.035,5.1e-07,5.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25490000,0.7,0.012,0.017,0.71,-0.4,-0.19,-1.1,-0.25,-0.17,-3.7e+02,-0.0012,-0.0059,4.1e-05,0.0031,-0.0005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.3e-05,0.0025,0.017,0.019,0.0079,0.044,0.044,0.035,5.1e-07,5.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25590000,0.7,0.011,0.015,0.71,-0.44,-0.22,-1.1,-0.28,-0.21,-3.7e+02,-0.0012,-0.0059,4e-05,0.0028,-0.0015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.1e-05,8.1e-05,0.0024,0.016,0.018,0.0079,0.04,0.04,0.035,4.9e-07,4.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25690000,0.7,0.015,0.022,0.71,-0.49,-0.24,-1.2,-0.33,-0.23,-3.7e+02,-0.0012,-0.0059,4e-05,0.0028,-0.0014,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.2e-05,8.2e-05,0.0025,0.017,0.02,0.0079,0.044,0.044,0.035,4.9e-07,5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25790000,0.7,0.017,0.028,0.71,-0.53,-0.27,-1.2,-0.34,-0.26,-3.7e+02,-0.0012,-0.0059,4.2e-05,0.0037,-0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.2e-05,8.1e-05,0.0024,0.016,0.019,0.0079,0.04,0.04,0.035,4.8e-07,4.8e-07,6.3e-06,0.029,0.028,0.0005,0,0,0,0,0,0,0,0 +25890000,0.7,0.017,0.029,0.71,-0.6,-0.3,-1.3,-0.4,-0.29,-3.7e+02,-0.0012,-0.0059,4.2e-05,0.0037,-0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.2e-05,0.0024,0.017,0.021,0.008,0.044,0.044,0.035,4.8e-07,4.8e-07,6.3e-06,0.029,0.028,0.0005,0,0,0,0,0,0,0,0 +25990000,0.7,0.016,0.025,0.71,-0.66,-0.33,-1.3,-0.44,-0.34,-3.7e+02,-0.0011,-0.0059,4e-05,0.0033,-0.0057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.1e-05,8.1e-05,0.0024,0.016,0.02,0.0079,0.04,0.04,0.035,4.6e-07,4.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26090000,0.7,0.021,0.035,0.71,-0.72,-0.36,-1.3,-0.51,-0.38,-3.7e+02,-0.0011,-0.0059,4e-05,0.0033,-0.0057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.3e-05,8.1e-05,0.0024,0.017,0.022,0.008,0.044,0.044,0.035,4.6e-07,4.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26190000,0.7,0.023,0.045,0.71,-0.77,-0.4,-1.3,-0.53,-0.42,-3.7e+02,-0.0011,-0.0059,4.3e-05,0.0045,-0.0096,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.4e-05,8.1e-05,0.0023,0.016,0.021,0.0079,0.039,0.04,0.035,4.5e-07,4.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26290000,0.7,0.024,0.047,0.71,-0.87,-0.44,-1.3,-0.62,-0.46,-3.7e+02,-0.0011,-0.0059,4.3e-05,0.0045,-0.0096,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.5e-05,8.1e-05,0.0023,0.017,0.024,0.008,0.044,0.045,0.035,4.5e-07,4.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26390000,0.7,0.023,0.043,0.71,-0.94,-0.5,-1.3,-0.68,-0.55,-3.7e+02,-0.001,-0.0059,4.2e-05,0.0032,-0.011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,8.2e-05,8e-05,0.0022,0.016,0.023,0.0079,0.039,0.04,0.035,4.4e-07,4.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26490000,0.7,0.031,0.059,0.71,-1,-0.54,-1.3,-0.78,-0.6,-3.7e+02,-0.001,-0.0059,4.2e-05,0.0032,-0.011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,8.7e-05,8.1e-05,0.0022,0.018,0.026,0.008,0.043,0.045,0.035,4.4e-07,4.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26590000,0.7,0.037,0.075,0.71,-1.1,-0.59,-1.3,-0.82,-0.67,-3.7e+02,-0.00096,-0.0059,5.1e-05,0.0041,-0.015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0022,9.2e-05,8.2e-05,0.0021,0.016,0.026,0.008,0.039,0.041,0.035,4.3e-07,4.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26690000,0.7,0.038,0.078,0.71,-1.3,-0.65,-1.3,-0.94,-0.73,-3.7e+02,-0.00096,-0.0059,5.1e-05,0.004,-0.015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0022,9.3e-05,8.2e-05,0.0021,0.018,0.031,0.008,0.043,0.045,0.035,4.3e-07,4.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26790000,0.7,0.036,0.072,0.71,-1.4,-0.74,-1.3,-1,-0.86,-3.7e+02,-0.00092,-0.006,3.4e-05,0.002,-0.017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.002,8.8e-05,8.1e-05,0.002,0.017,0.03,0.008,0.039,0.041,0.035,4.2e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26890000,0.7,0.044,0.094,0.71,-1.5,-0.8,-1.3,-1.2,-0.93,-3.7e+02,-0.00092,-0.006,3.4e-05,0.002,-0.017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.002,9.8e-05,8.3e-05,0.002,0.019,0.036,0.0081,0.043,0.046,0.035,4.2e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26990000,0.7,0.051,0.12,0.7,-1.7,-0.88,-1.3,-1.2,-1,-3.7e+02,-0.0008,-0.006,3.3e-05,0.0028,-0.023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0018,0.00011,8.3e-05,0.0018,0.018,0.035,0.008,0.039,0.042,0.035,4.1e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0.0025,0.0023,0.0025,0.0025,0.0025,0.0025,0,0 +27090000,0.7,0.051,0.12,0.7,-1.9,-0.97,-1.3,-1.4,-1.1,-3.7e+02,-0.0008,-0.006,3.3e-05,0.0028,-0.022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0018,0.00011,8.3e-05,0.0018,0.02,0.043,0.0081,0.043,0.047,0.035,4.1e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0.0025,0.0015,0.0025,0.0025,0.0025,0.0025,0,0 +27190000,0.71,0.049,0.11,0.7,-2.1,-1,-1.2,-1.6,-1.2,-3.7e+02,-0.00078,-0.0059,0.00013,0.0026,-0.023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0016,9.7e-05,8.1e-05,0.0016,0.02,0.044,0.0081,0.046,0.05,0.035,4.1e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.0011,0.0025,0.0025,0.0025,0.0025,0,0 +27290000,0.71,0.043,0.094,0.7,-2.2,-1.1,-1.2,-1.8,-1.3,-3.7e+02,-0.00078,-0.0059,0.00013,0.0026,-0.023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0016,9e-05,8e-05,0.0016,0.022,0.051,0.0081,0.051,0.057,0.035,4.1e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.0009,0.0025,0.0025,0.0025,0.0025,0,0 +27390000,0.72,0.037,0.077,0.69,-2.4,-1.1,-1.2,-2,-1.4,-3.7e+02,-0.00071,-0.0059,0.00019,0.0034,-0.025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0014,8.2e-05,7.8e-05,0.0014,0.021,0.047,0.0081,0.053,0.059,0.035,4e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.00074,0.0025,0.0025,0.0025,0.0025,0,0 +27490000,0.72,0.031,0.062,0.69,-2.4,-1.1,-1.2,-2.3,-1.5,-3.7e+02,-0.00071,-0.0059,0.00019,0.0034,-0.025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0014,8e-05,7.8e-05,0.0014,0.023,0.05,0.0082,0.058,0.067,0.035,4e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.00063,0.0025,0.0025,0.0025,0.0025,0,0 +27590000,0.72,0.026,0.05,0.69,-2.5,-1.1,-1.2,-2.5,-1.6,-3.7e+02,-0.00072,-0.0059,0.00026,0.0022,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.7e-05,7.7e-05,0.0012,0.022,0.044,0.0082,0.061,0.068,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00056,0.0025,0.0025,0.0025,0.0025,0,0 +27690000,0.72,0.026,0.048,0.69,-2.6,-1.2,-1.2,-2.8,-1.7,-3.7e+02,-0.00072,-0.0059,0.00026,0.0022,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.7e-05,7.7e-05,0.0012,0.023,0.045,0.0082,0.067,0.077,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00049,0.0025,0.0025,0.0025,0.0025,0,0 +27790000,0.72,0.026,0.05,0.69,-2.6,-1.2,-1.2,-3.1,-1.8,-3.7e+02,-0.00071,-0.0059,0.0003,0.0015,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.7e-05,7.7e-05,0.0011,0.022,0.04,0.0082,0.069,0.078,0.035,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00044,0.0025,0.0025,0.0025,0.0025,0,0 +27890000,0.72,0.025,0.048,0.69,-2.6,-1.2,-1.2,-3.3,-1.9,-3.7e+02,-0.00071,-0.0059,0.0003,0.0013,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.7e-05,7.7e-05,0.0011,0.023,0.041,0.0083,0.076,0.087,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.0004,0.0025,0.0025,0.0025,0.0025,0,0 +27990000,0.72,0.024,0.044,0.69,-2.7,-1.2,-1.2,-3.6,-2,-3.7e+02,-0.00074,-0.0059,0.00034,0.0005,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.001,7.7e-05,7.6e-05,0.001,0.022,0.036,0.0083,0.078,0.087,0.035,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00037,0.0025,0.0025,0.0025,0.0025,0,0 +28090000,0.72,0.03,0.057,0.69,-2.7,-1.2,-1.2,-3.9,-2.1,-3.7e+02,-0.00074,-0.0059,0.00034,0.00037,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.001,7.7e-05,7.7e-05,0.001,0.023,0.037,0.0084,0.085,0.097,0.035,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00034,0.0025,0.0025,0.0025,0.0025,0,0 +28190000,0.72,0.036,0.071,0.68,-2.8,-1.2,-0.95,-4.2,-2.2,-3.7e+02,-0.00076,-0.0059,0.00036,-0.00023,-0.021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00094,7.8e-05,7.6e-05,0.00094,0.022,0.034,0.0084,0.087,0.097,0.035,3.9e-07,4.4e-07,5.9e-06,0.028,0.028,0.0005,0.0025,0.00032,0.0025,0.0025,0.0025,0.0025,0,0 +28290000,0.73,0.028,0.054,0.68,-2.8,-1.2,-0.089,-4.5,-2.4,-3.7e+02,-0.00076,-0.0059,0.00036,-0.00041,-0.021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,7.7e-05,7.6e-05,0.00094,0.022,0.034,0.0086,0.094,0.11,0.036,3.9e-07,4.4e-07,5.9e-06,0.028,0.028,0.0005,0.0025,0.0003,0.0025,0.0025,0.0025,0.0025,0,0 +28390000,0.73,0.011,0.023,0.68,-2.8,-1.2,0.77,-4.8,-2.5,-3.7e+02,-0.00076,-0.0059,0.00036,-0.00072,-0.02,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,7.9e-05,7.7e-05,0.00095,0.023,0.035,0.0087,0.1,0.12,0.036,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00028,0.0025,0.0025,0.0025,0.0025,0,0 +28490000,0.73,0.0014,0.0047,0.68,-2.7,-1.2,1.1,-5,-2.6,-3.7e+02,-0.00075,-0.0059,0.00036,-0.001,-0.019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,8.1e-05,7.7e-05,0.00095,0.024,0.035,0.0088,0.11,0.13,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00026,0.0025,0.0025,0.0025,0.0025,0,0 +28590000,0.73,-0.00043,0.0012,0.69,-2.7,-1.2,0.96,-5.3,-2.7,-3.7e+02,-0.00075,-0.0059,0.00036,-0.0014,-0.018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00094,8.2e-05,7.7e-05,0.00095,0.025,0.034,0.0089,0.12,0.14,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00025,0.0025,0.0025,0.0025,0.0025,0,0 +28690000,0.73,-0.0012,0.00034,0.69,-2.6,-1.2,0.96,-5.6,-2.8,-3.7e+02,-0.00075,-0.0059,0.00036,-0.0018,-0.017,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00094,8.3e-05,7.8e-05,0.00095,0.026,0.034,0.009,0.13,0.15,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00023,0.0025,0.0025,0.0025,0.0025,0,0 +28790000,0.73,-0.0016,0.00013,0.68,-2.6,-1.2,0.97,-5.9,-2.9,-3.7e+02,-0.00081,-0.006,0.00039,-0.0028,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00091,8.3e-05,7.8e-05,0.00091,0.025,0.03,0.0089,0.13,0.15,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00022,0.0025,0.0025,0.0025,0.0025,0,0 +28890000,0.73,-0.0016,0.00036,0.69,-2.5,-1.2,0.96,-6.1,-3.1,-3.7e+02,-0.00081,-0.006,0.00039,-0.0032,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00091,8.3e-05,7.8e-05,0.00091,0.026,0.03,0.009,0.14,0.16,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00021,0.0025,0.0025,0.0025,0.0025,0,0 +28990000,0.73,-0.0015,0.00081,0.68,-2.5,-1.1,0.95,-6.5,-3.2,-3.7e+02,-0.00089,-0.006,0.00041,-0.0028,-0.03,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.025,0.027,0.009,0.14,0.16,0.036,3.8e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.0002,0.0025,0.0025,0.0025,0.0025,0,0 +29090000,0.73,-0.0013,0.0012,0.68,-2.4,-1.1,0.94,-6.7,-3.3,-3.7e+02,-0.00089,-0.006,0.00041,-0.0033,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.3e-05,7.8e-05,0.00089,0.026,0.027,0.009,0.15,0.17,0.036,3.8e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00019,0.0025,0.0025,0.0025,0.0025,0,0 +29190000,0.73,-0.0011,0.0016,0.68,-2.4,-1.1,0.94,-7,-3.4,-3.7e+02,-0.00093,-0.006,0.00041,-0.0034,-0.032,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.024,0.025,0.009,0.15,0.17,0.036,3.8e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 +29290000,0.73,-0.00076,0.0025,0.68,-2.3,-1.1,0.96,-7.3,-3.5,-3.7e+02,-0.00093,-0.006,0.00041,-0.004,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.026,0.026,0.0091,0.16,0.18,0.036,3.8e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 +29390000,0.73,-0.00017,0.0039,0.68,-2.3,-1.1,0.97,-7.6,-3.6,-3.7e+02,-0.00099,-0.006,0.00041,-0.0036,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.024,0.025,0.009,0.16,0.18,0.036,3.7e-07,4.4e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00017,0.0025,0.0025,0.0025,0.0025,0,0 +29490000,0.73,0.00036,0.0051,0.68,-2.2,-1.1,0.97,-7.8,-3.7,-3.7e+02,-0.00099,-0.006,0.00041,-0.004,-0.032,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.026,0.026,0.0091,0.17,0.19,0.037,3.7e-07,4.4e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00016,0.0025,0.0025,0.0025,0.0025,0,0 +29590000,0.73,0.00079,0.0061,0.68,-2.2,-1.1,0.96,-8.1,-3.8,-3.7e+02,-0.001,-0.006,0.0004,-0.0052,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.025,0.025,0.009,0.17,0.19,0.036,3.7e-07,4.3e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00016,0.0025,0.0025,0.0025,0.0025,0,0 +29690000,0.73,0.0011,0.0068,0.68,-2.2,-1.1,0.95,-8.3,-3.9,-3.7e+02,-0.001,-0.006,0.0004,-0.0057,-0.032,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.3e-05,7.8e-05,0.0009,0.026,0.026,0.0091,0.18,0.2,0.036,3.7e-07,4.3e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00015,0.0025,0.0025,0.0025,0.0025,0,0 +29790000,0.73,0.0014,0.0073,0.68,-2.2,-1.1,0.94,-8.6,-4,-3.7e+02,-0.0011,-0.006,0.00038,-0.006,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.025,0.025,0.0091,0.18,0.2,0.037,3.6e-07,4.3e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00015,0.0025,0.0025,0.0025,0.0025,0,0 +29890000,0.73,0.0015,0.0075,0.68,-2.1,-1.1,0.92,-8.8,-4.1,-3.7e+02,-0.0011,-0.006,0.00038,-0.0068,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.026,0.027,0.0091,0.19,0.21,0.037,3.6e-07,4.3e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00014,0.0025,0.0025,0.0025,0.0025,0,0 +29990000,0.73,0.0017,0.0077,0.68,-2.1,-1.1,0.91,-9,-4.2,-3.7e+02,-0.0011,-0.006,0.00035,-0.0081,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.025,0.026,0.009,0.19,0.21,0.036,3.5e-07,4.2e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00014,0.0025,0.0025,0.0025,0.0025,0,0 +30090000,0.73,0.0017,0.0076,0.68,-2.1,-1.1,0.89,-9.2,-4.3,-3.7e+02,-0.0011,-0.006,0.00035,-0.0087,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.026,0.028,0.0091,0.2,0.22,0.036,3.6e-07,4.2e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 +30190000,0.73,0.0018,0.0074,0.68,-2,-1.1,0.88,-9.5,-4.4,-3.7e+02,-0.0011,-0.0059,0.00032,-0.0089,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.4e-05,7.8e-05,0.0009,0.025,0.027,0.009,0.2,0.22,0.037,3.5e-07,4.1e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 +30290000,0.73,0.0017,0.0073,0.68,-2,-1.1,0.87,-9.7,-4.5,-3.7e+02,-0.0011,-0.0059,0.00032,-0.0092,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.026,0.029,0.0091,0.21,0.23,0.037,3.5e-07,4.1e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 +30390000,0.73,0.0018,0.0071,0.68,-2,-1.1,0.85,-9.9,-4.6,-3.7e+02,-0.0012,-0.0059,0.00029,-0.0099,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.4e-05,7.8e-05,0.00089,0.025,0.028,0.009,0.21,0.23,0.036,3.4e-07,4e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 +30490000,0.73,0.0016,0.0068,0.68,-2,-1.1,0.84,-10,-4.7,-3.7e+02,-0.0012,-0.0059,0.00029,-0.01,-0.028,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.5e-05,7.8e-05,0.0009,0.026,0.03,0.0091,0.22,0.24,0.037,3.5e-07,4e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 +30590000,0.73,0.0016,0.0065,0.68,-1.9,-1,0.8,-10,-4.8,-3.7e+02,-0.0012,-0.0059,0.00026,-0.01,-0.028,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00088,8.5e-05,7.8e-05,0.00089,0.025,0.028,0.009,0.22,0.24,0.037,3.4e-07,3.9e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 +30690000,0.73,0.0014,0.0062,0.68,-1.9,-1,0.79,-11,-4.9,-3.7e+02,-0.0012,-0.0059,0.00026,-0.011,-0.027,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00088,8.5e-05,7.8e-05,0.00089,0.026,0.03,0.009,0.23,0.25,0.037,3.4e-07,3.9e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 +30790000,0.73,0.0015,0.0058,0.68,-1.9,-1,0.78,-11,-5,-3.7e+02,-0.0012,-0.0059,0.00022,-0.011,-0.027,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00087,8.5e-05,7.7e-05,0.00088,0.025,0.029,0.0089,0.23,0.25,0.037,3.3e-07,3.8e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 +30890000,0.73,0.0013,0.0053,0.68,-1.9,-1,0.77,-11,-5.1,-3.7e+02,-0.0012,-0.0059,0.00022,-0.011,-0.026,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00087,8.5e-05,7.7e-05,0.00088,0.026,0.031,0.009,0.24,0.26,0.037,3.3e-07,3.8e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 +30990000,0.73,0.0014,0.0048,0.68,-1.8,-1,0.76,-11,-5.2,-3.7e+02,-0.0013,-0.0059,0.00018,-0.011,-0.025,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.5e-05,7.7e-05,0.00086,0.025,0.03,0.0089,0.24,0.26,0.036,3.3e-07,3.7e-07,5.8e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 +31090000,0.73,0.0011,0.0043,0.68,-1.8,-1,0.75,-11,-5.3,-3.7e+02,-0.0013,-0.0059,0.00018,-0.012,-0.024,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.5e-05,7.7e-05,0.00087,0.027,0.032,0.0089,0.25,0.27,0.037,3.3e-07,3.7e-07,5.8e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 +31190000,0.73,0.0012,0.004,0.68,-1.8,-1,0.74,-12,-5.4,-3.7e+02,-0.0013,-0.0058,0.00015,-0.012,-0.021,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00084,8.5e-05,7.7e-05,0.00085,0.025,0.03,0.0088,0.25,0.27,0.037,3.2e-07,3.6e-07,5.8e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 +31290000,0.73,0.00092,0.0035,0.68,-1.8,-0.99,0.74,-12,-5.5,-3.7e+02,-0.0013,-0.0058,0.00015,-0.013,-0.02,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00084,8.5e-05,7.7e-05,0.00085,0.027,0.033,0.0089,0.26,0.28,0.037,3.2e-07,3.6e-07,5.8e-06,0.027,0.026,0.0005,0.0025,9.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +31390000,0.73,0.00089,0.003,0.69,-1.7,-0.98,0.73,-12,-5.6,-3.7e+02,-0.0013,-0.0058,0.00011,-0.013,-0.018,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00082,8.5e-05,7.6e-05,0.00083,0.025,0.031,0.0088,0.25,0.28,0.036,3.2e-07,3.5e-07,5.8e-06,0.027,0.026,0.0005,0.0025,9.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +31490000,0.73,0.00067,0.0024,0.69,-1.7,-0.98,0.73,-12,-5.7,-3.7e+02,-0.0013,-0.0058,0.00011,-0.014,-0.017,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00082,8.5e-05,7.7e-05,0.00083,0.027,0.033,0.0088,0.27,0.29,0.037,3.2e-07,3.5e-07,5.8e-06,0.027,0.026,0.0005,0.0025,9.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +31590000,0.73,0.00073,0.002,0.69,-1.7,-0.96,0.72,-12,-5.7,-3.7e+02,-0.0013,-0.0058,7.3e-05,-0.014,-0.016,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.0008,8.5e-05,7.6e-05,0.00081,0.025,0.031,0.0087,0.26,0.29,0.037,3.1e-07,3.4e-07,5.8e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31690000,0.73,0.00045,0.0013,0.69,-1.7,-0.96,0.73,-12,-5.8,-3.7e+02,-0.0013,-0.0058,7.3e-05,-0.014,-0.014,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.0008,8.5e-05,7.6e-05,0.00081,0.027,0.033,0.0087,0.28,0.3,0.037,3.1e-07,3.4e-07,5.8e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31790000,0.73,0.00038,0.00063,0.69,-1.6,-0.94,0.73,-13,-5.9,-3.7e+02,-0.0014,-0.0058,3.4e-05,-0.014,-0.013,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00078,8.5e-05,7.6e-05,0.00079,0.025,0.031,0.0087,0.27,0.3,0.037,3.1e-07,3.3e-07,5.7e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31890000,0.73,0.00011,-6.5e-05,0.69,-1.6,-0.93,0.72,-13,-6,-3.7e+02,-0.0014,-0.0058,3.4e-05,-0.015,-0.011,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00078,8.5e-05,7.6e-05,0.00079,0.027,0.034,0.0087,0.29,0.31,0.037,3.1e-07,3.3e-07,5.7e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31990000,0.72,8.4e-05,-0.00058,0.69,-1.6,-0.92,0.71,-13,-6.1,-3.7e+02,-0.0014,-0.0058,-8.5e-07,-0.015,-0.0097,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00076,8.4e-05,7.5e-05,0.00077,0.025,0.032,0.0086,0.28,0.31,0.036,3e-07,3.2e-07,5.7e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +32090000,0.72,-0.00023,-0.0013,0.69,-1.5,-0.91,0.72,-13,-6.2,-3.7e+02,-0.0014,-0.0058,-1.3e-06,-0.016,-0.0081,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00076,8.5e-05,7.6e-05,0.00077,0.027,0.034,0.0087,0.3,0.32,0.037,3.1e-07,3.3e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32190000,0.72,-0.00033,-0.0021,0.69,-1.5,-0.9,0.72,-13,-6.3,-3.7e+02,-0.0014,-0.0058,-3.5e-05,-0.016,-0.0061,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00074,8.4e-05,7.5e-05,0.00075,0.025,0.032,0.0086,0.29,0.32,0.036,3e-07,3.2e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32290000,0.72,-0.00058,-0.0029,0.69,-1.5,-0.89,0.71,-14,-6.3,-3.7e+02,-0.0014,-0.0058,-3.5e-05,-0.017,-0.0045,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00074,8.5e-05,7.5e-05,0.00075,0.027,0.034,0.0086,0.3,0.33,0.037,3e-07,3.2e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32390000,0.72,-0.00068,-0.0035,0.69,-1.5,-0.87,0.71,-14,-6.4,-3.7e+02,-0.0014,-0.0057,-5.8e-05,-0.017,-0.0037,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00072,8.4e-05,7.5e-05,0.00073,0.025,0.032,0.0085,0.3,0.33,0.037,3e-07,3.1e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32490000,0.72,-0.00081,-0.0038,0.69,-1.4,-0.86,0.72,-14,-6.5,-3.7e+02,-0.0014,-0.0057,-5.8e-05,-0.017,-0.0025,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00072,8.4e-05,7.5e-05,0.00073,0.027,0.034,0.0085,0.31,0.34,0.037,3e-07,3.1e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32590000,0.72,-0.00071,-0.004,0.69,-1.4,-0.85,0.72,-14,-6.6,-3.7e+02,-0.0014,-0.0057,-8.1e-05,-0.017,-0.0017,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.0007,8.4e-05,7.5e-05,0.00071,0.025,0.032,0.0084,0.31,0.34,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32690000,0.72,-0.00076,-0.0041,0.69,-1.4,-0.84,0.71,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-8.1e-05,-0.017,-0.0011,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.0007,8.4e-05,7.5e-05,0.00071,0.027,0.034,0.0085,0.32,0.35,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32790000,0.72,-0.00063,-0.0041,0.69,-1.3,-0.83,0.71,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-0.0001,-0.017,-0.00017,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00068,8.3e-05,7.4e-05,0.00069,0.025,0.032,0.0084,0.32,0.35,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32890000,0.72,-0.00054,-0.004,0.69,-1.3,-0.82,0.71,-14,-6.8,-3.7e+02,-0.0014,-0.0057,-0.0001,-0.018,0.00098,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00068,8.4e-05,7.4e-05,0.00069,0.027,0.033,0.0084,0.33,0.36,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32990000,0.72,-0.00031,-0.004,0.69,-1.3,-0.81,0.7,-15,-6.9,-3.7e+02,-0.0014,-0.0057,-0.00012,-0.018,0.0023,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00066,8.3e-05,7.4e-05,0.00067,0.025,0.031,0.0083,0.33,0.36,0.036,2.9e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +33090000,0.72,-0.00035,-0.0041,0.69,-1.3,-0.8,0.7,-15,-7,-3.7e+02,-0.0014,-0.0057,-0.00012,-0.018,0.0029,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00066,8.3e-05,7.4e-05,0.00067,0.026,0.033,0.0084,0.34,0.37,0.036,2.9e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +33190000,0.72,0.0032,-0.0032,0.7,-1.2,-0.78,0.64,-15,-7,-3.7e+02,-0.0015,-0.0057,-0.00014,-0.018,0.0036,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00065,8.2e-05,7.4e-05,0.00064,0.025,0.031,0.0083,0.34,0.37,0.036,2.8e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +33290000,0.67,0.015,-0.0025,0.75,-1.2,-0.77,0.62,-15,-7.1,-3.7e+02,-0.0015,-0.0057,-0.00014,-0.018,0.004,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00075,8.2e-05,7.4e-05,0.00055,0.026,0.033,0.0083,0.35,0.38,0.036,2.8e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0.0025,9.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33390000,0.56,0.013,-0.0027,0.83,-1.2,-0.75,0.81,-15,-7.2,-3.7e+02,-0.0015,-0.0057,-0.00015,-0.018,0.0051,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8e-05,7.5e-05,0.00038,0.025,0.03,0.0083,0.35,0.38,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33490000,0.43,0.0066,-0.00011,0.9,-1.2,-0.75,0.83,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-0.00015,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.9e-05,7.7e-05,0.00022,0.026,0.032,0.0081,0.36,0.39,0.036,2.8e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33590000,0.27,0.00059,-0.0025,0.96,-1.2,-0.74,0.79,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-0.00017,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.7e-05,7.9e-05,8.1e-05,0.026,0.031,0.0078,0.35,0.39,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33690000,0.1,-0.0028,-0.0055,0.99,-1.1,-0.74,0.8,-15,-7.4,-3.7e+02,-0.0015,-0.0057,-0.00017,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.5e-05,8.1e-05,9.7e-06,0.028,0.034,0.0076,0.37,0.4,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33790000,-0.068,-0.0044,-0.0073,1,-1.1,-0.72,0.78,-16,-7.5,-3.7e+02,-0.0015,-0.0057,-0.00019,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.3e-05,8e-05,8.1e-06,0.028,0.034,0.0074,0.36,0.4,0.036,2.7e-07,2.8e-07,5.5e-06,0.027,0.025,0.0005,0.0025,8.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33890000,-0.24,-0.0057,-0.008,0.97,-1,-0.7,0.77,-16,-7.5,-3.7e+02,-0.0015,-0.0057,-0.00019,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.2e-05,8.2e-05,7.6e-05,0.032,0.039,0.0072,0.38,0.41,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33990000,-0.38,-0.004,-0.012,0.92,-0.96,-0.65,0.74,-16,-7.6,-3.7e+02,-0.0015,-0.0057,-0.00022,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.001,6.9e-05,7.8e-05,0.00019,0.032,0.038,0.007,0.37,0.41,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34090000,-0.49,-0.003,-0.013,0.87,-0.91,-0.61,0.74,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00022,-0.018,0.0052,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0009,6.9e-05,7.8e-05,0.0003,0.037,0.044,0.0069,0.39,0.42,0.036,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34190000,-0.56,-0.0023,-0.012,0.83,-0.89,-0.56,0.74,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00023,-0.021,0.0073,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00079,6.5e-05,7.3e-05,0.00038,0.037,0.043,0.0067,0.38,0.42,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34290000,-0.6,-0.0032,-0.0091,0.8,-0.84,-0.51,0.74,-16,-7.8,-3.7e+02,-0.0015,-0.0057,-0.00023,-0.021,0.0073,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00074,6.5e-05,7.2e-05,0.00044,0.043,0.049,0.0066,0.39,0.43,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34390000,-0.63,-0.0033,-0.0067,0.78,-0.83,-0.47,0.74,-16,-7.9,-3.7e+02,-0.0016,-0.0057,-0.00025,-0.026,0.013,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00068,6.1e-05,6.6e-05,0.00047,0.042,0.048,0.0065,0.39,0.43,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34490000,-0.65,-0.0042,-0.0045,0.76,-0.77,-0.42,0.74,-16,-7.9,-3.7e+02,-0.0016,-0.0057,-0.00025,-0.026,0.013,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00066,6.1e-05,6.6e-05,0.00049,0.049,0.055,0.0064,0.4,0.44,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34590000,-0.66,-0.0035,-0.0035,0.75,-0.78,-0.4,0.73,-17,-8,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.036,0.022,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00063,5.6e-05,6e-05,0.0005,0.048,0.052,0.0063,0.4,0.44,0.034,2.7e-07,2.7e-07,5.5e-06,0.026,0.024,0.0005,0.0025,7.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34690000,-0.66,-0.0039,-0.0027,0.75,-0.72,-0.36,0.73,-17,-8,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.036,0.022,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00062,5.6e-05,6e-05,0.00051,0.055,0.06,0.0063,0.41,0.45,0.034,2.7e-07,2.7e-07,5.5e-06,0.026,0.024,0.0005,0.0025,7.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34790000,-0.67,-0.0027,-0.0025,0.75,-0.73,-0.35,0.72,-17,-8.1,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.047,0.033,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00061,5.1e-05,5.5e-05,0.0005,0.052,0.056,0.0062,0.41,0.45,0.034,2.6e-07,2.7e-07,5.5e-06,0.025,0.024,0.0005,0.0025,7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34890000,-0.67,-0.0028,-0.0024,0.74,-0.68,-0.31,0.72,-17,-8.1,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.047,0.033,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00061,5.1e-05,5.5e-05,0.00051,0.059,0.064,0.0062,0.42,0.46,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.024,0.0005,0.0025,6.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34990000,-0.67,-0.009,-0.0051,0.74,0.34,0.28,-0.13,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.06,0.07,0.0068,0.42,0.45,0.034,2.6e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35090000,-0.67,-0.0091,-0.0051,0.74,0.47,0.3,-0.18,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.065,0.077,0.0068,0.43,0.46,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35190000,-0.67,-0.0091,-0.0052,0.74,0.49,0.33,-0.18,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.07,0.082,0.0067,0.44,0.47,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35290000,-0.67,-0.0091,-0.0052,0.74,0.51,0.37,-0.18,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.076,0.087,0.0066,0.46,0.48,0.033,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35390000,-0.67,-0.0092,-0.0052,0.74,0.54,0.4,-0.18,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.082,0.094,0.0066,0.47,0.49,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35490000,-0.67,-0.0092,-0.0052,0.74,0.56,0.44,-0.18,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.6e-05,5e-05,0.00052,0.088,0.1,0.0065,0.49,0.51,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35590000,-0.67,-0.0061,-0.0053,0.74,0.45,0.36,-0.19,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00025,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00057,3.9e-05,4.3e-05,0.00049,0.07,0.076,0.0062,0.48,0.5,0.033,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35690000,-0.67,-0.0061,-0.0053,0.74,0.47,0.39,-0.19,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00025,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00057,3.9e-05,4.3e-05,0.00049,0.075,0.081,0.0062,0.5,0.52,0.033,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35790000,-0.67,-0.0038,-0.0053,0.74,0.38,0.33,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.064,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,3.4e-05,3.8e-05,0.00048,0.062,0.066,0.0059,0.49,0.51,0.033,2.7e-07,2.7e-07,5.4e-06,0.025,0.023,0.0005,0.0025,6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35890000,-0.67,-0.0038,-0.0053,0.74,0.4,0.36,-0.19,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.064,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,3.4e-05,3.8e-05,0.00048,0.067,0.071,0.0059,0.5,0.53,0.033,2.7e-07,2.7e-07,5.4e-06,0.025,0.023,0.0005,0.0025,5.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35990000,-0.67,-0.0018,-0.0053,0.74,0.33,0.31,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.072,0.056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,3e-05,3.4e-05,0.00047,0.058,0.06,0.0057,0.5,0.52,0.033,2.7e-07,2.7e-07,5.4e-06,0.025,0.023,0.0005,0.0025,5.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36090000,-0.67,-0.0019,-0.0052,0.74,0.34,0.33,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.072,0.056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,3e-05,3.4e-05,0.00047,0.063,0.066,0.0057,0.51,0.53,0.032,2.7e-07,2.8e-07,5.4e-06,0.025,0.023,0.0005,0.0025,5.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36190000,-0.67,-0.00043,-0.0051,0.74,0.28,0.28,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00022,-0.083,0.066,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,2.7e-05,3.1e-05,0.00047,0.055,0.057,0.0056,0.5,0.53,0.032,2.7e-07,2.8e-07,5.4e-06,0.024,0.023,0.0005,0.0025,5.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36290000,-0.67,-0.00055,-0.0051,0.74,0.29,0.31,-0.18,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00022,-0.083,0.066,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,2.7e-05,3.2e-05,0.00047,0.061,0.063,0.0056,0.52,0.54,0.032,2.8e-07,2.8e-07,5.4e-06,0.024,0.023,0.0005,0.0025,5.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36390000,-0.67,0.00056,-0.0049,0.74,0.24,0.26,-0.19,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.096,0.076,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,2.4e-05,2.9e-05,0.00047,0.054,0.055,0.0055,0.51,0.54,0.032,2.8e-07,2.8e-07,5.4e-06,0.024,0.022,0.0005,0.0025,5.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36490000,-0.67,0.00047,-0.0049,0.74,0.25,0.28,-0.18,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.097,0.076,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.4e-05,2.9e-05,0.00047,0.059,0.061,0.0055,0.53,0.55,0.032,2.8e-07,2.8e-07,5.4e-06,0.024,0.022,0.0005,0.0025,5.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36590000,-0.67,0.0013,-0.0047,0.74,0.21,0.24,-0.18,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.11,0.086,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.2e-05,2.7e-05,0.00047,0.053,0.054,0.0055,0.52,0.55,0.031,2.8e-07,2.8e-07,5.4e-06,0.023,0.022,0.0005,0.0025,5.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36690000,-0.67,0.0013,-0.0047,0.74,0.21,0.26,-0.17,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.11,0.086,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.2e-05,2.7e-05,0.00047,0.058,0.06,0.0055,0.54,0.56,0.031,2.8e-07,2.8e-07,5.4e-06,0.023,0.022,0.0005,0.0025,5.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36790000,-0.67,0.0019,-0.0045,0.74,0.18,0.22,-0.17,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.12,0.095,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.1e-05,2.6e-05,0.00048,0.052,0.053,0.0055,0.53,0.56,0.031,2.8e-07,2.8e-07,5.4e-06,0.022,0.021,0.0005,0.0025,5.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36890000,-0.67,0.0018,-0.0045,0.74,0.18,0.24,-0.16,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.12,0.096,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.1e-05,2.6e-05,0.00048,0.057,0.059,0.0056,0.55,0.57,0.031,2.8e-07,2.8e-07,5.4e-06,0.022,0.021,0.0005,0.0025,5.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36990000,-0.67,0.0023,-0.0043,0.74,0.15,0.2,-0.16,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.14,0.1,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2e-05,2.5e-05,0.00048,0.051,0.052,0.0056,0.54,0.57,0.031,2.8e-07,2.8e-07,5.4e-06,0.021,0.02,0.0005,0.0025,5.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37090000,-0.67,0.0023,-0.0042,0.74,0.15,0.21,-0.15,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.14,0.1,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2e-05,2.5e-05,0.00048,0.056,0.058,0.0057,0.55,0.58,0.031,2.8e-07,2.9e-07,5.4e-06,0.021,0.02,0.0005,0.0025,5.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37190000,-0.67,0.0026,-0.0041,0.74,0.12,0.18,-0.14,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.15,0.11,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.9e-05,2.4e-05,0.00048,0.05,0.052,0.0057,0.55,0.58,0.031,2.8e-07,2.9e-07,5.4e-06,0.021,0.019,0.0005,0.0025,5.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37290000,-0.67,0.0026,-0.0041,0.74,0.12,0.19,-0.14,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.15,0.11,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.9e-05,2.4e-05,0.00048,0.055,0.057,0.0059,0.56,0.59,0.031,2.8e-07,2.9e-07,5.4e-06,0.021,0.019,0.0005,0.0025,5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37390000,-0.67,0.0028,-0.0039,0.74,0.1,0.16,-0.13,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.16,0.12,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00048,0.05,0.051,0.0059,0.56,0.59,0.031,2.9e-07,2.9e-07,5.4e-06,0.02,0.018,0.0005,0.0025,5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37490000,-0.67,0.0028,-0.0038,0.74,0.099,0.17,-0.13,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.16,0.12,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00049,0.054,0.056,0.006,0.57,0.6,0.031,2.9e-07,2.9e-07,5.4e-06,0.02,0.018,0.0005,0.0025,4.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37590000,-0.67,0.003,-0.0037,0.74,0.079,0.14,-0.12,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.17,0.12,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00049,0.049,0.05,0.0061,0.57,0.6,0.031,2.9e-07,2.9e-07,5.4e-06,0.019,0.018,0.0005,0.0025,4.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37690000,-0.67,0.0029,-0.0037,0.74,0.077,0.15,-0.11,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.17,0.12,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00049,0.053,0.055,0.0062,0.58,0.61,0.031,2.9e-07,2.9e-07,5.4e-06,0.019,0.018,0.0005,0.0025,4.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37790000,-0.67,0.003,-0.0036,0.74,0.061,0.13,-0.1,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00025,-0.18,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.2e-05,0.00049,0.048,0.049,0.0063,0.58,0.61,0.03,2.9e-07,2.9e-07,5.4e-06,0.018,0.017,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37890000,-0.67,0.003,-0.0036,0.74,0.058,0.13,-0.094,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00025,-0.18,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.2e-05,0.00049,0.052,0.053,0.0064,0.59,0.62,0.03,2.9e-07,2.9e-07,5.4e-06,0.018,0.017,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37990000,-0.67,0.0031,-0.0036,0.74,0.044,0.11,-0.085,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.19,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.7e-05,2.2e-05,0.00049,0.047,0.048,0.0065,0.59,0.62,0.031,2.9e-07,2.9e-07,5.4e-06,0.017,0.016,0.0005,0.0025,4.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38090000,-0.67,0.003,-0.0035,0.74,0.04,0.12,-0.075,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.19,0.13,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.051,0.052,0.0066,0.6,0.63,0.031,2.9e-07,3e-07,5.4e-06,0.017,0.016,0.0005,0.0025,4.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38190000,-0.67,0.0031,-0.0034,0.74,0.027,0.1,-0.067,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.2,0.14,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.7e-05,2.2e-05,0.00049,0.045,0.047,0.0067,0.6,0.63,0.031,2.9e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38290000,-0.67,0.0031,-0.0034,0.74,0.024,0.1,-0.06,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.2,0.14,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.049,0.051,0.0068,0.61,0.64,0.031,2.9e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38390000,-0.67,0.0031,-0.0033,0.74,0.015,0.087,-0.052,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.2,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.7e-05,2.2e-05,0.00049,0.044,0.046,0.0069,0.61,0.64,0.031,2.9e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38490000,-0.67,0.0031,-0.0033,0.74,0.012,0.089,-0.045,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.2,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.048,0.049,0.007,0.62,0.65,0.031,3e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38590000,-0.67,0.0031,-0.0031,0.74,0.0072,0.076,-0.038,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.043,0.044,0.0071,0.62,0.65,0.031,3e-07,3e-07,5.4e-06,0.015,0.014,0.0005,0.0025,4.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38690000,-0.67,0.003,-0.0031,0.74,0.003,0.076,-0.031,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.046,0.048,0.0072,0.63,0.66,0.031,3e-07,3e-07,5.4e-06,0.015,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38790000,-0.67,0.003,-0.0031,0.74,-0.0022,0.063,-0.023,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00028,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.042,0.043,0.0073,0.63,0.66,0.031,3e-07,3e-07,5.4e-06,0.014,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38890000,-0.67,0.0028,-0.0031,0.74,-0.012,0.052,0.48,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00028,-0.21,0.14,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.0005,0.045,0.046,0.0075,0.64,0.67,0.032,3e-07,3e-07,5.4e-06,0.014,0.014,0.0005,0.0025,4.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index 25937ccda5..d8c44da9a3 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,351 +1,351 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.00026,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6.2e-07,0.0026,0.0026,0.00033,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-2.1e-11,0,0,-6.8e-08,0,0,0,0,0,0,0,0,6.7e-07,0.0027,0.0027,0.00044,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -290000,1,-0.012,-0.011,0.00045,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.9e-09,-5.9e-09,-2.3e-10,0,0,-2.9e-06,0,0,0,0,0,0,0,0,7.4e-07,0.0029,0.0029,0.00021,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -390000,1,-0.012,-0.011,0.0005,-0.0025,-0.006,-0.063,-0.00056,-0.00072,-0.013,-7.2e-09,-5.8e-09,-1.5e-10,0,0,8.8e-06,0,0,0,0,0,0,0,0,8.2e-07,0.0031,0.0031,0.0003,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -490000,1,-0.012,-0.012,0.00056,-0.0007,-0.0063,-0.069,-0.00015,-0.00047,-0.011,-1.2e-06,7.4e-07,4.4e-09,0,0,1.6e-05,0,0,0,0,0,0,0,0,9.2e-07,0.0034,0.0034,0.00018,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -590000,1,-0.012,-0.012,0.00058,-0.002,-0.009,-0.12,-0.00028,-0.0013,-0.029,-1.3e-06,7.7e-07,4.9e-09,0,0,6.4e-05,0,0,0,0,0,0,0,0,1e-06,0.0037,0.0037,0.00026,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -690000,1,-0.012,-0.012,0.00062,5.3e-05,-0.0089,-0.05,-8e-05,-0.00079,-0.0088,-5.6e-06,1.6e-06,-1.5e-08,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.2e-06,0.004,0.004,0.00016,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0026,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -790000,1,-0.012,-0.012,0.00062,0.0022,-0.011,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.6e-06,-1.4e-08,0,0,-0.00013,0,0,0,0,0,0,0,0,1.3e-06,0.0044,0.0044,0.00022,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.0026,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -890000,1,-0.012,-0.013,0.00063,0.0031,-0.0085,-0.093,0.00015,-0.0011,-0.031,-2.2e-05,1.1e-06,6.2e-07,0,0,-3.3e-05,0,0,0,0,0,0,0,0,1.5e-06,0.0048,0.0048,0.00014,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.0016,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -990000,1,-0.012,-0.013,0.0006,0.006,-0.0099,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1.1e-06,6.3e-07,0,0,1.3e-05,0,0,0,0,0,0,0,0,1.7e-06,0.0053,0.0053,0.00018,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.0016,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1090000,1,-0.012,-0.013,0.00056,0.011,-0.013,-0.13,0.00077,-0.0015,-0.062,-6.1e-05,-1.5e-05,3e-06,0,0,4.5e-05,0,0,0,0,0,0,0,0,1.8e-06,0.0057,0.0057,0.00013,0.92,0.92,0.69,0.17,0.17,0.2,0.0098,0.0098,0.001,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1190000,1,-0.012,-0.013,0.0005,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,2.8e-06,0,0,-0.00048,0,0,0,0,0,0,0,0,2.1e-06,0.0063,0.0063,0.00016,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.001,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1290000,1,-0.012,-0.014,0.00045,0.019,-0.018,-0.11,0.002,-0.0024,-0.048,-0.00017,-9.6e-05,1e-05,0,0,-0.00075,0,0,0,0,0,0,0,0,2.2e-06,0.0064,0.0064,0.00011,0.88,0.88,0.41,0.15,0.15,0.18,0.0095,0.0095,0.00069,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1390000,1,-0.012,-0.014,0.00041,0.026,-0.023,-0.097,0.0043,-0.0045,-0.038,-0.00016,-9.1e-05,9.9e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,2.4e-06,0.0071,0.0071,0.00014,1.1,1.1,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00069,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1490000,1,-0.012,-0.014,0.0004,0.024,-0.021,-0.12,0.0034,-0.0033,-0.053,-0.00039,-0.00033,2.6e-05,0,0,-0.0012,0,0,0,0,0,0,0,0,2.3e-06,0.0067,0.0067,0.0001,0.95,0.95,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00049,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1590000,1,-0.012,-0.014,0.00042,0.031,-0.025,-0.13,0.0061,-0.0056,-0.063,-0.00039,-0.00033,2.6e-05,0,0,-0.0014,0,0,0,0,0,0,0,0,2.6e-06,0.0073,0.0073,0.00012,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00049,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1690000,1,-0.012,-0.014,0.00045,0.028,-0.02,-0.13,0.0044,-0.0037,-0.068,-0.00074,-0.00073,4.8e-05,0,0,-0.0018,0,0,0,0,0,0,0,0,2.3e-06,0.0064,0.0064,9.2e-05,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.00035,0.04,0.04,0.04,0,0,0,0,0,0,0,0 -1790000,1,-0.012,-0.014,0.00041,0.036,-0.024,-0.13,0.0076,-0.006,-0.067,-0.00073,-0.00073,4.7e-05,0,0,-0.0028,0,0,0,0,0,0,0,0,2.6e-06,0.007,0.007,0.00011,1.3,1.3,0.16,0.2,0.2,0.12,0.0078,0.0078,0.00035,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -1890000,1,-0.012,-0.014,0.0004,0.043,-0.026,-0.14,0.012,-0.0085,-0.075,-0.00073,-0.00072,4.7e-05,0,0,-0.0032,0,0,0,0,0,0,0,0,2.9e-06,0.0076,0.0076,0.00013,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.00035,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -1990000,1,-0.011,-0.014,0.00039,0.035,-0.018,-0.14,0.0082,-0.0055,-0.074,-0.0011,-0.0013,7e-05,0,0,-0.0046,0,0,0,0,0,0,0,0,2.3e-06,0.006,0.006,9.7e-05,1.3,1.3,0.13,0.2,0.2,0.11,0.0066,0.0066,0.00026,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -2090000,1,-0.011,-0.014,0.00041,0.042,-0.021,-0.14,0.012,-0.0075,-0.071,-0.0011,-0.0012,6.9e-05,0,0,-0.0065,0,0,0,0,0,0,0,0,2.6e-06,0.0066,0.0066,0.00011,1.7,1.7,0.12,0.31,0.31,0.11,0.0066,0.0066,0.00026,0.04,0.04,0.039,0,0,0,0,0,0,0,0 -2190000,1,-0.011,-0.014,0.00034,0.033,-0.014,-0.14,0.0081,-0.0044,-0.077,-0.0015,-0.0018,8.8e-05,0,0,-0.0075,0,0,0,0,0,0,0,0,2e-06,0.005,0.005,8.7e-05,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,0.0002,0.04,0.04,0.038,0,0,0,0,0,0,0,0 -2290000,1,-0.011,-0.014,0.00033,0.038,-0.015,-0.14,0.012,-0.0059,-0.075,-0.0014,-0.0018,8.7e-05,0,0,-0.0098,0,0,0,0,0,0,0,0,2.2e-06,0.0054,0.0054,9.9e-05,1.5,1.5,0.11,0.29,0.29,0.1,0.0055,0.0055,0.0002,0.04,0.04,0.038,0,0,0,0,0,0,0,0 -2390000,1,-0.011,-0.013,0.00033,0.029,-0.01,-0.14,0.0075,-0.0034,-0.072,-0.0017,-0.0023,9.9e-05,0,0,-0.012,0,0,0,0,0,0,0,0,1.6e-06,0.004,0.004,8e-05,1,1,0.1,0.19,0.19,0.098,0.0045,0.0045,0.00015,0.04,0.04,0.038,0,0,0,0,0,0,0,0 -2490000,1,-0.011,-0.014,0.0004,0.033,-0.0092,-0.14,0.011,-0.0044,-0.079,-0.0017,-0.0023,9.9e-05,0,0,-0.013,0,0,0,0,0,0,0,0,1.7e-06,0.0044,0.0044,8.9e-05,1.3,1.3,0.1,0.28,0.28,0.097,0.0045,0.0045,0.00015,0.04,0.04,0.037,0,0,0,0,0,0,0,0 -2590000,1,-0.01,-0.013,0.00029,0.023,-0.0062,-0.15,0.0066,-0.0024,-0.085,-0.0019,-0.0027,0.00011,0,0,-0.015,0,0,0,0,0,0,0,0,1.3e-06,0.0032,0.0032,7.3e-05,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,0.00012,0.04,0.04,0.036,0,0,0,0,0,0,0,0 -2690000,1,-0.01,-0.013,0.00033,0.027,-0.0054,-0.15,0.0091,-0.0031,-0.084,-0.0018,-0.0027,0.00011,0,0,-0.018,0,0,0,0,0,0,0,0,1.4e-06,0.0035,0.0035,8.1e-05,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,0.00012,0.04,0.04,0.036,0,0,0,0,0,0,0,0 -2790000,1,-0.01,-0.013,0.00026,0.021,-0.0031,-0.14,0.0059,-0.0017,-0.081,-0.002,-0.003,0.00011,0,0,-0.022,0,0,0,0,0,0,0,0,1e-06,0.0027,0.0027,6.7e-05,0.77,0.77,0.096,0.16,0.16,0.089,0.0032,0.0032,9.8e-05,0.04,0.04,0.035,0,0,0,0,0,0,0,0 -2890000,1,-0.01,-0.013,0.00018,0.026,-0.005,-0.14,0.0082,-0.0021,-0.081,-0.0019,-0.003,0.00011,0,0,-0.025,0,0,0,0,0,0,0,0,1.1e-06,0.0029,0.0029,7.4e-05,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,9.8e-05,0.04,0.04,0.034,0,0,0,0,0,0,0,0 -2990000,1,-0.01,-0.013,0.00018,0.02,-0.0038,-0.15,0.0054,-0.0013,-0.086,-0.002,-0.0033,0.00011,0,0,-0.028,0,0,0,0,0,0,0,0,8.5e-07,0.0023,0.0023,6.2e-05,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,8e-05,0.04,0.04,0.033,0,0,0,0,0,0,0,0 -3090000,1,-0.01,-0.013,0.00038,0.025,-0.0068,-0.15,0.0077,-0.0019,-0.087,-0.002,-0.0033,0.00011,0,0,-0.031,0,0,0,0,0,0,0,0,9.3e-07,0.0025,0.0025,6.9e-05,0.82,0.82,0.095,0.22,0.22,0.086,0.0027,0.0027,8e-05,0.04,0.04,0.032,0,0,0,0,0,0,0,0 -3190000,1,-0.01,-0.013,0.00041,0.02,-0.0065,-0.15,0.0051,-0.0014,-0.097,-0.0021,-0.0036,0.00011,0,0,-0.033,0,0,0,0,0,0,0,0,7.2e-07,0.002,0.002,5.8e-05,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,6.6e-05,0.04,0.04,0.031,0,0,0,0,0,0,0,0 -3290000,1,-0.01,-0.013,0.00043,0.024,-0.0067,-0.15,0.0073,-0.0021,-0.11,-0.0021,-0.0036,0.00011,0,0,-0.034,0,0,0,0,0,0,0,0,7.8e-07,0.0022,0.0022,6.4e-05,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,6.6e-05,0.04,0.04,0.03,0,0,0,0,0,0,0,0 -3390000,1,-0.0098,-0.013,0.00043,0.019,-0.0037,-0.15,0.005,-0.0014,-0.1,-0.0021,-0.0038,0.00011,0,0,-0.04,0,0,0,0,0,0,0,0,6e-07,0.0017,0.0017,5.5e-05,0.53,0.53,0.095,0.14,0.14,0.085,0.0019,0.0019,5.6e-05,0.04,0.04,0.029,0,0,0,0,0,0,0,0 -3490000,1,-0.0098,-0.013,0.0004,0.025,-0.0025,-0.15,0.0073,-0.0017,-0.1,-0.0021,-0.0038,0.00011,0,0,-0.044,0,0,0,0,0,0,0,0,6.5e-07,0.0019,0.0019,5.9e-05,0.65,0.65,0.095,0.19,0.19,0.086,0.0019,0.0019,5.6e-05,0.04,0.04,0.028,0,0,0,0,0,0,0,0 -3590000,1,-0.0096,-0.013,0.00035,0.022,-0.002,-0.15,0.0051,-0.001,-0.11,-0.0022,-0.004,0.00011,0,0,-0.047,0,0,0,0,0,0,0,0,5e-07,0.0015,0.0015,5.1e-05,0.48,0.48,0.094,0.13,0.13,0.086,0.0016,0.0016,4.7e-05,0.04,0.04,0.026,0,0,0,0,0,0,0,0 -3690000,1,-0.0096,-0.013,0.00033,0.024,-0.0015,-0.15,0.0075,-0.0013,-0.11,-0.0022,-0.004,0.00011,0,0,-0.052,0,0,0,0,0,0,0,0,5.4e-07,0.0017,0.0017,5.6e-05,0.59,0.59,0.093,0.18,0.18,0.085,0.0016,0.0016,4.7e-05,0.04,0.04,0.025,0,0,0,0,0,0,0,0 -3790000,1,-0.0095,-0.012,0.00034,0.02,0.003,-0.15,0.0052,-0.00062,-0.11,-0.0022,-0.0042,0.00011,0,0,-0.055,0,0,0,0,0,0,0,0,4.3e-07,0.0014,0.0014,4.9e-05,0.44,0.44,0.093,0.12,0.12,0.086,0.0014,0.0014,4e-05,0.04,0.04,0.024,0,0,0,0,0,0,0,0 -3890000,1,-0.0094,-0.013,0.00041,0.022,0.0041,-0.14,0.0074,-0.00028,-0.11,-0.0022,-0.0042,0.00011,0,0,-0.059,0,0,0,0,0,0,0,0,4.7e-07,0.0015,0.0015,5.2e-05,0.54,0.54,0.091,0.17,0.17,0.086,0.0014,0.0014,4e-05,0.04,0.04,0.022,0,0,0,0,0,0,0,0 -3990000,1,-0.0094,-0.013,0.00047,0.027,0.0036,-0.14,0.0099,3.3e-05,-0.11,-0.0022,-0.0042,0.00011,0,0,-0.064,0,0,0,0,0,0,0,0,5.1e-07,0.0016,0.0016,5.7e-05,0.66,0.66,0.089,0.22,0.22,0.085,0.0014,0.0014,4e-05,0.04,0.04,0.021,0,0,0,0,0,0,0,0 -4090000,1,-0.0094,-0.012,0.00053,0.023,0.003,-0.12,0.0073,0.00029,-0.098,-0.0022,-0.0044,0.00011,0,0,-0.072,0,0,0,0,0,0,0,0,3.9e-07,0.0013,0.0013,5e-05,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,3.5e-05,0.04,0.04,0.02,0,0,0,0,0,0,0,0 -4190000,1,-0.0095,-0.012,0.00048,0.025,0.0026,-0.12,0.0098,0.00056,-0.1,-0.0022,-0.0044,0.00011,0,0,-0.074,0,0,0,0,0,0,0,0,4.3e-07,0.0015,0.0015,5.3e-05,0.6,0.6,0.086,0.21,0.21,0.086,0.0012,0.0012,3.5e-05,0.04,0.04,0.019,0,0,0,0,0,0,0,0 -4290000,1,-0.0095,-0.012,0.00048,0.023,0.0025,-0.12,0.0071,0.00049,-0.11,-0.0022,-0.0046,0.00011,0,0,-0.077,0,0,0,0,0,0,0,0,3.4e-07,0.0012,0.0012,4.7e-05,0.46,0.46,0.084,0.15,0.15,0.085,0.00095,0.00095,3e-05,0.04,0.04,0.017,0,0,0,0,0,0,0,0 -4390000,1,-0.0095,-0.012,0.00043,0.026,0.00086,-0.11,0.0096,0.00056,-0.094,-0.0022,-0.0046,0.00011,0,0,-0.083,0,0,0,0,0,0,0,0,3.6e-07,0.0013,0.0013,5e-05,0.55,0.55,0.081,0.2,0.2,0.084,0.00095,0.00095,3e-05,0.04,0.04,0.016,0,0,0,0,0,0,0,0 -4490000,1,-0.0095,-0.012,0.00049,0.022,0.0027,-0.11,0.0071,0.00048,-0.095,-0.0022,-0.0048,0.0001,0,0,-0.086,0,0,0,0,0,0,0,0,2.8e-07,0.001,0.001,4.5e-05,0.43,0.43,0.08,0.14,0.14,0.085,0.00079,0.00079,2.6e-05,0.04,0.04,0.015,0,0,0,0,0,0,0,0 -4590000,1,-0.0095,-0.012,0.00054,0.025,0.0014,-0.11,0.0095,0.00067,-0.098,-0.0022,-0.0048,0.0001,0,0,-0.089,0,0,0,0,0,0,0,0,3.1e-07,0.0011,0.0011,4.8e-05,0.51,0.51,0.077,0.19,0.19,0.084,0.00079,0.00079,2.6e-05,0.04,0.04,0.014,0,0,0,0,0,0,0,0 -4690000,1,-0.0095,-0.012,0.00047,0.019,0.0017,-0.1,0.0069,0.00047,-0.09,-0.0021,-0.005,0.0001,0,0,-0.093,0,0,0,0,0,0,0,0,2.4e-07,0.00092,0.00092,4.2e-05,0.39,0.39,0.074,0.14,0.14,0.083,0.00064,0.00064,2.3e-05,0.04,0.04,0.013,0,0,0,0,0,0,0,0 -4790000,1,-0.0094,-0.012,0.00056,0.017,0.0038,-0.099,0.0087,0.00079,-0.092,-0.0021,-0.005,0.0001,0,0,-0.095,0,0,0,0,0,0,0,0,2.6e-07,0.00099,0.00099,4.5e-05,0.47,0.47,0.073,0.18,0.18,0.084,0.00064,0.00064,2.3e-05,0.04,0.04,0.012,0,0,0,0,0,0,0,0 -4890000,1,-0.0093,-0.012,0.00059,0.012,0.0014,-0.093,0.0058,0.00061,-0.088,-0.0021,-0.0051,0.0001,0,0,-0.099,0,0,0,0,0,0,0,0,2e-07,0.0008,0.0008,4.1e-05,0.36,0.36,0.07,0.13,0.13,0.083,0.00052,0.00052,2e-05,0.04,0.04,0.011,0,0,0,0,0,0,0,0 -4990000,1,-0.0093,-0.012,0.00056,0.016,0.002,-0.085,0.0073,0.0008,-0.083,-0.0021,-0.0051,0.0001,0,0,-0.1,0,0,0,0,0,0,0,0,2.1e-07,0.00086,0.00086,4.3e-05,0.43,0.43,0.067,0.17,0.17,0.082,0.00052,0.00052,2e-05,0.04,0.04,0.01,0,0,0,0,0,0,0,0 -5090000,1,-0.0092,-0.012,0.00063,0.012,0.0025,-0.082,0.0051,0.00057,-0.081,-0.0021,-0.0052,0.0001,0,0,-0.1,0,0,0,0,0,0,0,0,1.6e-07,0.00069,0.00069,3.9e-05,0.33,0.33,0.065,0.12,0.12,0.082,0.00042,0.00042,1.8e-05,0.04,0.04,0.0098,0,0,0,0,0,0,0,0 -5190000,1,-0.009,-0.012,0.00067,0.012,0.0059,-0.08,0.0063,0.00097,-0.079,-0.0021,-0.0052,0.0001,0,0,-0.11,0,0,0,0,0,0,0,0,1.8e-07,0.00075,0.00075,4.1e-05,0.39,0.39,0.063,0.16,0.16,0.081,0.00042,0.00042,1.8e-05,0.04,0.04,0.0091,0,0,0,0,0,0,0,0 -5290000,1,-0.0089,-0.012,0.00071,0.01,0.0061,-0.068,0.0044,0.00094,-0.072,-0.0021,-0.0053,0.0001,0,0,-0.11,0,0,0,0,0,0,0,0,1.3e-07,0.0006,0.0006,3.7e-05,0.3,0.3,0.06,0.12,0.12,0.08,0.00034,0.00034,1.6e-05,0.04,0.04,0.0084,0,0,0,0,0,0,0,0 -5390000,1,-0.0088,-0.012,0.00071,0.0098,0.0096,-0.065,0.0054,0.0017,-0.067,-0.0021,-0.0053,0.0001,0,0,-0.11,0,0,0,0,0,0,0,0,1.4e-07,0.00065,0.00065,3.9e-05,0.36,0.36,0.057,0.15,0.15,0.079,0.00034,0.00034,1.6e-05,0.04,0.04,0.0078,0,0,0,0,0,0,0,0 -5490000,1,-0.0089,-0.012,0.0007,0.0091,0.011,-0.06,0.0037,0.0017,-0.065,-0.0021,-0.0054,9.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.1e-07,0.00052,0.00052,3.6e-05,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,1.4e-05,0.04,0.04,0.0073,0,0,0,0,0,0,0,0 -5590000,1,-0.0089,-0.012,0.00062,0.01,0.015,-0.053,0.0048,0.0029,-0.058,-0.0021,-0.0054,9.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,1.2e-07,0.00056,0.00056,3.8e-05,0.32,0.32,0.053,0.15,0.15,0.078,0.00028,0.00028,1.4e-05,0.04,0.04,0.0067,0,0,0,0,0,0,0,0 -5690000,1,-0.0089,-0.011,0.00051,0.0095,0.015,-0.052,0.0034,0.0026,-0.055,-0.002,-0.0055,9.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,9.3e-08,0.00045,0.00045,3.4e-05,0.25,0.25,0.051,0.11,0.11,0.076,0.00022,0.00022,1.3e-05,0.04,0.04,0.0062,0,0,0,0,0,0,0,0 -5790000,1,-0.0088,-0.011,0.00046,0.011,0.017,-0.049,0.0044,0.0042,-0.053,-0.002,-0.0055,9.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,9.8e-08,0.00048,0.00048,3.6e-05,0.29,0.29,0.05,0.14,0.14,0.077,0.00022,0.00022,1.3e-05,0.04,0.04,0.0058,0,0,0,0,0,0,0,0 -5890000,1,-0.0088,-0.011,0.00048,0.011,0.015,-0.048,0.0033,0.0035,-0.056,-0.002,-0.0055,9.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,7.7e-08,0.00039,0.00039,3.3e-05,0.23,0.23,0.048,0.1,0.1,0.075,0.00018,0.00018,1.2e-05,0.04,0.04,0.0054,0,0,0,0,0,0,0,0 -5990000,1,-0.0088,-0.012,0.00045,0.013,0.016,-0.041,0.0046,0.005,-0.05,-0.002,-0.0055,9.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,8.3e-08,0.00041,0.00041,3.5e-05,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,1.2e-05,0.04,0.04,0.005,0,0,0,0,0,0,0,0 -6090000,1,-0.0088,-0.012,0.00025,0.013,0.017,-0.039,0.0059,0.0066,-0.047,-0.002,-0.0055,9.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,8.8e-08,0.00044,0.00044,3.7e-05,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,1.2e-05,0.04,0.04,0.0047,0,0,0,0,0,0,0,0 -6190000,0.98,-0.0066,-0.013,0.19,0.0036,0.019,-0.037,0.004,0.0056,-0.047,-0.0019,-0.0056,9.4e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00012,0.00034,0.00034,0.0031,0.22,0.22,0.042,0.13,0.13,0.073,0.00015,0.00015,1.1e-05,0.04,0.04,0.0044,0,0,0,0,0,0,0,0 -6290000,0.98,-0.0066,-0.013,0.19,0.0021,0.021,-0.041,0.0043,0.0076,-0.053,-0.0019,-0.0056,9.4e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,5.6e-05,0.00034,0.00034,0.0014,0.22,0.22,0.04,0.16,0.16,0.072,0.00015,0.00015,1.1e-05,0.04,0.04,0.0041,0,0,0,0,0,0,0,0 -6390000,0.98,-0.0065,-0.013,0.19,0.0043,0.023,-0.042,0.0046,0.0098,-0.056,-0.0019,-0.0056,9.4e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.8e-05,0.00034,0.00034,0.00099,0.23,0.23,0.039,0.19,0.19,0.072,0.00015,0.00015,1.1e-05,0.04,0.04,0.0038,0,0,0,0,0,0,0,0 -6490000,0.98,-0.0064,-0.013,0.19,0.0022,0.022,-0.039,0.005,0.012,-0.053,-0.0019,-0.0056,9.4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,2.9e-05,0.00034,0.00034,0.00072,0.24,0.24,0.038,0.23,0.23,0.07,0.00015,0.00015,1.1e-05,0.04,0.04,0.0036,0,0,0,0,0,0,0,0 -6590000,0.98,-0.0064,-0.013,0.19,0.0016,0.025,-0.042,0.0052,0.014,-0.056,-0.0019,-0.0056,9.3e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,2.3e-05,0.00035,0.00035,0.00057,0.26,0.26,0.036,0.28,0.28,0.069,0.00015,0.00015,1.1e-05,0.04,0.04,0.0033,0,0,0,0,0,0,0,0 -6690000,0.98,-0.0063,-0.013,0.19,-0.001,0.028,-0.044,0.0052,0.017,-0.057,-0.0019,-0.0056,9.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1.9e-05,0.00035,0.00035,0.00047,0.28,0.28,0.035,0.33,0.33,0.068,0.00015,0.00015,1.1e-05,0.04,0.04,0.0031,0,0,0,0,0,0,0,0 -6790000,0.98,-0.0063,-0.013,0.19,0.00082,0.03,-0.042,0.0051,0.02,-0.058,-0.0019,-0.0056,9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1.7e-05,0.00036,0.00036,0.00041,0.3,0.3,0.034,0.38,0.38,0.068,0.00015,0.00015,1.1e-05,0.04,0.04,0.0029,0,0,0,0,0,0,0,0 -6890000,0.98,-0.0061,-0.013,0.19,0.00042,0.031,-0.038,0.0052,0.023,-0.055,-0.0019,-0.0056,8.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1.5e-05,0.00036,0.00036,0.00036,0.32,0.32,0.032,0.45,0.45,0.067,0.00015,0.00015,1.1e-05,0.04,0.04,0.0027,0,0,0,0,0,0,0,0 -6990000,0.98,-0.006,-0.013,0.19,0.00043,0.032,-0.037,0.0052,0.026,-0.055,-0.0019,-0.0056,8.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1.3e-05,0.00037,0.00037,0.00032,0.35,0.35,0.031,0.52,0.52,0.066,0.00015,0.00015,1.1e-05,0.04,0.04,0.0026,0,0,0,0,0,0,0,0 -7090000,0.98,-0.0059,-0.013,0.19,-0.00042,0.038,-0.037,0.0052,0.029,-0.056,-0.0019,-0.0056,8.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1.2e-05,0.00037,0.00037,0.00029,0.39,0.39,0.03,0.6,0.6,0.066,0.00015,0.00015,1.1e-05,0.04,0.04,0.0024,0,0,0,0,0,0,0,0 -7190000,0.98,-0.0059,-0.013,0.19,-0.00082,0.041,-0.036,0.005,0.033,-0.058,-0.0019,-0.0056,8.7e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1.1e-05,0.00038,0.00038,0.00026,0.42,0.42,0.029,0.69,0.69,0.065,0.00015,0.00015,1.1e-05,0.04,0.04,0.0023,0,0,0,0,0,0,0,0 -7290000,0.98,-0.0058,-0.013,0.19,3.1e-05,0.045,-0.034,0.005,0.037,-0.054,-0.0019,-0.0056,8.7e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,1e-05,0.00039,0.00039,0.00024,0.46,0.46,0.028,0.79,0.79,0.064,0.00015,0.00015,1.1e-05,0.04,0.04,0.0021,0,0,0,0,0,0,0,0 -7390000,0.98,-0.0057,-0.013,0.19,-0.0017,0.049,-0.032,0.0049,0.042,-0.052,-0.0019,-0.0056,8.7e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,9.6e-06,0.0004,0.0004,0.00022,0.51,0.51,0.027,0.9,0.9,0.064,0.00014,0.00014,1.1e-05,0.04,0.04,0.002,0,0,0,0,0,0,0,0 -7490000,0.98,-0.0057,-0.013,0.19,0.00071,0.052,-0.026,0.0049,0.047,-0.046,-0.0019,-0.0056,8.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,9e-06,0.00041,0.00041,0.0002,0.56,0.56,0.026,1,1,0.063,0.00014,0.00014,1e-05,0.04,0.04,0.0019,0,0,0,0,0,0,0,0 -7590000,0.98,-0.0057,-0.013,0.19,0.0017,0.056,-0.023,0.005,0.052,-0.041,-0.0019,-0.0056,8.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,8.5e-06,0.00042,0.00042,0.00019,0.6,0.61,0.025,1.2,1.2,0.062,0.00014,0.00014,1e-05,0.04,0.04,0.0018,0,0,0,0,0,0,0,0 -7690000,0.98,-0.0057,-0.013,0.19,0.0015,0.06,-0.022,0.0051,0.058,-0.036,-0.0019,-0.0056,8.8e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,8.1e-06,0.00043,0.00043,0.00018,0.66,0.66,0.025,1.3,1.3,0.062,0.00014,0.00014,1e-05,0.04,0.04,0.0017,0,0,0,0,0,0,0,0 -7790000,0.98,-0.0056,-0.013,0.19,0.0031,0.063,-0.024,0.0053,0.063,-0.041,-0.0019,-0.0056,8.6e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,7.7e-06,0.00043,0.00043,0.00017,0.72,0.72,0.024,1.5,1.5,0.061,0.00014,0.00014,1e-05,0.04,0.04,0.0016,0,0,0,0,0,0,0,0 -7890000,0.98,-0.0056,-0.013,0.19,0.0018,0.068,-0.025,0.0054,0.07,-0.045,-0.0019,-0.0056,8.4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,7.3e-06,0.00045,0.00045,0.00016,0.78,0.78,0.023,1.7,1.7,0.06,0.00014,0.00014,1e-05,0.04,0.04,0.0015,0,0,0,0,0,0,0,0 -7990000,0.98,-0.0056,-0.013,0.19,0.0021,0.071,-0.021,0.0055,0.075,-0.041,-0.0019,-0.0056,8.4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,7e-06,0.00045,0.00045,0.00015,0.84,0.84,0.022,1.9,1.9,0.059,0.00014,0.00014,1e-05,0.04,0.04,0.0014,0,0,0,0,0,0,0,0 -8090000,0.98,-0.0054,-0.013,0.19,0.0036,0.076,-0.022,0.0058,0.082,-0.044,-0.0019,-0.0056,7.7e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.8e-06,0.00047,0.00047,0.00014,0.92,0.92,0.022,2.1,2.1,0.059,0.00014,0.00014,1e-05,0.04,0.04,0.0014,0,0,0,0,0,0,0,0 -8190000,0.98,-0.0055,-0.013,0.19,0.0043,0.083,-0.018,0.0062,0.091,-0.038,-0.0019,-0.0056,7.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.5e-06,0.00048,0.00048,0.00014,1,1,0.021,2.4,2.4,0.058,0.00014,0.00014,1e-05,0.04,0.04,0.0013,0,0,0,0,0,0,0,0 -8290000,0.98,-0.0055,-0.013,0.19,0.0065,0.087,-0.016,0.0066,0.097,-0.038,-0.0019,-0.0056,6.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,6.3e-06,0.00049,0.00049,0.00013,1.1,1.1,0.02,2.6,2.6,0.057,0.00014,0.00014,1e-05,0.04,0.04,0.0012,0,0,0,0,0,0,0,0 -8390000,0.98,-0.0054,-0.013,0.19,0.0044,0.09,-0.015,0.0071,0.11,-0.036,-0.0019,-0.0056,6.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,6.1e-06,0.0005,0.0005,0.00013,1.1,1.1,0.02,2.9,2.9,0.057,0.00014,0.00014,1e-05,0.04,0.04,0.0012,0,0,0,0,0,0,0,0 -8490000,0.98,-0.0053,-0.013,0.19,0.0043,0.093,-0.017,0.0073,0.11,-0.041,-0.0018,-0.0056,7e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,5.9e-06,0.00051,0.00051,0.00012,1.2,1.2,0.019,3.2,3.2,0.056,0.00014,0.00014,1e-05,0.04,0.04,0.0011,0,0,0,0,0,0,0,0 -8590000,0.98,-0.0053,-0.013,0.19,0.0054,0.098,-0.012,0.0078,0.12,-0.036,-0.0018,-0.0056,6.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5.8e-06,0.00052,0.00052,0.00012,1.3,1.3,0.018,3.6,3.6,0.055,0.00014,0.00014,1e-05,0.04,0.04,0.0011,0,0,0,0,0,0,0,0 -8690000,0.98,-0.0053,-0.013,0.19,0.0054,0.099,-0.014,0.008,0.13,-0.037,-0.0018,-0.0056,6.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5.6e-06,0.00053,0.00053,0.00011,1.4,1.4,0.018,3.9,3.9,0.055,0.00013,0.00013,1e-05,0.04,0.04,0.001,0,0,0,0,0,0,0,0 -8790000,0.98,-0.0053,-0.013,0.19,0.0069,0.1,-0.013,0.0085,0.14,-0.035,-0.0018,-0.0056,6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5.5e-06,0.00054,0.00054,0.00011,1.5,1.5,0.018,4.3,4.3,0.055,0.00013,0.00013,1e-05,0.04,0.04,0.00096,0,0,0,0,0,0,0,0 -8890000,0.98,-0.0054,-0.013,0.19,0.0071,0.1,-0.0091,0.0089,0.14,-0.029,-0.0018,-0.0056,6.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5.3e-06,0.00054,0.00054,0.00011,1.5,1.5,0.017,4.6,4.6,0.054,0.00013,0.00013,1e-05,0.04,0.04,0.00091,0,0,0,0,0,0,0,0 -8990000,0.98,-0.0053,-0.013,0.19,0.0063,0.11,-0.0083,0.0096,0.15,-0.032,-0.0018,-0.0056,7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5.2e-06,0.00056,0.00056,0.0001,1.6,1.6,0.017,5.2,5.2,0.054,0.00013,0.00013,1e-05,0.04,0.04,0.00088,0,0,0,0,0,0,0,0 -9090000,0.98,-0.0053,-0.013,0.19,0.0068,0.11,-0.0093,0.0098,0.15,-0.032,-0.0018,-0.0056,7.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5.1e-06,0.00055,0.00055,0.0001,1.7,1.7,0.016,5.4,5.5,0.053,0.00013,0.00013,1e-05,0.04,0.04,0.00084,0,0,0,0,0,0,0,0 -9190000,0.98,-0.0053,-0.013,0.19,0.011,0.12,-0.0088,0.011,0.17,-0.032,-0.0018,-0.0056,8.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,5e-06,0.00057,0.00057,9.8e-05,1.8,1.8,0.016,6.1,6.1,0.052,0.00013,0.00013,1e-05,0.04,0.04,0.0008,0,0,0,0,0,0,0,0 -9290000,0.98,-0.0052,-0.013,0.19,0.013,0.11,-0.0072,0.011,0.17,-0.03,-0.0017,-0.0056,8.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.9e-06,0.00056,0.00055,9.5e-05,1.8,1.8,0.015,6.3,6.3,0.052,0.00012,0.00012,1e-05,0.04,0.04,0.00076,0,0,0,0,0,0,0,0 -9390000,0.98,-0.0051,-0.013,0.19,0.013,0.12,-0.006,0.013,0.18,-0.03,-0.0017,-0.0056,7.8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.9e-06,0.00057,0.00057,9.3e-05,1.9,1.9,0.015,7,7,0.052,0.00012,0.00012,1e-05,0.04,0.04,0.00073,0,0,0,0,0,0,0,0 -9490000,0.98,-0.0052,-0.013,0.19,0.013,0.12,-0.0044,0.013,0.18,-0.027,-0.0017,-0.0056,8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.8e-06,0.00056,0.00056,9.1e-05,1.9,1.9,0.015,7.2,7.2,0.051,0.00012,0.00012,1e-05,0.04,0.04,0.0007,0,0,0,0,0,0,0,0 -9590000,0.98,-0.0053,-0.013,0.19,0.013,0.12,-0.0043,0.014,0.19,-0.028,-0.0017,-0.0056,6.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.7e-06,0.00057,0.00057,8.9e-05,2,2,0.014,7.9,7.9,0.05,0.00012,0.00012,1e-05,0.04,0.04,0.00067,0,0,0,0,0,0,0,0 -9690000,0.98,-0.0054,-0.013,0.19,0.013,0.11,-0.0014,0.014,0.19,-0.027,-0.0017,-0.0056,5.8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.6e-06,0.00055,0.00055,8.7e-05,2,2,0.014,8.1,8.1,0.05,0.00012,0.00012,1e-05,0.04,0.04,0.00065,0,0,0,0,0,0,0,0 -9790000,0.98,-0.0053,-0.013,0.19,0.015,0.12,-0.0027,0.015,0.2,-0.028,-0.0017,-0.0056,4.2e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.6e-06,0.00057,0.00057,8.5e-05,2.1,2.2,0.014,8.9,8.9,0.05,0.00012,0.00012,1e-05,0.04,0.04,0.00062,0,0,0,0,0,0,0,0 -9890000,0.98,-0.0054,-0.013,0.19,0.017,0.11,-0.0014,0.015,0.19,-0.029,-0.0016,-0.0056,4.5e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.5e-06,0.00054,0.00054,8.4e-05,2.1,2.1,0.013,8.9,8.9,0.049,0.00011,0.00011,1e-05,0.04,0.04,0.0006,0,0,0,0,0,0,0,0 -9990000,0.98,-0.0054,-0.013,0.19,0.019,0.12,-0.00072,0.017,0.2,-0.031,-0.0016,-0.0056,3.1e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.5e-06,0.00056,0.00056,8.2e-05,2.2,2.2,0.013,9.8,9.8,0.049,0.00011,0.00011,1e-05,0.04,0.04,0.00058,0,0,0,0,0,0,0,0 -10090000,0.98,-0.0055,-0.013,0.19,0.017,0.11,0.00048,0.017,0.19,-0.029,-0.0016,-0.0056,1.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.4e-06,0.00053,0.00053,8.1e-05,2.1,2.1,0.013,9.7,9.7,0.048,0.00011,0.00011,1e-05,0.04,0.04,0.00056,0,0,0,0,0,0,0,0 -10190000,0.98,-0.0055,-0.013,0.19,0.015,0.11,0.0014,0.018,0.2,-0.03,-0.0016,-0.0056,-4.4e-06,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.3e-06,0.00055,0.00055,8e-05,2.3,2.3,0.012,11,11,0.048,0.00011,0.00011,1e-05,0.04,0.04,0.00054,0,0,0,0,0,0,0,0 -10290000,0.98,-0.0055,-0.013,0.19,0.016,0.11,0.00029,0.02,0.22,-0.029,-0.0016,-0.0056,2.8e-06,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.3e-06,0.00057,0.00057,7.9e-05,2.4,2.4,0.012,12,12,0.048,0.00011,0.00011,1e-05,0.04,0.04,0.00052,0,0,0,0,0,0,0,0 -10390000,0.98,-0.0055,-0.013,0.19,0.0072,0.0051,-0.0025,0.00076,0.00015,-0.028,-0.0016,-0.0056,7.5e-06,-3.6e-08,2.5e-08,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.3e-06,0.00059,0.00058,7.8e-05,0.25,0.25,0.56,0.25,0.25,0.048,0.00011,0.00011,1e-05,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10490000,0.98,-0.0054,-0.013,0.19,0.0088,0.0075,0.007,0.0015,0.00074,-0.023,-0.0016,-0.0056,-7.8e-06,-1.2e-06,8.4e-07,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.2e-06,0.0006,0.0006,7.6e-05,0.26,0.26,0.55,0.26,0.26,0.057,0.00011,0.00011,9.9e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10590000,0.98,-0.0053,-0.012,0.19,-0.00088,0.0053,0.013,-0.0012,-0.0054,-0.021,-0.0016,-0.0057,1.3e-06,0.00033,4.9e-06,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.2e-06,0.00061,0.00061,7.5e-05,0.13,0.13,0.27,0.13,0.13,0.055,0.00011,0.00011,9.9e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10690000,0.98,-0.0053,-0.013,0.19,0.00044,0.0066,0.016,-0.0012,-0.0048,-0.017,-0.0016,-0.0057,-4.1e-06,0.00033,5.6e-06,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.2e-06,0.00063,0.00063,7.5e-05,0.15,0.15,0.26,0.14,0.14,0.065,0.00011,0.00011,9.8e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10790000,0.98,-0.0054,-0.013,0.19,0.0022,0.003,0.014,-0.00076,-0.0047,-0.015,-0.0016,-0.0057,-6.6e-06,0.00054,0.00046,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.1e-06,0.00062,0.00062,7.4e-05,0.1,0.1,0.17,0.091,0.091,0.062,0.00011,0.00011,9.8e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10890000,0.98,-0.0054,-0.013,0.19,0.0025,0.0064,0.01,-0.00051,-0.0043,-0.018,-0.0016,-0.0057,1.2e-05,0.00054,0.00046,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.1e-06,0.00064,0.00064,7.3e-05,0.12,0.12,0.16,0.098,0.098,0.068,0.00011,0.00011,9.7e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 -10990000,0.98,-0.0054,-0.013,0.19,0.0019,0.012,0.016,-0.00041,-0.0031,-0.011,-0.0015,-0.0057,5.3e-06,0.00071,0.00069,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.1e-06,0.0006,0.0006,7.2e-05,0.096,0.096,0.12,0.073,0.073,0.065,9.9e-05,9.9e-05,9.7e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 -11090000,0.98,-0.0056,-0.013,0.19,0.0031,0.017,0.02,-0.00021,-0.0016,-0.0074,-0.0015,-0.0057,-1.5e-05,0.00071,0.00069,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4.1e-06,0.00062,0.00062,7.2e-05,0.12,0.12,0.11,0.08,0.08,0.069,9.9e-05,9.9e-05,9.7e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 -11190000,0.98,-0.0059,-0.013,0.19,0.0046,0.017,0.026,0.0011,-0.0018,-0.00035,-0.0014,-0.0057,-2.7e-05,0.00073,0.0015,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4e-06,0.00055,0.00055,7.1e-05,0.096,0.096,0.083,0.063,0.063,0.066,9.1e-05,9.1e-05,9.6e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11290000,0.98,-0.0058,-0.013,0.19,0.005,0.018,0.026,0.0016,-6.2e-05,-0.00012,-0.0015,-0.0057,-3.2e-05,0.00073,0.0015,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4e-06,0.00057,0.00057,7.1e-05,0.12,0.12,0.077,0.07,0.07,0.069,9.1e-05,9.1e-05,9.6e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11390000,0.98,-0.006,-0.013,0.19,0.0031,0.015,0.016,0.00094,-0.0009,-0.0086,-0.0014,-0.0058,-3.1e-05,0.0013,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4e-06,0.00049,0.00049,7e-05,0.097,0.097,0.062,0.057,0.057,0.066,8.3e-05,8.3e-05,9.5e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11490000,0.98,-0.0059,-0.013,0.19,0.0023,0.016,0.02,0.0011,0.00068,-0.0025,-0.0014,-0.0058,-2.8e-05,0.0013,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,4e-06,0.0005,0.0005,7e-05,0.12,0.12,0.057,0.065,0.065,0.067,8.3e-05,8.3e-05,9.4e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 -11590000,0.98,-0.0063,-0.013,0.19,0.0039,0.013,0.018,0.0009,-0.00038,-0.0036,-0.0013,-0.0058,-2.8e-05,0.0017,0.0027,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.9e-06,0.00042,0.00042,6.9e-05,0.096,0.096,0.048,0.054,0.054,0.065,7.5e-05,7.5e-05,9.4e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -11690000,0.98,-0.0062,-0.012,0.19,0.0045,0.017,0.018,0.0013,0.0011,-0.0049,-0.0013,-0.0058,-2.1e-05,0.0017,0.0027,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.9e-06,0.00044,0.00044,6.9e-05,0.12,0.12,0.044,0.062,0.062,0.066,7.5e-05,7.5e-05,9.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 -11790000,0.98,-0.0066,-0.012,0.19,0.003,0.011,0.019,0.00076,-0.0018,-0.002,-0.0012,-0.0059,-5.6e-07,0.0022,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.9e-06,0.00037,0.00037,6.8e-05,0.092,0.092,0.037,0.052,0.052,0.063,6.8e-05,6.8e-05,9.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -11890000,0.98,-0.0067,-0.012,0.19,0.0057,0.012,0.017,0.0011,-0.00073,-0.0013,-0.0012,-0.0059,-2.8e-06,0.0022,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.9e-06,0.00038,0.00038,6.8e-05,0.11,0.11,0.034,0.061,0.061,0.063,6.8e-05,6.8e-05,9.2e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -11990000,0.98,-0.0069,-0.012,0.19,0.0085,0.012,0.015,0.0022,-0.0018,-0.0049,-0.0012,-0.0059,5.5e-06,0.0022,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.9e-06,0.00032,0.00032,6.8e-05,0.088,0.088,0.03,0.051,0.051,0.061,6.2e-05,6.2e-05,9.2e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12090000,0.98,-0.0068,-0.012,0.19,0.01,0.012,0.018,0.0031,-0.00066,0.0011,-0.0012,-0.0059,7e-06,0.0022,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00033,0.00033,6.7e-05,0.11,0.11,0.027,0.06,0.06,0.06,6.2e-05,6.2e-05,9.1e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12190000,0.98,-0.0067,-0.012,0.19,0.0081,0.011,0.017,0.0018,0.00038,0.0029,-0.0012,-0.0059,5e-07,0.0024,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00029,0.00029,6.7e-05,0.083,0.083,0.024,0.05,0.05,0.058,5.7e-05,5.7e-05,9e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12290000,0.98,-0.0068,-0.012,0.19,0.0059,0.01,0.016,0.0025,0.0015,0.0039,-0.0012,-0.0059,-4.9e-06,0.0024,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.0003,0.0003,6.7e-05,0.098,0.098,0.022,0.059,0.059,0.058,5.7e-05,5.7e-05,9e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12390000,0.98,-0.0069,-0.012,0.19,0.0043,0.0071,0.014,0.0017,0.00048,-0.0021,-0.0012,-0.0059,-4.5e-06,0.0027,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00026,0.00026,6.7e-05,0.078,0.078,0.02,0.05,0.05,0.056,5.3e-05,5.3e-05,8.9e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12490000,0.98,-0.0069,-0.012,0.19,0.0044,0.0081,0.018,0.0022,0.0012,-6.9e-05,-0.0012,-0.0059,-4.5e-06,0.0026,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00027,0.00027,6.7e-05,0.091,0.091,0.018,0.059,0.059,0.055,5.3e-05,5.3e-05,8.8e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12590000,0.98,-0.0071,-0.012,0.19,0.0081,0.0015,0.019,0.0033,-0.0014,0.0017,-0.0011,-0.006,-4.9e-06,0.0027,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00024,0.00024,6.7e-05,0.072,0.072,0.017,0.049,0.049,0.054,5e-05,5e-05,8.8e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12690000,0.98,-0.007,-0.012,0.19,0.0086,-0.00071,0.019,0.0041,-0.0013,0.0033,-0.0011,-0.006,-1.6e-06,0.0027,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00025,0.00025,6.6e-05,0.084,0.084,0.016,0.058,0.058,0.053,5e-05,5e-05,8.7e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12790000,0.98,-0.0073,-0.012,0.19,0.01,-0.0041,0.021,0.0042,-0.0044,0.0054,-0.0011,-0.006,3e-05,0.0028,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00023,0.00023,6.6e-05,0.068,0.068,0.014,0.049,0.049,0.052,4.7e-05,4.7e-05,8.6e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12890000,0.98,-0.0073,-0.012,0.19,0.01,-0.005,0.022,0.0052,-0.0049,0.0085,-0.0011,-0.006,5.1e-05,0.0028,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.8e-06,0.00024,0.00024,6.6e-05,0.078,0.078,0.013,0.058,0.058,0.051,4.7e-05,4.7e-05,8.6e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -12990000,0.98,-0.0072,-0.012,0.19,0.0082,-0.003,0.022,0.0036,-0.0036,0.0096,-0.0011,-0.006,7.3e-05,0.0028,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00022,0.00022,6.6e-05,0.064,0.064,0.012,0.049,0.049,0.05,4.5e-05,4.5e-05,8.5e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13090000,0.98,-0.0073,-0.012,0.19,0.0091,-0.0028,0.02,0.0045,-0.0038,0.0085,-0.0011,-0.006,0.0001,0.0029,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00023,0.00023,6.6e-05,0.073,0.073,0.012,0.057,0.057,0.049,4.5e-05,4.5e-05,8.4e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13190000,0.98,-0.0073,-0.012,0.19,0.004,-0.0045,0.019,0.00099,-0.0046,0.0091,-0.0011,-0.006,0.00012,0.003,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00021,0.00021,6.6e-05,0.06,0.06,0.011,0.049,0.049,0.047,4.3e-05,4.3e-05,8.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13290000,0.98,-0.0073,-0.012,0.19,0.0037,-0.0055,0.016,0.0013,-0.005,0.0084,-0.0011,-0.006,0.00012,0.003,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00022,0.00022,6.6e-05,0.068,0.068,0.01,0.057,0.057,0.047,4.3e-05,4.3e-05,8.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13390000,0.98,-0.0072,-0.012,0.19,0.0028,-0.0035,0.016,0.00087,-0.0037,0.0091,-0.0011,-0.006,0.00012,0.0031,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00021,0.00021,6.6e-05,0.056,0.056,0.0097,0.049,0.049,0.046,4e-05,4.1e-05,8.2e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13490000,0.98,-0.0072,-0.012,0.19,0.0032,-0.0018,0.015,0.0012,-0.0039,0.0053,-0.0011,-0.006,0.00013,0.0031,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00021,0.00021,6.6e-05,0.064,0.064,0.0093,0.057,0.057,0.045,4e-05,4.1e-05,8.1e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13590000,0.98,-0.0072,-0.012,0.19,0.0077,-0.002,0.017,0.004,-0.0032,0.0038,-0.0011,-0.006,0.00013,0.0034,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.6e-05,0.053,0.053,0.0088,0.048,0.048,0.044,3.9e-05,3.9e-05,8.1e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13690000,0.98,-0.0072,-0.012,0.19,0.0077,-0.0035,0.017,0.0048,-0.0034,0.0064,-0.0011,-0.006,0.00014,0.0034,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00021,0.00021,6.6e-05,0.061,0.061,0.0085,0.056,0.056,0.044,3.9e-05,3.9e-05,8e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13790000,0.98,-0.0071,-0.012,0.19,0.015,0.00073,0.017,0.0083,-0.00094,0.0059,-0.0011,-0.0059,0.00014,0.0037,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.5e-05,0.051,0.051,0.0082,0.048,0.048,0.043,3.7e-05,3.7e-05,7.9e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13890000,0.98,-0.007,-0.012,0.19,0.016,0.0014,0.018,0.0098,-0.00082,0.0081,-0.0011,-0.0059,0.00016,0.0037,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00021,0.00021,6.6e-05,0.058,0.058,0.008,0.056,0.056,0.042,3.7e-05,3.7e-05,7.8e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -13990000,0.98,-0.0071,-0.012,0.19,0.015,0.0016,0.017,0.0074,-0.0023,0.007,-0.0011,-0.006,0.00018,0.0035,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.6e-05,0.049,0.049,0.0077,0.048,0.048,0.041,3.5e-05,3.5e-05,7.7e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14090000,0.98,-0.0071,-0.012,0.19,0.013,-0.0029,0.018,0.0089,-0.0024,0.0034,-0.0011,-0.006,0.00015,0.0037,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00021,0.00021,6.5e-05,0.055,0.055,0.0076,0.055,0.055,0.041,3.5e-05,3.5e-05,7.7e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14190000,0.98,-0.0071,-0.012,0.19,0.01,-0.0016,0.018,0.0081,-0.0018,0.0036,-0.0011,-0.006,0.00013,0.0038,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.6e-05,0.047,0.047,0.0074,0.048,0.048,0.04,3.3e-05,3.3e-05,7.6e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14290000,0.98,-0.007,-0.011,0.19,0.012,-0.0016,0.016,0.009,-0.002,0.0079,-0.0011,-0.006,0.00013,0.0037,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.6e-05,0.053,0.053,0.0073,0.055,0.055,0.04,3.3e-05,3.3e-05,7.5e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 -14390000,0.98,-0.0071,-0.011,0.19,0.012,-0.0046,0.017,0.0084,-0.0031,0.012,-0.0011,-0.006,0.00015,0.0036,0.0032,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.5e-05,0.045,0.045,0.0071,0.048,0.048,0.039,3.1e-05,3.1e-05,7.4e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -14490000,0.98,-0.0072,-0.011,0.19,0.01,-0.0044,0.021,0.0095,-0.0036,0.015,-0.0011,-0.006,0.00013,0.0036,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.5e-05,0.051,0.051,0.0071,0.055,0.055,0.038,3.1e-05,3.1e-05,7.4e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -14590000,0.98,-0.0073,-0.011,0.19,0.0079,-0.0045,0.019,0.0059,-0.0042,0.011,-0.0011,-0.006,0.00013,0.0033,0.0031,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.6e-05,0.044,0.044,0.007,0.047,0.047,0.038,2.9e-05,2.9e-05,7.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -14690000,0.98,-0.0073,-0.011,0.19,0.0071,-0.0045,0.019,0.0066,-0.0046,0.011,-0.0011,-0.006,0.00014,0.0034,0.0031,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.5e-05,0.05,0.05,0.007,0.054,0.054,0.037,2.9e-05,2.9e-05,7.2e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -14790000,0.98,-0.0072,-0.011,0.19,0.0088,0.0025,0.019,0.0053,0.00079,0.014,-0.0012,-0.006,0.00017,0.0034,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.5e-05,0.043,0.043,0.0069,0.047,0.047,0.037,2.7e-05,2.8e-05,7.1e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -14890000,0.98,-0.0071,-0.011,0.19,0.0075,0.0001,0.023,0.0061,0.00093,0.014,-0.0012,-0.006,0.00019,0.0035,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.0002,6.6e-05,0.049,0.049,0.007,0.054,0.054,0.037,2.7e-05,2.8e-05,7e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -14990000,0.98,-0.0073,-0.011,0.19,0.0062,-0.0015,0.026,0.0048,-0.00069,0.017,-0.0012,-0.0061,0.0002,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.5e-05,0.042,0.042,0.0069,0.047,0.047,0.036,2.6e-05,2.6e-05,7e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15090000,0.98,-0.0072,-0.011,0.19,0.0063,-0.00047,0.03,0.0054,-0.00083,0.019,-0.0012,-0.0061,0.00021,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.0002,0.00019,6.5e-05,0.048,0.048,0.007,0.054,0.054,0.036,2.6e-05,2.6e-05,6.9e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15190000,0.98,-0.0073,-0.011,0.19,0.0044,-0.0015,0.03,0.0042,-0.00076,0.021,-0.0012,-0.0061,0.0002,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.6e-05,0.041,0.041,0.007,0.047,0.047,0.036,2.4e-05,2.4e-05,6.8e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15290000,0.98,-0.0074,-0.011,0.19,0.005,-0.0026,0.03,0.0047,-0.00094,0.018,-0.0012,-0.0061,0.00022,0.0036,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.5e-05,0.047,0.047,0.0071,0.054,0.054,0.035,2.4e-05,2.4e-05,6.7e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 -15390000,0.98,-0.0075,-0.011,0.19,0.0052,-0.00022,0.029,0.0038,-0.00064,0.018,-0.0012,-0.0061,0.00022,0.0036,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00018,0.00018,6.5e-05,0.041,0.041,0.0071,0.047,0.047,0.035,2.2e-05,2.2e-05,6.7e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -15490000,0.98,-0.0076,-0.011,0.19,0.0046,-0.0028,0.029,0.0043,-0.00076,0.019,-0.0012,-0.0061,0.00022,0.0037,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.6e-05,0.046,0.046,0.0072,0.053,0.053,0.035,2.2e-05,2.2e-05,6.6e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -15590000,0.98,-0.0078,-0.011,0.19,0.0081,-0.0066,0.029,0.0063,-0.0047,0.018,-0.0011,-0.0061,0.00024,0.0041,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00018,0.00018,6.5e-05,0.04,0.04,0.0072,0.047,0.047,0.035,2.1e-05,2.1e-05,6.5e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -15690000,0.98,-0.0077,-0.011,0.19,0.0099,-0.0097,0.029,0.0072,-0.0055,0.019,-0.0011,-0.0061,0.00026,0.0041,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00019,0.00019,6.5e-05,0.045,0.045,0.0073,0.053,0.053,0.034,2.1e-05,2.1e-05,6.4e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -15790000,0.98,-0.0077,-0.011,0.19,0.0064,-0.009,0.029,0.0055,-0.0043,0.02,-0.0011,-0.0061,0.00029,0.004,0.0025,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00018,0.00018,6.5e-05,0.04,0.04,0.0073,0.046,0.046,0.034,1.9e-05,1.9e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -15890000,0.98,-0.0078,-0.011,0.19,0.0053,-0.0074,0.03,0.0061,-0.0051,0.02,-0.0011,-0.0061,0.00027,0.0042,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00018,0.00018,6.5e-05,0.045,0.045,0.0074,0.053,0.053,0.034,1.9e-05,1.9e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -15990000,0.98,-0.0076,-0.011,0.19,0.0032,-0.0059,0.027,0.0048,-0.0039,0.019,-0.0012,-0.0061,0.00027,0.0044,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00017,0.00017,6.5e-05,0.039,0.039,0.0074,0.046,0.046,0.034,1.7e-05,1.7e-05,6.2e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16090000,0.98,-0.0076,-0.011,0.19,0.0026,-0.0072,0.024,0.005,-0.0046,0.019,-0.0012,-0.0061,0.00027,0.0046,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00018,0.00018,6.5e-05,0.044,0.044,0.0076,0.053,0.053,0.034,1.7e-05,1.7e-05,6.1e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 -16190000,0.98,-0.0075,-0.011,0.19,-0.0013,-0.0049,0.023,0.0027,-0.0034,0.016,-0.0012,-0.0061,0.00025,0.0047,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00017,0.00017,6.5e-05,0.039,0.039,0.0076,0.046,0.046,0.034,1.6e-05,1.6e-05,6e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16290000,0.98,-0.0075,-0.011,0.19,-0.001,-0.0064,0.023,0.0026,-0.004,0.017,-0.0012,-0.0061,0.00026,0.0047,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00017,0.00017,6.5e-05,0.043,0.043,0.0077,0.053,0.053,0.034,1.6e-05,1.6e-05,6e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16390000,0.98,-0.0075,-0.011,0.19,0.0014,-0.0057,0.023,0.0036,-0.003,0.017,-0.0012,-0.0061,0.00028,0.0053,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00017,0.00016,6.5e-05,0.038,0.038,0.0077,0.046,0.046,0.034,1.5e-05,1.5e-05,5.9e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16490000,0.98,-0.0076,-0.011,0.19,0.0034,-0.0073,0.026,0.0038,-0.0037,0.021,-0.0012,-0.0061,0.00028,0.0052,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00017,0.00017,6.5e-05,0.043,0.043,0.0079,0.053,0.053,0.034,1.5e-05,1.5e-05,5.8e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16590000,0.98,-0.0075,-0.011,0.19,0.0075,-0.0073,0.029,0.0034,-0.0029,0.021,-0.0012,-0.0061,0.00028,0.0053,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00016,0.00016,6.5e-05,0.037,0.037,0.0079,0.046,0.046,0.034,1.3e-05,1.3e-05,5.7e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16690000,0.98,-0.0076,-0.011,0.19,0.0089,-0.012,0.029,0.0042,-0.0038,0.022,-0.0012,-0.0061,0.00029,0.0054,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00016,0.00016,6.5e-05,0.042,0.042,0.008,0.053,0.053,0.034,1.3e-05,1.3e-05,5.7e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 -16790000,0.98,-0.0074,-0.011,0.19,0.0098,-0.011,0.028,0.0033,-0.0027,0.022,-0.0012,-0.0061,0.0003,0.0055,0.0026,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00016,0.00016,6.5e-05,0.037,0.037,0.008,0.046,0.046,0.034,1.2e-05,1.2e-05,5.6e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -16890000,0.98,-0.0074,-0.011,0.19,0.0088,-0.011,0.029,0.0042,-0.0038,0.02,-0.0012,-0.0061,0.00033,0.0058,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00016,0.00016,6.5e-05,0.041,0.041,0.0082,0.052,0.052,0.034,1.2e-05,1.2e-05,5.5e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -16990000,0.98,-0.0074,-0.011,0.19,0.0085,-0.011,0.029,0.004,-0.0028,0.019,-0.0013,-0.0061,0.00033,0.0064,0.0025,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00015,0.00015,6.5e-05,0.036,0.036,0.0082,0.046,0.046,0.034,1.1e-05,1.1e-05,5.5e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17090000,0.98,-0.0075,-0.011,0.19,0.01,-0.013,0.028,0.005,-0.0041,0.018,-0.0013,-0.0061,0.00033,0.0066,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00016,0.00015,6.5e-05,0.04,0.041,0.0083,0.052,0.052,0.034,1.1e-05,1.1e-05,5.4e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17190000,0.98,-0.0076,-0.011,0.19,0.0089,-0.018,0.03,0.0033,-0.0075,0.021,-0.0012,-0.0061,0.00031,0.0065,0.002,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00015,0.00015,6.5e-05,0.036,0.036,0.0083,0.046,0.046,0.034,9.9e-06,9.9e-06,5.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17290000,0.98,-0.0076,-0.011,0.19,0.0098,-0.019,0.03,0.0043,-0.0094,0.021,-0.0012,-0.0061,0.00029,0.0067,0.0018,-0.14,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00015,0.00015,6.5e-05,0.04,0.04,0.0084,0.052,0.052,0.034,9.9e-06,9.9e-06,5.2e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17390000,0.98,-0.0075,-0.011,0.19,0.0066,-0.018,0.029,0.0057,-0.0059,0.021,-0.0013,-0.006,0.00031,0.0075,0.0024,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00014,0.00014,6.5e-05,0.035,0.035,0.0084,0.046,0.046,0.034,8.9e-06,8.9e-06,5.2e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17490000,0.98,-0.0075,-0.011,0.19,0.0046,-0.019,0.029,0.0062,-0.0078,0.022,-0.0013,-0.006,0.0003,0.0076,0.0023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.7e-06,0.00015,0.00015,6.5e-05,0.039,0.039,0.0085,0.052,0.052,0.034,8.9e-06,8.9e-06,5.1e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 -17590000,0.98,-0.0074,-0.011,0.19,0.00081,-0.015,0.028,0.0024,-0.0058,0.02,-0.0013,-0.0061,0.0003,0.0074,0.0028,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00014,0.00014,6.5e-05,0.034,0.034,0.0085,0.046,0.046,0.034,8.1e-06,8.1e-06,5e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -17690000,0.98,-0.0075,-0.011,0.19,-0.00015,-0.016,0.029,0.0025,-0.0074,0.023,-0.0013,-0.0061,0.00031,0.0075,0.0028,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00014,0.00014,6.5e-05,0.038,0.038,0.0086,0.052,0.052,0.034,8.1e-06,8.1e-06,5e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -17790000,0.98,-0.0074,-0.011,0.19,0.0025,-0.014,0.029,0.0035,-0.0062,0.028,-0.0014,-0.006,0.00031,0.0078,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00014,0.00013,6.5e-05,0.034,0.034,0.0086,0.046,0.046,0.034,7.3e-06,7.3e-06,4.9e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -17890000,0.98,-0.0073,-0.011,0.19,0.0046,-0.016,0.029,0.0039,-0.0077,0.032,-0.0014,-0.006,0.00032,0.0077,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00014,0.00014,6.5e-05,0.037,0.037,0.0086,0.052,0.052,0.035,7.3e-06,7.3e-06,4.8e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -17990000,0.98,-0.0071,-0.011,0.19,0.004,-0.0092,0.029,0.0031,-0.0019,0.033,-0.0014,-0.006,0.00032,0.0081,0.0045,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00013,0.00013,6.4e-05,0.033,0.033,0.0086,0.045,0.045,0.034,6.6e-06,6.6e-06,4.8e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18090000,0.98,-0.0072,-0.011,0.19,0.0035,-0.0098,0.028,0.0035,-0.0029,0.031,-0.0014,-0.006,0.00031,0.0084,0.0043,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00013,0.00013,6.4e-05,0.036,0.036,0.0087,0.052,0.052,0.035,6.6e-06,6.6e-06,4.7e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18190000,0.98,-0.0072,-0.011,0.19,0.0036,-0.0087,0.028,0.0041,-0.0022,0.029,-0.0014,-0.006,0.00034,0.0089,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00013,0.00013,6.4e-05,0.032,0.032,0.0086,0.045,0.045,0.035,6e-06,6e-06,4.7e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18290000,0.98,-0.0072,-0.011,0.19,0.0045,-0.0092,0.027,0.0045,-0.003,0.027,-0.0014,-0.006,0.00034,0.0092,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00013,0.00013,6.4e-05,0.036,0.036,0.0087,0.051,0.051,0.035,6e-06,6e-06,4.6e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 -18390000,0.98,-0.0072,-0.011,0.19,0.0054,-0.0079,0.027,0.0061,-0.0023,0.026,-0.0014,-0.006,0.00033,0.0098,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00013,0.00012,6.4e-05,0.031,0.031,0.0086,0.045,0.045,0.035,5.4e-06,5.4e-06,4.5e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 -18490000,0.98,-0.0072,-0.011,0.19,0.0081,-0.008,0.026,0.0069,-0.0031,0.028,-0.0014,-0.006,0.00034,0.0098,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00013,0.00013,6.4e-05,0.035,0.035,0.0087,0.051,0.051,0.035,5.4e-06,5.4e-06,4.5e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 -18590000,0.98,-0.007,-0.011,0.19,0.0065,-0.0073,0.026,0.0055,-0.0024,0.031,-0.0015,-0.006,0.00033,0.0097,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00012,0.00012,6.4e-05,0.031,0.031,0.0087,0.045,0.045,0.035,4.9e-06,4.9e-06,4.4e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -18690000,0.98,-0.007,-0.011,0.19,0.0066,-0.0063,0.024,0.0062,-0.0031,0.029,-0.0015,-0.006,0.00034,0.0099,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00012,0.00012,6.4e-05,0.034,0.034,0.0087,0.051,0.051,0.035,4.9e-06,4.9e-06,4.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -18790000,0.98,-0.007,-0.011,0.19,0.0056,-0.006,0.024,0.0062,-0.0025,0.027,-0.0015,-0.006,0.00034,0.01,0.0038,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00012,0.00012,6.4e-05,0.03,0.03,0.0087,0.045,0.045,0.035,4.4e-06,4.4e-06,4.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -18890000,0.98,-0.0069,-0.011,0.19,0.0043,-0.0056,0.021,0.0067,-0.0031,0.023,-0.0015,-0.006,0.00033,0.011,0.0036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00012,0.00012,6.4e-05,0.033,0.033,0.0087,0.051,0.051,0.035,4.4e-06,4.4e-06,4.2e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -18990000,0.98,-0.0069,-0.011,0.19,0.0026,-0.0057,0.022,0.0055,-0.0025,0.026,-0.0015,-0.006,0.00033,0.011,0.0038,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00012,0.00012,6.3e-05,0.029,0.029,0.0086,0.045,0.045,0.035,4e-06,4e-06,4.2e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19090000,0.98,-0.007,-0.011,0.19,0.0006,-0.0062,0.023,0.0057,-0.003,0.022,-0.0015,-0.006,0.00034,0.011,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00012,0.00012,6.3e-05,0.032,0.032,0.0087,0.051,0.051,0.036,4e-06,4e-06,4.1e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19190000,0.98,-0.0069,-0.011,0.19,-0.00089,-0.0058,0.022,0.0048,-0.0025,0.021,-0.0015,-0.006,0.00031,0.011,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00011,0.00011,6.3e-05,0.028,0.028,0.0086,0.045,0.045,0.036,3.7e-06,3.7e-06,4.1e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19290000,0.98,-0.0069,-0.011,0.19,-0.0018,-0.0057,0.023,0.0047,-0.0031,0.021,-0.0015,-0.006,0.00031,0.011,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00011,0.00011,6.3e-05,0.031,0.031,0.0087,0.05,0.051,0.036,3.7e-06,3.7e-06,4e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19390000,0.98,-0.0069,-0.011,0.19,-0.0022,-0.0022,0.024,0.004,-0.0011,0.019,-0.0015,-0.006,0.0003,0.011,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00011,0.00011,6.3e-05,0.028,0.028,0.0086,0.045,0.045,0.036,3.3e-06,3.3e-06,4e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19490000,0.98,-0.007,-0.011,0.19,-0.003,-0.0022,0.023,0.0038,-0.0014,0.019,-0.0015,-0.006,0.00028,0.012,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00011,0.00011,6.3e-05,0.03,0.03,0.0087,0.05,0.05,0.036,3.3e-06,3.3e-06,3.9e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19590000,0.98,-0.0069,-0.011,0.19,-0.0041,-0.0051,0.025,0.0043,-0.0024,0.019,-0.0015,-0.006,0.00027,0.012,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.6e-06,0.00011,0.00011,6.3e-05,0.027,0.027,0.0086,0.044,0.044,0.036,3e-06,3e-06,3.8e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19690000,0.98,-0.007,-0.011,0.19,-0.0058,-0.0036,0.023,0.0039,-0.0028,0.019,-0.0015,-0.006,0.00028,0.012,0.003,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.00011,0.00011,6.2e-05,0.03,0.03,0.0086,0.05,0.05,0.036,3e-06,3e-06,3.8e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 -19790000,0.98,-0.007,-0.011,0.19,-0.0058,-0.0022,0.022,0.0063,-0.0023,0.014,-0.0015,-0.006,0.00027,0.013,0.0027,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.00011,0.00011,6.2e-05,0.026,0.026,0.0086,0.044,0.044,0.036,2.8e-06,2.8e-06,3.7e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 -19890000,0.98,-0.0071,-0.011,0.19,-0.0058,-0.0019,0.022,0.0057,-0.0025,0.013,-0.0015,-0.006,0.00026,0.013,0.0026,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.00011,0.00011,6.2e-05,0.029,0.029,0.0086,0.05,0.05,0.036,2.8e-06,2.8e-06,3.7e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 -19990000,0.98,-0.0071,-0.011,0.19,-0.0055,-0.0019,0.019,0.0061,-0.00089,0.0097,-0.0015,-0.0059,0.00026,0.013,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,0.0001,6.2e-05,0.026,0.026,0.0085,0.044,0.044,0.036,2.5e-06,2.5e-06,3.6e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20090000,0.98,-0.0071,-0.011,0.19,-0.005,-0.0041,0.019,0.0055,-0.0012,0.013,-0.0015,-0.0059,0.00026,0.013,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.00011,0.0001,6.2e-05,0.028,0.028,0.0086,0.05,0.05,0.036,2.5e-06,2.5e-06,3.6e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20190000,0.98,-0.0071,-0.011,0.19,-0.004,-0.0015,0.02,0.0065,-0.00088,0.013,-0.0015,-0.0059,0.00024,0.013,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,0.0001,6.2e-05,0.025,0.025,0.0085,0.044,0.044,0.036,2.3e-06,2.3e-06,3.5e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20290000,0.98,-0.0071,-0.011,0.19,-0.0072,-0.0026,0.02,0.006,-0.001,0.013,-0.0015,-0.0059,0.00024,0.014,0.0024,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,0.0001,6.2e-05,0.027,0.027,0.0085,0.049,0.049,0.036,2.3e-06,2.3e-06,3.5e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20390000,0.98,-0.007,-0.011,0.19,-0.0079,-0.0014,0.021,0.0069,-0.00075,0.014,-0.0015,-0.0059,0.00025,0.014,0.0024,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,0.0001,6.2e-05,0.024,0.024,0.0084,0.044,0.044,0.036,2.1e-06,2.1e-06,3.5e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20490000,0.98,-0.007,-0.011,0.19,-0.012,-0.0024,0.02,0.0058,-0.00092,0.012,-0.0015,-0.0059,0.00024,0.014,0.0022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,0.0001,6.1e-05,0.027,0.027,0.0085,0.049,0.049,0.036,2.1e-06,2.1e-06,3.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20590000,0.98,-0.007,-0.011,0.19,-0.012,-0.0033,0.02,0.0069,-0.00077,0.011,-0.0015,-0.0059,0.00026,0.014,0.002,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,9.9e-05,6.1e-05,0.024,0.024,0.0084,0.044,0.044,0.036,2e-06,2e-06,3.4e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20690000,0.98,-0.0069,-0.011,0.19,-0.013,-0.0021,0.021,0.0057,-0.00099,0.011,-0.0015,-0.0059,0.00025,0.014,0.002,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,0.0001,9.9e-05,6.1e-05,0.026,0.026,0.0084,0.049,0.049,0.036,2e-06,2e-06,3.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20790000,0.98,-0.0063,-0.011,0.19,-0.016,0.00045,0.0055,0.0048,-0.00079,0.0097,-0.0015,-0.0059,0.00025,0.015,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,9.8e-05,9.7e-05,6.1e-05,0.023,0.023,0.0084,0.043,0.043,0.036,1.8e-06,1.8e-06,3.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20890000,0.98,0.0028,-0.0073,0.19,-0.022,0.012,-0.11,0.0029,-0.0001,0.0034,-0.0015,-0.0059,0.00025,0.015,0.0018,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,9.9e-05,9.8e-05,6.1e-05,0.026,0.026,0.0084,0.049,0.049,0.036,1.8e-06,1.8e-06,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -20990000,0.98,0.0061,-0.0038,0.19,-0.032,0.03,-0.25,0.0022,0.00054,-0.011,-0.0015,-0.0059,0.00025,0.015,0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,9.7e-05,9.6e-05,6.1e-05,0.023,0.023,0.0083,0.043,0.043,0.036,1.7e-06,1.7e-06,3.2e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21090000,0.98,0.0045,-0.0042,0.19,-0.046,0.046,-0.37,-0.0017,0.0044,-0.042,-0.0015,-0.0059,0.00024,0.015,0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,9.8e-05,9.6e-05,6.1e-05,0.026,0.026,0.0084,0.048,0.048,0.036,1.7e-06,1.7e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21190000,0.98,0.0016,-0.0059,0.19,-0.049,0.05,-0.5,-0.0013,0.0035,-0.078,-0.0014,-0.0059,0.00025,0.015,0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,9.5e-05,9.4e-05,6e-05,0.023,0.023,0.0083,0.043,0.043,0.036,1.6e-06,1.6e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21290000,0.98,-0.00052,-0.0072,0.19,-0.049,0.054,-0.63,-0.0062,0.0088,-0.14,-0.0014,-0.0059,0.00023,0.015,0.001,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,9.6e-05,9.5e-05,6e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.6e-06,1.6e-06,3.1e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21390000,0.98,-0.002,-0.0079,0.19,-0.045,0.05,-0.75,-0.005,0.011,-0.2,-0.0014,-0.0059,0.00023,0.015,0.00069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.5e-06,9.4e-05,9.2e-05,6e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-06,1.4e-06,3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0093,0.016,-0.29,-0.0014,-0.0059,0.00023,0.015,0.00058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,9.4e-05,9.3e-05,6e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-06,1.4e-06,3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.008,0.017,-0.38,-0.0014,-0.0059,0.00024,0.015,0.00054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,9.2e-05,9e-05,6e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-06,1.3e-06,2.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21690000,0.98,-0.0036,-0.0082,0.19,-0.029,0.039,-1.1,-0.011,0.021,-0.49,-0.0014,-0.0059,0.00025,0.015,0.00036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,9.2e-05,9.1e-05,6e-05,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-06,1.3e-06,2.9e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 -21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.033,-1.3,-0.0038,0.018,-0.61,-0.0014,-0.0058,0.00027,0.016,0.00016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,9e-05,8.9e-05,5.9e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-06,1.2e-06,2.9e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 -21890000,0.98,-0.0043,-0.0085,0.19,-0.018,0.028,-1.4,-0.0057,0.021,-0.75,-0.0014,-0.0058,0.00026,0.016,3.2e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,9e-05,8.9e-05,5.9e-05,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-06,1.2e-06,2.8e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 -21990000,0.98,-0.005,-0.0088,0.19,-0.014,0.023,-1.4,-0.00037,0.017,-0.89,-0.0014,-0.0058,0.00026,0.016,0.00027,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.8e-05,8.7e-05,5.9e-05,0.022,0.022,0.0082,0.043,0.043,0.036,1.1e-06,1.1e-06,2.8e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22090000,0.98,-0.0057,-0.0096,0.19,-0.012,0.019,-1.4,-0.0017,0.019,-1,-0.0014,-0.0058,0.00028,0.016,0.00013,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.8e-05,8.7e-05,5.9e-05,0.024,0.024,0.0082,0.048,0.048,0.036,1.1e-06,1.1e-06,2.8e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22190000,0.98,-0.0062,-0.0099,0.19,-0.0035,0.013,-1.4,0.006,0.013,-1.2,-0.0014,-0.0058,0.00029,0.016,0.00016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.6e-05,8.5e-05,5.9e-05,0.021,0.021,0.0081,0.043,0.043,0.036,1.1e-06,1.1e-06,2.7e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22290000,0.98,-0.0069,-0.01,0.19,0.0016,0.0078,-1.4,0.0059,0.014,-1.3,-0.0014,-0.0058,0.00028,0.016,6.8e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.7e-05,8.5e-05,5.8e-05,0.023,0.023,0.0081,0.048,0.048,0.036,1.1e-06,1.1e-06,2.7e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22390000,0.98,-0.0073,-0.01,0.19,0.0066,-0.0018,-1.4,0.013,0.0042,-1.5,-0.0014,-0.0058,0.00027,0.016,-0.00031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.5e-05,8.3e-05,5.8e-05,0.021,0.021,0.0081,0.043,0.043,0.036,1e-06,1e-06,2.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22490000,0.98,-0.0074,-0.011,0.19,0.01,-0.0077,-1.4,0.014,0.0036,-1.6,-0.0014,-0.0058,0.00025,0.016,-0.00038,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.5e-05,8.4e-05,5.8e-05,0.022,0.022,0.0081,0.047,0.047,0.036,1e-06,1e-06,2.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22590000,0.98,-0.0073,-0.011,0.19,0.02,-0.017,-1.4,0.026,-0.0053,-1.7,-0.0014,-0.0058,0.00027,0.016,-0.00054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.3e-05,8.2e-05,5.8e-05,0.02,0.02,0.0081,0.042,0.042,0.036,9.4e-07,9.4e-07,2.6e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22690000,0.98,-0.0073,-0.012,0.19,0.022,-0.021,-1.4,0.028,-0.0072,-1.9,-0.0014,-0.0058,0.00026,0.016,-0.00062,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.4e-05,8.2e-05,5.8e-05,0.021,0.021,0.0081,0.047,0.047,0.036,9.4e-07,9.4e-07,2.5e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22790000,0.98,-0.0073,-0.012,0.19,0.028,-0.029,-1.4,0.038,-0.017,-2,-0.0014,-0.0058,0.00025,0.017,-0.00073,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.2e-05,8.1e-05,5.8e-05,0.019,0.019,0.0081,0.042,0.042,0.036,8.9e-07,8.9e-07,2.5e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22890000,0.98,-0.0074,-0.012,0.19,0.031,-0.035,-1.4,0.041,-0.02,-2.2,-0.0014,-0.0058,0.00026,0.017,-0.00081,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.4e-06,8.2e-05,8.1e-05,5.7e-05,0.021,0.021,0.0081,0.047,0.047,0.036,8.9e-07,8.9e-07,2.5e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -22990000,0.98,-0.0074,-0.013,0.19,0.036,-0.04,-1.4,0.051,-0.031,-2.3,-0.0014,-0.0058,0.00027,0.017,-0.00095,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,8.1e-05,8e-05,5.7e-05,0.019,0.019,0.0081,0.042,0.042,0.036,8.4e-07,8.4e-07,2.5e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23090000,0.98,-0.0074,-0.013,0.19,0.041,-0.044,-1.4,0.055,-0.035,-2.5,-0.0014,-0.0058,0.00027,0.017,-0.00098,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,8.1e-05,8e-05,5.7e-05,0.02,0.02,0.0081,0.046,0.046,0.036,8.4e-07,8.4e-07,2.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23190000,0.98,-0.0074,-0.013,0.19,0.047,-0.046,-1.4,0.066,-0.045,-2.6,-0.0014,-0.0058,0.00027,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,8e-05,7.9e-05,5.7e-05,0.018,0.018,0.008,0.042,0.042,0.035,7.9e-07,7.9e-07,2.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23290000,0.98,-0.0079,-0.013,0.19,0.052,-0.051,-1.4,0.071,-0.05,-2.8,-0.0014,-0.0058,0.00027,0.017,-0.0012,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,8e-05,7.9e-05,5.7e-05,0.02,0.02,0.0081,0.046,0.046,0.036,8e-07,8e-07,2.4e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23390000,0.98,-0.0078,-0.014,0.19,0.057,-0.053,-1.4,0.082,-0.055,-2.9,-0.0014,-0.0058,0.00025,0.017,-0.00092,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.9e-05,7.8e-05,5.7e-05,0.018,0.018,0.008,0.041,0.041,0.036,7.5e-07,7.5e-07,2.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23490000,0.98,-0.0079,-0.014,0.19,0.061,-0.055,-1.4,0.088,-0.061,-3,-0.0014,-0.0058,0.00026,0.017,-0.00097,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,8e-05,7.8e-05,5.6e-05,0.019,0.019,0.0081,0.046,0.046,0.036,7.6e-07,7.6e-07,2.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23590000,0.98,-0.0082,-0.014,0.18,0.064,-0.058,-1.4,0.095,-0.07,-3.2,-0.0014,-0.0059,0.00027,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.9e-05,7.7e-05,5.6e-05,0.017,0.017,0.008,0.041,0.041,0.035,7.2e-07,7.2e-07,2.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23690000,0.98,-0.0088,-0.014,0.18,0.062,-0.061,-1.3,0.1,-0.076,-3.3,-0.0014,-0.0059,0.00028,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.9e-05,7.8e-05,5.6e-05,0.018,0.018,0.0081,0.046,0.046,0.036,7.2e-07,7.2e-07,2.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23790000,0.98,-0.011,-0.017,0.18,0.057,-0.058,-0.96,0.11,-0.081,-3.4,-0.0014,-0.0058,0.00029,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.8e-05,7.7e-05,5.6e-05,0.016,0.016,0.008,0.041,0.041,0.035,6.9e-07,6.9e-07,2.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23890000,0.98,-0.014,-0.021,0.18,0.052,-0.059,-0.52,0.12,-0.087,-3.5,-0.0014,-0.0058,0.00029,0.018,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.8e-05,7.7e-05,5.6e-05,0.017,0.017,0.008,0.045,0.045,0.035,6.9e-07,6.9e-07,2.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -23990000,0.98,-0.016,-0.024,0.18,0.054,-0.058,-0.14,0.12,-0.089,-3.6,-0.0014,-0.0058,0.00029,0.018,-0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.8e-05,7.7e-05,5.6e-05,0.015,0.015,0.008,0.041,0.041,0.036,6.6e-07,6.6e-07,2.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24090000,0.98,-0.016,-0.023,0.18,0.06,-0.066,0.09,0.13,-0.095,-3.6,-0.0014,-0.0058,0.00028,0.018,-0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.8e-05,7.7e-05,5.5e-05,0.016,0.016,0.0081,0.045,0.045,0.036,6.6e-07,6.6e-07,2.1e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24190000,0.98,-0.013,-0.019,0.18,0.071,-0.071,0.077,0.14,-0.1,-3.6,-0.0014,-0.0058,0.00029,0.019,-0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.3e-06,7.8e-05,7.7e-05,5.5e-05,0.015,0.015,0.008,0.04,0.04,0.035,6.3e-07,6.3e-07,2.1e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24290000,0.98,-0.01,-0.016,0.18,0.075,-0.075,0.055,0.15,-0.11,-3.6,-0.0014,-0.0058,0.00029,0.019,-0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.5e-05,0.016,0.016,0.0081,0.044,0.044,0.036,6.3e-07,6.3e-07,2.1e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24390000,0.98,-0.0096,-0.015,0.18,0.069,-0.069,0.071,0.15,-0.11,-3.6,-0.0014,-0.0058,0.0003,0.019,-0.0033,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.5e-05,0.015,0.015,0.008,0.04,0.04,0.035,6.1e-07,6.1e-07,2.1e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24490000,0.98,-0.0098,-0.015,0.18,0.064,-0.066,0.069,0.16,-0.11,-3.6,-0.0014,-0.0058,0.00032,0.019,-0.0033,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.5e-05,0.016,0.016,0.008,0.044,0.044,0.035,6.1e-07,6.1e-07,2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24590000,0.98,-0.01,-0.015,0.18,0.061,-0.062,0.065,0.16,-0.11,-3.6,-0.0014,-0.0058,0.00032,0.019,-0.004,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.5e-05,0.015,0.015,0.008,0.04,0.04,0.036,5.9e-07,5.9e-07,2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24690000,0.98,-0.011,-0.015,0.18,0.059,-0.061,0.064,0.17,-0.12,-3.6,-0.0014,-0.0058,0.00032,0.019,-0.004,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.4e-05,0.016,0.016,0.0081,0.044,0.044,0.036,5.9e-07,5.9e-07,2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24790000,0.98,-0.011,-0.014,0.18,0.056,-0.059,0.056,0.17,-0.11,-3.6,-0.0015,-0.0058,0.00032,0.019,-0.0046,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.4e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.7e-07,5.7e-07,2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24890000,0.98,-0.011,-0.013,0.18,0.054,-0.062,0.045,0.18,-0.12,-3.6,-0.0015,-0.0058,0.00033,0.019,-0.0046,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.4e-05,0.016,0.016,0.008,0.043,0.043,0.035,5.7e-07,5.7e-07,1.9e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -24990000,0.98,-0.011,-0.013,0.18,0.045,-0.058,0.038,0.18,-0.11,-3.6,-0.0015,-0.0058,0.00033,0.019,-0.0053,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.4e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.5e-07,5.5e-07,1.9e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25090000,0.98,-0.011,-0.013,0.18,0.042,-0.057,0.035,0.18,-0.12,-3.6,-0.0015,-0.0058,0.00033,0.019,-0.0053,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.9e-05,7.7e-05,5.4e-05,0.016,0.016,0.0081,0.043,0.043,0.035,5.5e-07,5.5e-07,1.9e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 -25190000,0.98,-0.011,-0.013,0.18,0.037,-0.05,0.035,0.18,-0.11,-3.6,-0.0015,-0.0058,0.00032,0.019,-0.0058,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.8e-05,7.7e-05,5.3e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.3e-07,5.3e-07,1.9e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25290000,0.98,-0.011,-0.012,0.18,0.032,-0.052,0.029,0.18,-0.11,-3.6,-0.0015,-0.0058,0.0003,0.019,-0.0059,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.9e-05,7.7e-05,5.3e-05,0.016,0.016,0.0081,0.043,0.043,0.036,5.3e-07,5.3e-07,1.8e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25390000,0.98,-0.011,-0.012,0.18,0.024,-0.044,0.028,0.18,-0.1,-3.6,-0.0015,-0.0058,0.0003,0.019,-0.0066,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.9e-05,7.7e-05,5.3e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.1e-07,5.1e-07,1.8e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25490000,0.98,-0.011,-0.012,0.18,0.019,-0.044,0.027,0.18,-0.11,-3.6,-0.0015,-0.0058,0.0003,0.019,-0.0066,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.9e-05,7.8e-05,5.3e-05,0.016,0.016,0.008,0.043,0.043,0.035,5.1e-07,5.1e-07,1.8e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25590000,0.98,-0.012,-0.012,0.18,0.014,-0.04,0.028,0.18,-0.098,-3.6,-0.0015,-0.0058,0.00029,0.019,-0.0069,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.9e-05,7.8e-05,5.3e-05,0.014,0.014,0.008,0.039,0.039,0.035,5e-07,5e-07,1.8e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25690000,0.98,-0.011,-0.011,0.18,0.013,-0.039,0.017,0.18,-0.1,-3.6,-0.0015,-0.0058,0.00029,0.019,-0.0069,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.2e-06,7.9e-05,7.8e-05,5.3e-05,0.015,0.015,0.0081,0.043,0.043,0.035,5e-07,5e-07,1.8e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25790000,0.98,-0.011,-0.011,0.18,0.002,-0.031,0.017,0.17,-0.093,-3.6,-0.0016,-0.0058,0.00028,0.019,-0.0074,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,7.9e-05,7.8e-05,5.3e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.8e-07,4.8e-07,1.7e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25890000,0.98,-0.011,-0.011,0.19,-0.0037,-0.028,0.019,0.17,-0.096,-3.6,-0.0016,-0.0058,0.00026,0.019,-0.0074,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,7.9e-05,7.8e-05,5.2e-05,0.015,0.015,0.0081,0.043,0.043,0.036,4.8e-07,4.8e-07,1.7e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -25990000,0.98,-0.011,-0.011,0.19,-0.013,-0.022,0.013,0.16,-0.086,-3.6,-0.0016,-0.0058,0.00026,0.019,-0.0079,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,7.9e-05,7.8e-05,5.2e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.7e-07,4.7e-07,1.7e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26090000,0.98,-0.01,-0.011,0.18,-0.018,-0.021,0.011,0.16,-0.088,-3.6,-0.0016,-0.0058,0.00026,0.019,-0.0079,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.8e-05,5.2e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.7e-07,4.7e-07,1.7e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26190000,0.98,-0.01,-0.011,0.18,-0.024,-0.015,0.0063,0.15,-0.081,-3.6,-0.0016,-0.0058,0.00027,0.019,-0.0081,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,7.9e-05,7.8e-05,5.2e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.6e-07,4.6e-07,1.7e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26290000,0.98,-0.01,-0.012,0.18,-0.026,-0.013,0.00051,0.15,-0.083,-3.6,-0.0016,-0.0058,0.00026,0.019,-0.0081,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.8e-05,5.2e-05,0.015,0.015,0.0081,0.042,0.042,0.036,4.6e-07,4.6e-07,1.6e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26390000,0.98,-0.0098,-0.012,0.18,-0.032,-0.0064,0.0045,0.13,-0.074,-3.6,-0.0016,-0.0058,0.00024,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.8e-05,5.2e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.4e-07,4.4e-07,1.6e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26490000,0.98,-0.0096,-0.011,0.18,-0.035,-0.0032,0.014,0.13,-0.075,-3.6,-0.0016,-0.0058,0.00024,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5.1e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.5e-07,4.5e-07,1.6e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26590000,0.98,-0.009,-0.012,0.18,-0.036,0.0047,0.014,0.12,-0.068,-3.6,-0.0016,-0.0058,0.00023,0.019,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5.1e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.3e-07,4.3e-07,1.6e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26690000,0.98,-0.0089,-0.011,0.18,-0.038,0.0098,0.013,0.12,-0.067,-3.6,-0.0016,-0.0058,0.00021,0.019,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5.1e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.3e-07,4.3e-07,1.6e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26790000,0.98,-0.0087,-0.011,0.18,-0.045,0.013,0.012,0.1,-0.062,-3.6,-0.0016,-0.0058,0.0002,0.02,-0.0088,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5.1e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.2e-07,4.2e-07,1.6e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26890000,0.98,-0.008,-0.011,0.18,-0.051,0.017,0.007,0.1,-0.06,-3.6,-0.0016,-0.0058,0.0002,0.02,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5.1e-05,0.015,0.015,0.0081,0.042,0.042,0.036,4.2e-07,4.2e-07,1.5e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -26990000,0.98,-0.0075,-0.011,0.18,-0.058,0.023,0.0061,0.088,-0.054,-3.6,-0.0016,-0.0058,0.0002,0.02,-0.0091,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5.1e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.1e-07,4.1e-07,1.5e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27090000,0.98,-0.0074,-0.012,0.18,-0.06,0.03,0.0089,0.082,-0.051,-3.6,-0.0016,-0.0058,0.00019,0.02,-0.0092,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.1e-07,4.1e-07,1.5e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27190000,0.98,-0.0074,-0.012,0.18,-0.066,0.036,0.011,0.071,-0.046,-3.6,-0.0016,-0.0058,0.00018,0.02,-0.0093,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3.1e-06,8e-05,7.9e-05,5e-05,0.014,0.014,0.008,0.038,0.038,0.035,4e-07,4e-07,1.5e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27290000,0.98,-0.0076,-0.013,0.18,-0.073,0.042,0.12,0.064,-0.042,-3.6,-0.0016,-0.0058,0.00018,0.02,-0.0094,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8e-05,7.9e-05,5e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4e-07,4e-07,1.5e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27390000,0.98,-0.009,-0.015,0.18,-0.078,0.047,0.45,0.055,-0.035,-3.5,-0.0016,-0.0058,0.00017,0.02,-0.0096,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8e-05,7.9e-05,5e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-07,3.9e-07,1.5e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27490000,0.98,-0.01,-0.017,0.18,-0.082,0.053,0.76,0.047,-0.029,-3.5,-0.0016,-0.0058,0.00016,0.02,-0.0097,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,7.9e-05,5e-05,0.014,0.014,0.008,0.042,0.042,0.035,3.9e-07,3.9e-07,1.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27590000,0.98,-0.01,-0.016,0.18,-0.075,0.055,0.85,0.038,-0.025,-3.4,-0.0016,-0.0058,0.00016,0.02,-0.0098,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,7.9e-05,5e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-07,3.9e-07,1.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27690000,0.98,-0.009,-0.013,0.18,-0.072,0.052,0.76,0.031,-0.02,-3.3,-0.0016,-0.0058,0.00015,0.02,-0.0099,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,7.9e-05,5e-05,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-07,3.9e-07,1.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 -27790000,0.98,-0.0076,-0.011,0.18,-0.071,0.05,0.75,0.025,-0.018,-3.3,-0.0016,-0.0058,0.00015,0.02,-0.0093,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,8e-05,4.9e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.8e-07,3.8e-07,1.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -27890000,0.98,-0.0073,-0.012,0.18,-0.078,0.057,0.79,0.017,-0.012,-3.2,-0.0016,-0.0058,0.00015,0.02,-0.0094,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,8e-05,4.9e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.8e-07,3.8e-07,1.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -27990000,0.98,-0.0077,-0.012,0.18,-0.078,0.058,0.78,0.012,-0.011,-3.1,-0.0016,-0.0058,0.00014,0.02,-0.009,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,8e-05,4.9e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-07,3.7e-07,1.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28090000,0.98,-0.008,-0.012,0.18,-0.082,0.059,0.78,0.0042,-0.0047,-3,-0.0016,-0.0058,0.00015,0.02,-0.0091,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,8e-05,4.9e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.7e-07,3.7e-07,1.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28190000,0.98,-0.0074,-0.012,0.18,-0.082,0.055,0.79,-0.0023,-0.0043,-3,-0.0016,-0.0058,0.00015,0.02,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.1e-05,8e-05,4.9e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-07,3.6e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28290000,0.98,-0.0069,-0.012,0.18,-0.087,0.059,0.79,-0.011,0.0015,-2.9,-0.0016,-0.0058,0.00015,0.02,-0.0088,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8e-05,4.9e-05,0.014,0.014,0.0081,0.041,0.041,0.035,3.7e-07,3.7e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28390000,0.98,-0.0069,-0.013,0.18,-0.088,0.062,0.79,-0.015,0.0044,-2.8,-0.0015,-0.0058,0.00016,0.02,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8e-05,4.8e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-07,3.6e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28490000,0.98,-0.0072,-0.014,0.18,-0.09,0.066,0.79,-0.024,0.011,-2.8,-0.0015,-0.0058,0.00015,0.02,-0.0088,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8e-05,4.8e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.6e-07,3.6e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28590000,0.98,-0.0073,-0.014,0.18,-0.083,0.061,0.79,-0.027,0.0085,-2.7,-0.0015,-0.0058,0.00015,0.02,-0.0085,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8e-05,4.8e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.5e-07,3.5e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28690000,0.98,-0.007,-0.013,0.18,-0.083,0.062,0.79,-0.036,0.015,-2.6,-0.0015,-0.0058,0.00015,0.02,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8.1e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.5e-07,3.5e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28790000,0.98,-0.0064,-0.013,0.18,-0.079,0.062,0.79,-0.038,0.016,-2.5,-0.0015,-0.0058,0.00015,0.02,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.5e-07,3.5e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28890000,0.98,-0.0062,-0.012,0.18,-0.084,0.064,0.78,-0.046,0.023,-2.5,-0.0015,-0.0058,0.00016,0.02,-0.0088,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8.1e-05,4.8e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.5e-07,3.5e-07,1.3e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -28990000,0.98,-0.006,-0.013,0.18,-0.079,0.06,0.78,-0.046,0.022,-2.4,-0.0015,-0.0058,0.00015,0.02,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,3e-06,8.2e-05,8.1e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-07,3.4e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29090000,0.98,-0.0058,-0.013,0.18,-0.082,0.063,0.78,-0.054,0.028,-2.3,-0.0015,-0.0058,0.00015,0.02,-0.0089,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.7e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-07,3.4e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29190000,0.98,-0.0058,-0.013,0.18,-0.078,0.061,0.78,-0.051,0.027,-2.2,-0.0015,-0.0058,0.00016,0.02,-0.0091,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-07,3.4e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29290000,0.98,-0.0061,-0.013,0.18,-0.08,0.067,0.78,-0.059,0.034,-2.2,-0.0015,-0.0058,0.00016,0.02,-0.0092,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.7e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-07,3.4e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29390000,0.98,-0.0065,-0.012,0.18,-0.075,0.065,0.78,-0.057,0.034,-2.1,-0.0015,-0.0058,0.00016,0.02,-0.0093,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-07,3.3e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29490000,0.98,-0.0065,-0.012,0.18,-0.078,0.066,0.78,-0.065,0.041,-2,-0.0015,-0.0058,0.00017,0.02,-0.0094,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.7e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.3e-07,3.3e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29590000,0.98,-0.0064,-0.012,0.18,-0.074,0.064,0.78,-0.062,0.04,-1.9,-0.0015,-0.0058,0.00018,0.02,-0.0096,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-07,3.3e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29690000,0.98,-0.0065,-0.012,0.18,-0.078,0.063,0.78,-0.07,0.046,-1.9,-0.0015,-0.0058,0.00019,0.02,-0.0097,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.3e-05,8.1e-05,4.7e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-07,3.3e-07,1.2e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29790000,0.98,-0.0063,-0.013,0.18,-0.074,0.056,0.78,-0.065,0.044,-1.8,-0.0014,-0.0058,0.00019,0.021,-0.0099,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.6e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.2e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29890000,0.98,-0.0058,-0.013,0.18,-0.074,0.057,0.77,-0.072,0.049,-1.7,-0.0014,-0.0058,0.0002,0.021,-0.01,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.3e-05,8.1e-05,4.6e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-07,3.2e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -29990000,0.98,-0.006,-0.013,0.18,-0.069,0.052,0.77,-0.068,0.044,-1.6,-0.0014,-0.0058,0.0002,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.6e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.2e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30090000,0.98,-0.0061,-0.013,0.18,-0.069,0.053,0.77,-0.075,0.05,-1.6,-0.0014,-0.0058,0.00019,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.3e-05,8.1e-05,4.6e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-07,3.2e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30190000,0.98,-0.0061,-0.013,0.18,-0.063,0.05,0.77,-0.068,0.048,-1.5,-0.0014,-0.0057,0.00018,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.6e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.1e-07,3.1e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30290000,0.98,-0.0061,-0.013,0.19,-0.062,0.05,0.77,-0.074,0.053,-1.4,-0.0014,-0.0057,0.00018,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.3e-05,8.1e-05,4.6e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.1e-07,3.1e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30390000,0.98,-0.0061,-0.013,0.19,-0.055,0.044,0.76,-0.066,0.049,-1.4,-0.0014,-0.0057,0.00019,0.022,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.6e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-07,3.1e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30490000,0.98,-0.0061,-0.013,0.18,-0.057,0.044,0.77,-0.072,0.054,-1.3,-0.0014,-0.0057,0.0002,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.3e-05,8.1e-05,4.5e-05,0.014,0.014,0.008,0.041,0.041,0.036,3.1e-07,3.1e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30590000,0.98,-0.0064,-0.014,0.18,-0.053,0.041,0.76,-0.065,0.05,-1.2,-0.0014,-0.0057,0.0002,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-07,3e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30690000,0.98,-0.0068,-0.014,0.18,-0.051,0.04,0.76,-0.07,0.054,-1.1,-0.0014,-0.0057,0.0002,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-07,3e-07,1.1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30790000,0.98,-0.0065,-0.013,0.18,-0.044,0.035,0.76,-0.063,0.052,-1.1,-0.0014,-0.0057,0.0002,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-07,3e-07,1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30890000,0.98,-0.0058,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-1,-0.0014,-0.0057,0.0002,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.008,0.04,0.04,0.035,3e-07,3e-07,1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -30990000,0.98,-0.006,-0.013,0.18,-0.037,0.026,0.76,-0.057,0.049,-0.94,-0.0014,-0.0057,0.0002,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3e-07,3e-07,1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31090000,0.98,-0.0062,-0.013,0.19,-0.035,0.025,0.76,-0.061,0.051,-0.86,-0.0014,-0.0057,0.00019,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.008,0.04,0.04,0.036,3e-07,3e-07,1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31190000,0.98,-0.0064,-0.013,0.18,-0.031,0.021,0.76,-0.052,0.046,-0.79,-0.0014,-0.0057,0.0002,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.5e-05,0.013,0.013,0.008,0.037,0.037,0.035,2.9e-07,2.9e-07,1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31290000,0.98,-0.0066,-0.014,0.18,-0.028,0.018,0.76,-0.055,0.048,-0.72,-0.0014,-0.0057,0.00021,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.9e-06,8.2e-05,8.1e-05,4.4e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,1e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31390000,0.98,-0.0064,-0.013,0.18,-0.022,0.012,0.76,-0.046,0.042,-0.65,-0.0014,-0.0057,0.0002,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.2e-05,8e-05,4.4e-05,0.013,0.013,0.0079,0.037,0.037,0.035,2.9e-07,2.9e-07,9.9e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31490000,0.98,-0.0061,-0.014,0.18,-0.022,0.009,0.76,-0.049,0.043,-0.58,-0.0014,-0.0057,0.0002,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.2e-05,8.1e-05,4.4e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,9.8e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31590000,0.98,-0.006,-0.014,0.18,-0.018,0.0067,0.76,-0.038,0.039,-0.51,-0.0014,-0.0057,0.00021,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.2e-05,8e-05,4.4e-05,0.012,0.012,0.0079,0.037,0.037,0.035,2.9e-07,2.9e-07,9.7e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31690000,0.98,-0.006,-0.015,0.18,-0.02,0.0057,0.76,-0.04,0.039,-0.44,-0.0014,-0.0057,0.00022,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.2e-05,8e-05,4.4e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,9.6e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31790000,0.98,-0.0062,-0.015,0.18,-0.011,0.0031,0.76,-0.028,0.037,-0.36,-0.0014,-0.0057,0.00022,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,8e-05,4.4e-05,0.012,0.012,0.008,0.037,0.037,0.035,2.8e-07,2.8e-07,9.6e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31890000,0.98,-0.0059,-0.015,0.18,-0.008,0.0008,0.76,-0.029,0.038,-0.3,-0.0014,-0.0057,0.00023,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.2e-05,8e-05,4.4e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,9.5e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -31990000,0.98,-0.0062,-0.015,0.18,-9.1e-05,0.00014,0.75,-0.017,0.034,-0.23,-0.0013,-0.0057,0.00022,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,8e-05,4.4e-05,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,9.4e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32090000,0.98,-0.0065,-0.014,0.18,-0.00063,-0.0033,0.76,-0.017,0.034,-0.16,-0.0013,-0.0057,0.00022,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,8e-05,4.3e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,9.3e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32190000,0.98,-0.0067,-0.015,0.18,0.0047,-0.0065,0.76,-0.006,0.033,-0.092,-0.0013,-0.0057,0.00022,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,8e-05,4.3e-05,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,9.2e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32290000,0.98,-0.0066,-0.015,0.18,0.0063,-0.0092,0.75,-0.0055,0.032,-0.024,-0.0013,-0.0057,0.00022,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,8e-05,4.3e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,9.2e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32390000,0.98,-0.0067,-0.015,0.18,0.013,-0.01,0.75,0.0057,0.03,0.051,-0.0013,-0.0057,0.00022,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,7.9e-05,4.3e-05,0.012,0.012,0.008,0.037,0.037,0.035,2.7e-07,2.7e-07,9.1e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32490000,0.98,-0.0096,-0.013,0.18,0.039,-0.074,-0.12,0.0089,0.023,0.054,-0.0013,-0.0057,0.00022,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8.1e-05,8e-05,4.3e-05,0.015,0.015,0.0078,0.04,0.04,0.035,2.8e-07,2.8e-07,9e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32590000,0.98,-0.0095,-0.013,0.18,0.04,-0.075,-0.12,0.021,0.02,0.035,-0.0014,-0.0057,0.00022,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8e-05,7.8e-05,4.3e-05,0.016,0.016,0.0075,0.037,0.037,0.035,2.7e-07,2.7e-07,8.9e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32690000,0.98,-0.0095,-0.013,0.18,0.035,-0.08,-0.12,0.025,0.012,0.02,-0.0014,-0.0057,0.00022,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,8e-05,7.9e-05,4.3e-05,0.019,0.019,0.0074,0.041,0.041,0.035,2.7e-07,2.7e-07,8.9e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32790000,0.98,-0.0092,-0.013,0.18,0.034,-0.078,-0.12,0.034,0.01,0.0044,-0.0014,-0.0057,0.00023,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,7.7e-05,7.6e-05,4.3e-05,0.019,0.02,0.0071,0.037,0.037,0.035,2.7e-07,2.7e-07,8.8e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32890000,0.98,-0.0092,-0.013,0.18,0.033,-0.084,-0.13,0.038,0.0021,-0.011,-0.0014,-0.0057,0.00023,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,7.7e-05,7.6e-05,4.2e-05,0.023,0.023,0.007,0.041,0.041,0.035,2.7e-07,2.7e-07,8.7e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -32990000,0.98,-0.0089,-0.013,0.18,0.03,-0.08,-0.13,0.045,-0.0011,-0.024,-0.0014,-0.0056,0.00024,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,7.3e-05,7.2e-05,4.2e-05,0.024,0.024,0.0067,0.038,0.038,0.035,2.7e-07,2.7e-07,8.7e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33090000,0.98,-0.0089,-0.013,0.18,0.026,-0.083,-0.12,0.048,-0.0092,-0.031,-0.0014,-0.0056,0.00024,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,7.3e-05,7.2e-05,4.2e-05,0.028,0.029,0.0066,0.042,0.042,0.035,2.7e-07,2.7e-07,8.6e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33190000,0.98,-0.0085,-0.013,0.18,0.022,-0.079,-0.12,0.054,-0.011,-0.037,-0.0014,-0.0056,0.00023,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,6.7e-05,6.6e-05,4.2e-05,0.029,0.029,0.0064,0.038,0.038,0.035,2.6e-07,2.6e-07,8.5e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33290000,0.98,-0.0086,-0.013,0.18,0.019,-0.08,-0.12,0.056,-0.019,-0.045,-0.0014,-0.0056,0.00024,0.026,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,6.7e-05,6.6e-05,4.2e-05,0.035,0.035,0.0063,0.043,0.043,0.034,2.6e-07,2.6e-07,8.5e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33390000,0.98,-0.0081,-0.013,0.18,0.014,-0.066,-0.12,0.059,-0.014,-0.053,-0.0014,-0.0056,0.00024,0.025,-0.017,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,6.1e-05,6e-05,4.2e-05,0.035,0.035,0.0062,0.039,0.039,0.034,2.6e-07,2.6e-07,8.4e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33490000,0.98,-0.0081,-0.013,0.18,0.0094,-0.067,-0.12,0.06,-0.021,-0.063,-0.0014,-0.0056,0.00024,0.025,-0.017,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.8e-06,6.1e-05,6e-05,4.2e-05,0.042,0.042,0.0061,0.044,0.044,0.034,2.6e-07,2.6e-07,8.3e-07,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 -33590000,0.98,-0.0077,-0.013,0.18,0.0055,-0.058,-0.11,0.063,-0.017,-0.069,-0.0014,-0.0056,0.00025,0.025,-0.019,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,5.3e-05,5.3e-05,4.2e-05,0.04,0.041,0.006,0.04,0.04,0.034,2.6e-07,2.6e-07,8.3e-07,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 -33690000,0.98,-0.0077,-0.013,0.18,0.00079,-0.058,-0.11,0.063,-0.023,-0.077,-0.0014,-0.0056,0.00025,0.025,-0.019,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,5.4e-05,5.3e-05,4.2e-05,0.048,0.048,0.0059,0.046,0.046,0.034,2.6e-07,2.6e-07,8.2e-07,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 -33790000,0.98,-0.0075,-0.013,0.18,-0.0024,-0.047,-0.11,0.067,-0.018,-0.083,-0.0014,-0.0056,0.00024,0.024,-0.022,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,4.7e-05,4.6e-05,4.1e-05,0.045,0.045,0.0059,0.041,0.041,0.034,2.6e-07,2.6e-07,8.1e-07,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 -33890000,0.98,-0.0075,-0.013,0.18,-0.0066,-0.045,-0.11,0.066,-0.023,-0.089,-0.0014,-0.0056,0.00025,0.023,-0.022,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,4.7e-05,4.6e-05,4.1e-05,0.052,0.053,0.0058,0.047,0.047,0.033,2.6e-07,2.6e-07,8.1e-07,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 -33990000,0.98,-0.0072,-0.013,0.18,-0.0062,-0.031,-0.1,0.069,-0.015,-0.092,-0.0014,-0.0056,0.00024,0.021,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,4.1e-05,4.1e-05,4.1e-05,0.047,0.047,0.0058,0.042,0.042,0.033,2.6e-07,2.6e-07,8e-07,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 -34090000,0.98,-0.0072,-0.013,0.18,-0.011,-0.031,-0.1,0.069,-0.018,-0.096,-0.0014,-0.0056,0.00024,0.021,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,4.1e-05,4.1e-05,4.1e-05,0.054,0.055,0.0058,0.049,0.049,0.033,2.6e-07,2.6e-07,8e-07,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 -34190000,0.98,-0.0071,-0.013,0.18,-0.011,-0.02,-0.098,0.072,-0.013,-0.099,-0.0014,-0.0056,0.00024,0.019,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3.7e-05,3.6e-05,4.1e-05,0.048,0.048,0.0058,0.043,0.043,0.033,2.6e-07,2.6e-07,7.9e-07,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 -34290000,0.98,-0.0069,-0.013,0.18,-0.012,-0.02,-0.097,0.071,-0.015,-0.1,-0.0014,-0.0056,0.00024,0.019,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3.7e-05,3.7e-05,4.1e-05,0.055,0.055,0.0058,0.05,0.05,0.033,2.6e-07,2.6e-07,7.8e-07,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 -34390000,0.98,-0.0068,-0.013,0.18,-0.013,-0.01,-0.092,0.073,-0.01,-0.11,-0.0014,-0.0056,0.00024,0.018,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3.3e-05,3.3e-05,4.1e-05,0.047,0.047,0.0058,0.044,0.044,0.033,2.6e-07,2.6e-07,7.8e-07,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 -34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0091,-0.089,0.071,-0.011,-0.11,-0.0014,-0.0056,0.00024,0.018,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3.4e-05,3.3e-05,4.1e-05,0.053,0.054,0.0059,0.051,0.051,0.032,2.6e-07,2.6e-07,7.7e-07,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 -34590000,0.98,-0.0069,-0.013,0.18,-0.014,-0.0048,0.71,0.073,-0.0087,-0.081,-0.0014,-0.0056,0.00024,0.016,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3.1e-05,3.1e-05,4.1e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-07,2.6e-07,7.7e-07,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 -34690000,0.98,-0.0069,-0.012,0.18,-0.017,-0.0027,1.7,0.071,-0.0091,0.038,-0.0014,-0.0056,0.00023,0.016,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3.1e-05,3.1e-05,4.1e-05,0.047,0.048,0.006,0.052,0.052,0.032,2.6e-07,2.6e-07,7.6e-07,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 -34790000,0.98,-0.0068,-0.012,0.18,-0.019,0.0019,2.7,0.072,-0.0068,0.21,-0.0014,-0.0056,0.00023,0.018,-0.03,-0.1,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3e-05,2.9e-05,4e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-07,2.6e-07,7.6e-07,0.021,0.021,0.0005,0,0,0,0,0,0,0,0 -34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0044,3.6,0.07,-0.0063,0.5,-0.0014,-0.0056,0.00023,0.018,-0.03,-0.1,0.37,0.0037,0.025,0,0,0,0,0,2.7e-06,3e-05,3e-05,4e-05,0.044,0.044,0.0061,0.052,0.052,0.032,2.6e-07,2.6e-07,7.5e-07,0.021,0.021,0.0005,0,0,0,0,0,0,0,0 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.7e-07,0.0025,0.0025,4.3e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6.2e-07,0.0026,0.0026,0.00011,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-1.5e-11,0,0,-6.8e-08,0,0,0,0,0,0,0,0,6.7e-07,0.0027,0.0027,0.00023,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +290000,1,-0.012,-0.011,0.00045,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.9e-09,-5.9e-09,-4.7e-10,0,0,-2.9e-06,0,0,0,0,0,0,0,0,7.4e-07,0.0029,0.0029,0.00019,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +390000,1,-0.012,-0.011,0.0005,-0.0025,-0.006,-0.063,-0.00056,-0.00072,-0.013,-7.2e-09,-5.8e-09,-1.3e-09,0,0,8.8e-06,0,0,0,0,0,0,0,0,8.2e-07,0.0031,0.0031,0.0003,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +490000,1,-0.012,-0.012,0.00056,-0.0007,-0.0063,-0.069,-0.00015,-0.00047,-0.011,-1.2e-06,7.5e-07,-1.7e-07,0,0,1.6e-05,0,0,0,0,0,0,0,0,9.2e-07,0.0033,0.0034,0.00018,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +590000,1,-0.012,-0.012,0.00058,-0.002,-0.009,-0.12,-0.00028,-0.0013,-0.029,-1.3e-06,7.8e-07,-1.8e-07,0,0,6.4e-05,0,0,0,0,0,0,0,0,1e-06,0.0037,0.0037,0.00025,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +690000,1,-0.012,-0.012,0.00062,5.3e-05,-0.0089,-0.05,-8e-05,-0.00079,-0.0088,-5.6e-06,1.6e-06,-5.2e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.2e-06,0.004,0.004,0.00015,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +790000,1,-0.012,-0.012,0.00062,0.0022,-0.011,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.7e-06,-5.1e-07,0,0,-0.00013,0,0,0,0,0,0,0,0,1.3e-06,0.0044,0.0044,0.0002,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.0012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +890000,1,-0.012,-0.013,0.00063,0.0031,-0.0085,-0.093,0.00015,-0.0011,-0.031,-2.2e-05,1.2e-06,-7.5e-07,0,0,-3.3e-05,0,0,0,0,0,0,0,0,1.5e-06,0.0048,0.0048,0.00013,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +990000,1,-0.012,-0.013,0.0006,0.006,-0.0099,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1.2e-06,-7.5e-07,0,0,1.3e-05,0,0,0,0,0,0,0,0,1.7e-06,0.0053,0.0053,0.00016,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1090000,1,-0.012,-0.013,0.00056,0.011,-0.013,-0.13,0.00077,-0.0015,-0.062,-6e-05,-1.5e-05,-1.8e-07,0,0,4.5e-05,0,0,0,0,0,0,0,0,1.8e-06,0.0056,0.0057,0.00011,0.92,0.92,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1190000,1,-0.012,-0.013,0.0005,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,-2.2e-07,0,0,-0.00048,0,0,0,0,0,0,0,0,2.1e-06,0.0062,0.0063,0.00013,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1290000,1,-0.012,-0.014,0.00045,0.019,-0.018,-0.11,0.002,-0.0024,-0.048,-0.00017,-9.5e-05,3e-06,0,0,-0.00075,0,0,0,0,0,0,0,0,2.1e-06,0.0064,0.0064,9.8e-05,0.87,0.87,0.41,0.15,0.15,0.18,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1390000,1,-0.012,-0.014,0.00042,0.026,-0.023,-0.097,0.0043,-0.0045,-0.038,-0.00016,-9e-05,2.8e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,2.4e-06,0.007,0.007,0.00011,1.1,1.1,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1490000,1,-0.012,-0.014,0.0004,0.024,-0.021,-0.12,0.0034,-0.0033,-0.053,-0.00039,-0.00033,1.1e-05,0,0,-0.0012,0,0,0,0,0,0,0,0,2.3e-06,0.0067,0.0067,8.7e-05,0.95,0.94,0.27,0.14,0.14,0.15,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1590000,1,-0.012,-0.014,0.00042,0.031,-0.025,-0.13,0.0062,-0.0056,-0.063,-0.00039,-0.00033,1.1e-05,0,0,-0.0014,0,0,0,0,0,0,0,0,2.6e-06,0.0073,0.0073,0.0001,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1690000,1,-0.012,-0.014,0.00046,0.028,-0.02,-0.13,0.0044,-0.0038,-0.068,-0.00074,-0.00073,2.4e-05,0,0,-0.0018,0,0,0,0,0,0,0,0,2.3e-06,0.0063,0.0063,7.9e-05,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1790000,1,-0.012,-0.014,0.00041,0.036,-0.025,-0.13,0.0076,-0.006,-0.067,-0.00073,-0.00072,2.4e-05,0,0,-0.0028,0,0,0,0,0,0,0,0,2.6e-06,0.0069,0.007,8.9e-05,1.3,1.3,0.16,0.2,0.2,0.12,0.0078,0.0078,0.00015,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +1890000,1,-0.012,-0.014,0.00041,0.043,-0.026,-0.14,0.012,-0.0085,-0.075,-0.00073,-0.00072,2.4e-05,0,0,-0.0032,0,0,0,0,0,0,0,0,2.9e-06,0.0076,0.0076,0.0001,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.00015,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0.0082,-0.0055,-0.074,-0.0011,-0.0013,3.9e-05,0,0,-0.0046,0,0,0,0,0,0,0,0,2.3e-06,0.006,0.006,8e-05,1.3,1.3,0.13,0.2,0.2,0.11,0.0066,0.0066,0.00011,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +2090000,1,-0.011,-0.014,0.00042,0.042,-0.021,-0.14,0.012,-0.0076,-0.071,-0.0011,-0.0012,3.8e-05,0,0,-0.0065,0,0,0,0,0,0,0,0,2.6e-06,0.0066,0.0066,8.9e-05,1.7,1.7,0.12,0.31,0.31,0.11,0.0066,0.0066,0.00011,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +2190000,1,-0.011,-0.014,0.00036,0.033,-0.014,-0.14,0.0081,-0.0045,-0.077,-0.0015,-0.0018,5.1e-05,0,0,-0.0075,0,0,0,0,0,0,0,0,2e-06,0.0049,0.005,7.2e-05,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,8.8e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +2290000,1,-0.011,-0.014,0.00035,0.038,-0.015,-0.14,0.012,-0.0059,-0.075,-0.0014,-0.0018,5.1e-05,0,0,-0.0098,0,0,0,0,0,0,0,0,2.2e-06,0.0054,0.0054,8e-05,1.5,1.5,0.11,0.29,0.29,0.1,0.0055,0.0055,8.8e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +2390000,1,-0.011,-0.013,0.00035,0.029,-0.01,-0.14,0.0075,-0.0034,-0.072,-0.0017,-0.0023,5.9e-05,0,0,-0.012,0,0,0,0,0,0,0,0,1.6e-06,0.004,0.004,6.6e-05,1,1,0.1,0.19,0.19,0.098,0.0045,0.0045,7e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +2490000,1,-0.011,-0.014,0.00043,0.033,-0.0092,-0.14,0.011,-0.0044,-0.079,-0.0017,-0.0023,5.9e-05,0,0,-0.013,0,0,0,0,0,0,0,0,1.7e-06,0.0044,0.0044,7.2e-05,1.3,1.3,0.1,0.28,0.28,0.097,0.0045,0.0045,7e-05,0.04,0.04,0.037,0,0,0,0,0,0,0,0 +2590000,1,-0.01,-0.013,0.00032,0.023,-0.0062,-0.15,0.0066,-0.0025,-0.085,-0.0019,-0.0027,6.4e-05,0,0,-0.015,0,0,0,0,0,0,0,0,1.3e-06,0.0032,0.0032,6.1e-05,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,5.7e-05,0.04,0.04,0.036,0,0,0,0,0,0,0,0 +2690000,1,-0.01,-0.013,0.00036,0.027,-0.0055,-0.15,0.0091,-0.0031,-0.084,-0.0019,-0.0027,6.4e-05,0,0,-0.018,0,0,0,0,0,0,0,0,1.4e-06,0.0035,0.0035,6.6e-05,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,5.7e-05,0.04,0.04,0.036,0,0,0,0,0,0,0,0 +2790000,1,-0.01,-0.013,0.00029,0.022,-0.0031,-0.14,0.0059,-0.0017,-0.081,-0.002,-0.003,6.7e-05,0,0,-0.022,0,0,0,0,0,0,0,0,1e-06,0.0027,0.0027,5.6e-05,0.76,0.77,0.096,0.16,0.16,0.089,0.0032,0.0032,4.7e-05,0.04,0.04,0.035,0,0,0,0,0,0,0,0 +2890000,1,-0.01,-0.013,0.00021,0.026,-0.0049,-0.14,0.0083,-0.0022,-0.081,-0.002,-0.003,6.6e-05,0,0,-0.025,0,0,0,0,0,0,0,0,1.1e-06,0.0029,0.0029,6.1e-05,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,4.7e-05,0.04,0.04,0.034,0,0,0,0,0,0,0,0 +2990000,1,-0.01,-0.013,0.00022,0.02,-0.0038,-0.15,0.0054,-0.0013,-0.086,-0.002,-0.0033,6.7e-05,0,0,-0.028,0,0,0,0,0,0,0,0,8.5e-07,0.0023,0.0023,5.3e-05,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,3.9e-05,0.04,0.04,0.033,0,0,0,0,0,0,0,0 +3090000,1,-0.01,-0.013,0.00042,0.025,-0.0067,-0.15,0.0077,-0.0019,-0.087,-0.002,-0.0033,6.7e-05,0,0,-0.031,0,0,0,0,0,0,0,0,9.3e-07,0.0025,0.0025,5.7e-05,0.82,0.82,0.095,0.22,0.22,0.086,0.0027,0.0027,3.9e-05,0.04,0.04,0.032,0,0,0,0,0,0,0,0 +3190000,1,-0.01,-0.013,0.00045,0.021,-0.0065,-0.15,0.0051,-0.0014,-0.097,-0.0021,-0.0036,6.8e-05,0,0,-0.033,0,0,0,0,0,0,0,0,7.2e-07,0.002,0.002,4.9e-05,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,3.3e-05,0.04,0.04,0.031,0,0,0,0,0,0,0,0 +3290000,1,-0.01,-0.013,0.00047,0.024,-0.0067,-0.15,0.0074,-0.0021,-0.11,-0.0021,-0.0036,6.7e-05,0,0,-0.034,0,0,0,0,0,0,0,0,7.8e-07,0.0022,0.0022,5.3e-05,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,3.3e-05,0.04,0.04,0.03,0,0,0,0,0,0,0,0 +3390000,1,-0.0098,-0.013,0.00047,0.019,-0.0037,-0.15,0.005,-0.0014,-0.1,-0.0021,-0.0038,6.7e-05,0,0,-0.04,0,0,0,0,0,0,0,0,6e-07,0.0017,0.0017,4.6e-05,0.53,0.53,0.095,0.14,0.14,0.085,0.0019,0.0019,2.8e-05,0.04,0.04,0.029,0,0,0,0,0,0,0,0 +3490000,1,-0.0097,-0.013,0.00045,0.025,-0.0024,-0.15,0.0073,-0.0017,-0.1,-0.0021,-0.0038,6.7e-05,0,0,-0.044,0,0,0,0,0,0,0,0,6.5e-07,0.0019,0.0019,5e-05,0.65,0.65,0.095,0.19,0.19,0.086,0.0019,0.0019,2.8e-05,0.04,0.04,0.028,0,0,0,0,0,0,0,0 +3590000,1,-0.0096,-0.013,0.0004,0.022,-0.0019,-0.15,0.0051,-0.001,-0.11,-0.0022,-0.004,6.6e-05,0,0,-0.047,0,0,0,0,0,0,0,0,5e-07,0.0015,0.0015,4.4e-05,0.48,0.48,0.094,0.13,0.13,0.086,0.0016,0.0016,2.4e-05,0.04,0.04,0.026,0,0,0,0,0,0,0,0 +3690000,1,-0.0096,-0.013,0.00038,0.024,-0.0014,-0.15,0.0075,-0.0013,-0.11,-0.0022,-0.004,6.6e-05,0,0,-0.052,0,0,0,0,0,0,0,0,5.4e-07,0.0017,0.0017,4.7e-05,0.59,0.59,0.093,0.18,0.18,0.085,0.0016,0.0016,2.4e-05,0.04,0.04,0.025,0,0,0,0,0,0,0,0 +3790000,1,-0.0095,-0.012,0.00039,0.02,0.0031,-0.15,0.0052,-0.0006,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.055,0,0,0,0,0,0,0,0,4.3e-07,0.0014,0.0014,4.1e-05,0.44,0.44,0.093,0.12,0.12,0.086,0.0014,0.0014,2.1e-05,0.04,0.04,0.024,0,0,0,0,0,0,0,0 +3890000,1,-0.0094,-0.013,0.00047,0.022,0.0042,-0.14,0.0074,-0.00025,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.059,0,0,0,0,0,0,0,0,4.7e-07,0.0015,0.0015,4.4e-05,0.54,0.54,0.091,0.17,0.17,0.086,0.0014,0.0014,2.1e-05,0.04,0.04,0.022,0,0,0,0,0,0,0,0 +3990000,1,-0.0094,-0.013,0.00053,0.027,0.0038,-0.14,0.0099,7.5e-05,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.064,0,0,0,0,0,0,0,0,5.1e-07,0.0016,0.0016,4.7e-05,0.66,0.66,0.089,0.22,0.22,0.085,0.0014,0.0014,2.1e-05,0.04,0.04,0.021,0,0,0,0,0,0,0,0 +4090000,1,-0.0094,-0.012,0.00059,0.023,0.0031,-0.12,0.0074,0.00032,-0.098,-0.0022,-0.0044,6.2e-05,0,0,-0.072,0,0,0,0,0,0,0,0,3.9e-07,0.0013,0.0013,4.2e-05,0.5,0.5,0.087,0.16,0.16,0.085,0.0011,0.0011,1.8e-05,0.04,0.04,0.02,0,0,0,0,0,0,0,0 +4190000,1,-0.0095,-0.012,0.00055,0.025,0.0027,-0.12,0.0098,0.00061,-0.1,-0.0022,-0.0044,6.2e-05,0,0,-0.074,0,0,0,0,0,0,0,0,4.3e-07,0.0015,0.0015,4.4e-05,0.6,0.6,0.086,0.21,0.21,0.086,0.0011,0.0011,1.8e-05,0.04,0.04,0.019,0,0,0,0,0,0,0,0 +4290000,1,-0.0095,-0.012,0.00055,0.023,0.0026,-0.12,0.0071,0.00052,-0.11,-0.0022,-0.0046,6e-05,0,0,-0.077,0,0,0,0,0,0,0,0,3.4e-07,0.0012,0.0012,4e-05,0.46,0.46,0.084,0.15,0.15,0.085,0.00095,0.00095,1.6e-05,0.04,0.04,0.017,0,0,0,0,0,0,0,0 +4390000,1,-0.0095,-0.012,0.0005,0.026,0.00099,-0.11,0.0097,0.00061,-0.094,-0.0022,-0.0046,6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,3.6e-07,0.0013,0.0013,4.2e-05,0.55,0.55,0.081,0.2,0.2,0.084,0.00095,0.00095,1.6e-05,0.04,0.04,0.016,0,0,0,0,0,0,0,0 +4490000,1,-0.0095,-0.012,0.00056,0.022,0.0028,-0.11,0.0071,0.00052,-0.095,-0.0022,-0.0048,5.7e-05,0,0,-0.086,0,0,0,0,0,0,0,0,2.8e-07,0.001,0.001,3.8e-05,0.42,0.42,0.08,0.14,0.14,0.085,0.00078,0.00078,1.4e-05,0.04,0.04,0.015,0,0,0,0,0,0,0,0 +4590000,1,-0.0095,-0.012,0.00062,0.025,0.0015,-0.11,0.0095,0.00072,-0.098,-0.0022,-0.0048,5.7e-05,0,0,-0.089,0,0,0,0,0,0,0,0,3.1e-07,0.0011,0.0011,4e-05,0.51,0.51,0.077,0.19,0.19,0.084,0.00078,0.00078,1.4e-05,0.04,0.04,0.014,0,0,0,0,0,0,0,0 +4690000,1,-0.0095,-0.012,0.00055,0.019,0.0018,-0.1,0.0069,0.00051,-0.09,-0.0022,-0.005,5.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,2.4e-07,0.00092,0.00092,3.6e-05,0.39,0.39,0.074,0.14,0.14,0.083,0.00064,0.00064,1.3e-05,0.04,0.04,0.013,0,0,0,0,0,0,0,0 +4790000,1,-0.0094,-0.012,0.00064,0.017,0.0039,-0.099,0.0087,0.00084,-0.092,-0.0022,-0.005,5.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,2.6e-07,0.00099,0.00099,3.8e-05,0.47,0.47,0.073,0.18,0.18,0.084,0.00064,0.00064,1.3e-05,0.04,0.04,0.012,0,0,0,0,0,0,0,0 +4890000,1,-0.0093,-0.012,0.00067,0.012,0.0015,-0.093,0.0059,0.00065,-0.088,-0.0021,-0.0051,5.3e-05,0,0,-0.099,0,0,0,0,0,0,0,0,2e-07,0.0008,0.0008,3.5e-05,0.36,0.36,0.07,0.13,0.13,0.083,0.00052,0.00052,1.1e-05,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +4990000,1,-0.0093,-0.012,0.00065,0.016,0.0021,-0.085,0.0073,0.00085,-0.083,-0.0021,-0.0051,5.3e-05,0,0,-0.1,0,0,0,0,0,0,0,0,2.1e-07,0.00086,0.00086,3.6e-05,0.43,0.43,0.067,0.17,0.17,0.082,0.00052,0.00052,1.1e-05,0.04,0.04,0.01,0,0,0,0,0,0,0,0 +5090000,1,-0.0092,-0.012,0.00072,0.012,0.0026,-0.082,0.0051,0.00061,-0.081,-0.0021,-0.0052,5.1e-05,0,0,-0.1,0,0,0,0,0,0,0,0,1.6e-07,0.00069,0.00069,3.3e-05,0.33,0.33,0.065,0.12,0.12,0.082,0.00042,0.00042,1e-05,0.04,0.04,0.0098,0,0,0,0,0,0,0,0 +5190000,1,-0.009,-0.012,0.00076,0.012,0.006,-0.08,0.0063,0.001,-0.079,-0.0021,-0.0052,5.1e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.8e-07,0.00075,0.00075,3.5e-05,0.39,0.39,0.063,0.16,0.16,0.081,0.00042,0.00042,1e-05,0.04,0.04,0.0091,0,0,0,0,0,0,0,0 +5290000,1,-0.0089,-0.012,0.00081,0.01,0.0062,-0.068,0.0044,0.00098,-0.072,-0.0021,-0.0053,4.9e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.3e-07,0.0006,0.0006,3.2e-05,0.3,0.3,0.06,0.12,0.12,0.08,0.00034,0.00034,9.1e-06,0.04,0.04,0.0084,0,0,0,0,0,0,0,0 +5390000,1,-0.0088,-0.012,0.00081,0.0099,0.0098,-0.065,0.0054,0.0017,-0.067,-0.0021,-0.0053,4.9e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.4e-07,0.00065,0.00065,3.4e-05,0.36,0.36,0.057,0.15,0.15,0.079,0.00034,0.00034,9.1e-06,0.04,0.04,0.0078,0,0,0,0,0,0,0,0 +5490000,1,-0.0089,-0.012,0.0008,0.0091,0.011,-0.06,0.0037,0.0017,-0.065,-0.0021,-0.0054,4.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.1e-07,0.00052,0.00052,3.1e-05,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,8.3e-06,0.04,0.04,0.0073,0,0,0,0,0,0,0,0 +5590000,1,-0.0089,-0.012,0.00072,0.011,0.015,-0.053,0.0048,0.003,-0.058,-0.0021,-0.0054,4.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,1.2e-07,0.00056,0.00056,3.2e-05,0.32,0.32,0.053,0.15,0.15,0.078,0.00028,0.00028,8.3e-06,0.04,0.04,0.0067,0,0,0,0,0,0,0,0 +5690000,1,-0.0089,-0.011,0.00062,0.0095,0.015,-0.052,0.0034,0.0027,-0.055,-0.002,-0.0055,4.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,9.3e-08,0.00045,0.00045,3e-05,0.25,0.25,0.051,0.11,0.11,0.076,0.00022,0.00022,7.5e-06,0.04,0.04,0.0062,0,0,0,0,0,0,0,0 +5790000,1,-0.0088,-0.011,0.00057,0.011,0.017,-0.049,0.0045,0.0042,-0.053,-0.002,-0.0055,4.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,9.8e-08,0.00048,0.00048,3.1e-05,0.29,0.29,0.05,0.14,0.14,0.077,0.00022,0.00022,7.5e-06,0.04,0.04,0.0058,0,0,0,0,0,0,0,0 +5890000,1,-0.0088,-0.011,0.0006,0.011,0.015,-0.048,0.0034,0.0035,-0.056,-0.002,-0.0055,4.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,7.7e-08,0.00039,0.00039,2.9e-05,0.23,0.23,0.048,0.1,0.1,0.075,0.00018,0.00018,6.8e-06,0.04,0.04,0.0054,0,0,0,0,0,0,0,0 +5990000,1,-0.0088,-0.012,0.00056,0.013,0.016,-0.041,0.0046,0.005,-0.05,-0.002,-0.0055,4.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,8.3e-08,0.00041,0.00041,3e-05,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,6.8e-06,0.04,0.04,0.005,0,0,0,0,0,0,0,0 +6090000,1,-0.0088,-0.012,0.00037,0.013,0.017,-0.039,0.0059,0.0067,-0.047,-0.002,-0.0055,4.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,8.8e-08,0.00044,0.00044,3.1e-05,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,6.8e-06,0.04,0.04,0.0047,0,0,0,0,0,0,0,0 +6190000,0.98,-0.0066,-0.013,0.19,0.0036,0.019,-0.037,0.004,0.0057,-0.047,-0.0019,-0.0056,4.2e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00034,0.00034,0.0054,0.22,0.22,0.042,0.13,0.13,0.073,0.00015,0.00015,6.3e-06,0.04,0.04,0.0044,0,0,0,0,0,0,0,0 +6290000,0.98,-0.0065,-0.013,0.19,0.0021,0.021,-0.041,0.0043,0.0076,-0.053,-0.0019,-0.0056,4.2e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00034,0.00034,0.0054,0.22,0.22,0.04,0.16,0.16,0.072,0.00015,0.00015,6.3e-06,0.04,0.04,0.0041,0,0,0,0,0,0,0,0 +6390000,0.98,-0.0065,-0.013,0.19,0.0043,0.023,-0.042,0.0046,0.0098,-0.056,-0.0019,-0.0056,4.2e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00034,0.00034,0.0054,0.23,0.23,0.039,0.19,0.19,0.072,0.00015,0.00015,6.3e-06,0.04,0.04,0.0038,0,0,0,0,0,0,0,0 +6490000,0.98,-0.0064,-0.013,0.19,0.0022,0.022,-0.039,0.005,0.012,-0.053,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00035,0.00034,0.0054,0.24,0.24,0.038,0.23,0.23,0.07,0.00015,0.00015,6.3e-06,0.04,0.04,0.0036,0,0,0,0,0,0,0,0 +6590000,0.98,-0.0064,-0.013,0.19,0.0016,0.025,-0.042,0.0052,0.014,-0.056,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00035,0.00035,0.0054,0.26,0.26,0.036,0.28,0.28,0.069,0.00015,0.00015,6.3e-06,0.04,0.04,0.0033,0,0,0,0,0,0,0,0 +6690000,0.98,-0.0063,-0.013,0.19,-0.001,0.028,-0.044,0.0052,0.017,-0.057,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00035,0.00035,0.0054,0.28,0.28,0.035,0.33,0.33,0.068,0.00015,0.00015,6.3e-06,0.04,0.04,0.0031,0,0,0,0,0,0,0,0 +6790000,0.98,-0.0063,-0.013,0.19,0.00082,0.03,-0.042,0.0051,0.02,-0.058,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00036,0.00036,0.0054,0.3,0.3,0.034,0.38,0.38,0.068,0.00015,0.00015,6.3e-06,0.04,0.04,0.0029,0,0,0,0,0,0,0,0 +6890000,0.98,-0.0061,-0.013,0.19,0.00043,0.031,-0.038,0.0052,0.023,-0.055,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00036,0.00036,0.0054,0.32,0.32,0.032,0.45,0.45,0.067,0.00015,0.00015,6.3e-06,0.04,0.04,0.0027,0,0,0,0,0,0,0,0 +6990000,0.98,-0.006,-0.013,0.19,0.00043,0.032,-0.037,0.0052,0.026,-0.055,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00037,0.00037,0.0054,0.35,0.35,0.031,0.52,0.52,0.066,0.00015,0.00015,6.3e-06,0.04,0.04,0.0026,0,0,0,0,0,0,0,0 +7090000,0.98,-0.0059,-0.013,0.19,-0.00042,0.038,-0.037,0.0052,0.029,-0.056,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00037,0.00037,0.0054,0.39,0.39,0.03,0.6,0.6,0.066,0.00015,0.00015,6.3e-06,0.04,0.04,0.0024,0,0,0,0,0,0,0,0 +7190000,0.98,-0.0058,-0.013,0.19,-0.00081,0.041,-0.036,0.0051,0.033,-0.058,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00038,0.00038,0.0054,0.42,0.42,0.029,0.69,0.69,0.065,0.00015,0.00015,6.3e-06,0.04,0.04,0.0023,0,0,0,0,0,0,0,0 +7290000,0.98,-0.0058,-0.013,0.19,3.9e-05,0.046,-0.034,0.005,0.038,-0.054,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00039,0.00039,0.0054,0.46,0.46,0.028,0.79,0.79,0.064,0.00015,0.00015,6.3e-06,0.04,0.04,0.0021,0,0,0,0,0,0,0,0 +7390000,0.98,-0.0057,-0.013,0.19,-0.0017,0.049,-0.032,0.0049,0.042,-0.052,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.0004,0.0004,0.0054,0.51,0.51,0.027,0.9,0.9,0.064,0.00014,0.00014,6.3e-06,0.04,0.04,0.002,0,0,0,0,0,0,0,0 +7490000,0.98,-0.0057,-0.013,0.19,0.00069,0.053,-0.026,0.0049,0.047,-0.046,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00041,0.00041,0.0054,0.56,0.56,0.026,1,1,0.063,0.00014,0.00014,6.3e-06,0.04,0.04,0.0019,0,0,0,0,0,0,0,0 +7590000,0.98,-0.0057,-0.013,0.19,0.0017,0.056,-0.023,0.005,0.052,-0.041,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00042,0.00041,0.0054,0.6,0.6,0.025,1.2,1.2,0.062,0.00014,0.00014,6.3e-06,0.04,0.04,0.0018,0,0,0,0,0,0,0,0 +7690000,0.98,-0.0057,-0.013,0.19,0.0014,0.061,-0.022,0.0052,0.058,-0.036,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00043,0.00043,0.0054,0.66,0.66,0.025,1.3,1.3,0.062,0.00014,0.00014,6.3e-06,0.04,0.04,0.0017,0,0,0,0,0,0,0,0 +7790000,0.98,-0.0056,-0.013,0.19,0.0031,0.063,-0.024,0.0053,0.063,-0.041,-0.0019,-0.0056,4.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00044,0.00043,0.0054,0.72,0.72,0.024,1.5,1.5,0.061,0.00014,0.00014,6.3e-06,0.04,0.04,0.0016,0,0,0,0,0,0,0,0 +7890000,0.98,-0.0056,-0.013,0.19,0.0018,0.068,-0.025,0.0055,0.07,-0.045,-0.0019,-0.0056,4.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00045,0.00045,0.0054,0.78,0.78,0.023,1.7,1.7,0.06,0.00014,0.00014,6.3e-06,0.04,0.04,0.0015,0,0,0,0,0,0,0,0 +7990000,0.98,-0.0056,-0.013,0.19,0.0021,0.071,-0.021,0.0056,0.075,-0.041,-0.0019,-0.0056,4.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00045,0.00045,0.0054,0.84,0.84,0.022,1.9,1.9,0.059,0.00014,0.00014,6.3e-06,0.04,0.04,0.0014,0,0,0,0,0,0,0,0 +8090000,0.98,-0.0054,-0.013,0.19,0.0035,0.076,-0.022,0.0058,0.083,-0.044,-0.0019,-0.0056,4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00047,0.00047,0.0043,0.91,0.92,0.022,2.1,2.1,0.059,0.00014,0.00014,6.3e-06,0.04,0.04,0.0014,0,0,0,0,0,0,0,0 +8190000,0.98,-0.0054,-0.013,0.19,0.0042,0.083,-0.018,0.0062,0.091,-0.038,-0.0019,-0.0056,4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00048,0.00048,0.0043,0.99,0.99,0.021,2.4,2.4,0.058,0.00014,0.00014,6.3e-06,0.04,0.04,0.0013,0,0,0,0,0,0,0,0 +8290000,0.98,-0.0054,-0.013,0.19,0.0064,0.087,-0.016,0.0065,0.097,-0.038,-0.0019,-0.0056,3.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00049,0.00049,0.0043,1.1,1.1,0.02,2.6,2.6,0.057,0.00014,0.00014,6.3e-06,0.04,0.04,0.0012,0,0,0,0,0,0,0,0 +8390000,0.98,-0.0054,-0.013,0.19,0.0043,0.09,-0.015,0.0071,0.11,-0.036,-0.0019,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0005,0.0005,0.0043,1.1,1.1,0.02,2.9,2.9,0.057,0.00014,0.00014,6.3e-06,0.04,0.04,0.0012,0,0,0,0,0,0,0,0 +8490000,0.98,-0.0053,-0.013,0.19,0.0041,0.094,-0.017,0.0073,0.11,-0.041,-0.0018,-0.0056,3.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00051,0.00051,0.0043,1.2,1.2,0.019,3.2,3.2,0.056,0.00014,0.00014,6.3e-06,0.04,0.04,0.0011,0,0,0,0,0,0,0,0 +8590000,0.98,-0.0052,-0.013,0.19,0.0052,0.098,-0.012,0.0078,0.12,-0.036,-0.0018,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00052,0.00052,0.0043,1.3,1.3,0.018,3.6,3.6,0.055,0.00014,0.00014,6.3e-06,0.04,0.04,0.0011,0,0,0,0,0,0,0,0 +8690000,0.98,-0.0053,-0.013,0.19,0.0053,0.099,-0.014,0.008,0.13,-0.037,-0.0018,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00053,0.00052,0.0043,1.4,1.4,0.018,3.9,3.9,0.055,0.00013,0.00013,6.3e-06,0.04,0.04,0.001,0,0,0,0,0,0,0,0 +8790000,0.98,-0.0053,-0.013,0.19,0.0068,0.1,-0.013,0.0085,0.14,-0.035,-0.0018,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00054,0.00054,0.0043,1.5,1.5,0.018,4.3,4.3,0.055,0.00013,0.00013,6.3e-06,0.04,0.04,0.00096,0,0,0,0,0,0,0,0 +8890000,0.98,-0.0053,-0.013,0.19,0.0069,0.1,-0.0091,0.0088,0.14,-0.029,-0.0018,-0.0056,3.8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00054,0.00054,0.0043,1.5,1.5,0.017,4.6,4.6,0.054,0.00013,0.00013,6.3e-06,0.04,0.04,0.00091,0,0,0,0,0,0,0,0 +8990000,0.98,-0.0053,-0.013,0.19,0.0061,0.11,-0.0083,0.0094,0.15,-0.032,-0.0018,-0.0056,3.8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00056,0.0043,1.6,1.6,0.017,5.2,5.2,0.054,0.00013,0.00013,6.3e-06,0.04,0.04,0.00088,0,0,0,0,0,0,0,0 +9090000,0.98,-0.0053,-0.013,0.19,0.0065,0.11,-0.0093,0.0096,0.16,-0.032,-0.0018,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0043,1.7,1.7,0.016,5.4,5.4,0.053,0.00013,0.00013,6.3e-06,0.04,0.04,0.00084,0,0,0,0,0,0,0,0 +9190000,0.98,-0.0053,-0.013,0.19,0.01,0.12,-0.0088,0.01,0.17,-0.032,-0.0018,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0043,1.8,1.8,0.016,6,6.1,0.052,0.00013,0.00013,6.3e-06,0.04,0.04,0.0008,0,0,0,0,0,0,0,0 +9290000,0.98,-0.0052,-0.013,0.19,0.013,0.12,-0.0072,0.011,0.17,-0.03,-0.0017,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00055,0.0044,1.8,1.8,0.015,6.3,6.3,0.052,0.00012,0.00012,6.3e-06,0.04,0.04,0.00076,0,0,0,0,0,0,0,0 +9390000,0.98,-0.0051,-0.013,0.19,0.013,0.12,-0.006,0.012,0.18,-0.03,-0.0017,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,1.9,1.9,0.015,7,7,0.052,0.00012,0.00012,6.3e-06,0.04,0.04,0.00073,0,0,0,0,0,0,0,0 +9490000,0.98,-0.0052,-0.013,0.19,0.013,0.12,-0.0044,0.013,0.18,-0.027,-0.0017,-0.0056,3.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00056,0.0044,1.9,1.9,0.015,7.2,7.2,0.051,0.00012,0.00012,6.3e-06,0.04,0.04,0.0007,0,0,0,0,0,0,0,0 +9590000,0.98,-0.0053,-0.013,0.19,0.013,0.12,-0.0043,0.014,0.19,-0.028,-0.0017,-0.0056,3.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,2,2,0.014,7.9,7.9,0.05,0.00012,0.00012,6.3e-06,0.04,0.04,0.00067,0,0,0,0,0,0,0,0 +9690000,0.98,-0.0054,-0.013,0.19,0.013,0.12,-0.0014,0.014,0.19,-0.027,-0.0017,-0.0056,3.5e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0044,2,2,0.014,8,8.1,0.05,0.00012,0.00012,6.3e-06,0.04,0.04,0.00065,0,0,0,0,0,0,0,0 +9790000,0.98,-0.0053,-0.013,0.19,0.014,0.12,-0.0027,0.015,0.2,-0.028,-0.0017,-0.0056,3.5e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,2.1,2.1,0.014,8.9,8.9,0.05,0.00012,0.00012,6.3e-06,0.04,0.04,0.00062,0,0,0,0,0,0,0,0 +9890000,0.98,-0.0054,-0.013,0.19,0.017,0.11,-0.0014,0.015,0.19,-0.029,-0.0016,-0.0056,3.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00054,0.00054,0.0044,2.1,2.1,0.013,8.9,8.9,0.049,0.00011,0.00011,6.3e-06,0.04,0.04,0.0006,0,0,0,0,0,0,0,0 +9990000,0.98,-0.0054,-0.013,0.19,0.019,0.12,-0.00072,0.017,0.2,-0.031,-0.0016,-0.0056,3.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00056,0.0044,2.2,2.2,0.013,9.8,9.8,0.049,0.00011,0.00011,6.3e-06,0.04,0.04,0.00058,0,0,0,0,0,0,0,0 +10090000,0.98,-0.0055,-0.013,0.19,0.017,0.11,0.00048,0.017,0.19,-0.029,-0.0016,-0.0056,3.3e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00053,0.00053,0.0044,2.1,2.1,0.013,9.7,9.7,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.00056,0,0,0,0,0,0,0,0 +10190000,0.98,-0.0055,-0.013,0.19,0.016,0.11,0.0014,0.019,0.21,-0.03,-0.0016,-0.0056,3.3e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0044,2.3,2.3,0.012,11,11,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.00054,0,0,0,0,0,0,0,0 +10290000,0.98,-0.0055,-0.013,0.19,0.016,0.11,0.00029,0.02,0.22,-0.029,-0.0016,-0.0056,3.3e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,2.4,2.4,0.012,12,12,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.00052,0,0,0,0,0,0,0,0 +10390000,0.98,-0.0055,-0.013,0.19,0.0072,0.0051,-0.0025,0.00076,0.00015,-0.028,-0.0016,-0.0056,3.3e-05,-3.6e-08,2.5e-08,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00059,0.00058,0.0044,0.25,0.25,0.56,0.25,0.25,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10490000,0.98,-0.0054,-0.013,0.19,0.0089,0.0076,0.007,0.0015,0.00075,-0.023,-0.0016,-0.0056,3.3e-05,-1.2e-06,8.4e-07,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0006,0.0006,0.0044,0.26,0.26,0.55,0.26,0.26,0.057,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10590000,0.98,-0.0053,-0.012,0.19,-0.00085,0.0054,0.013,-0.0012,-0.0054,-0.021,-0.0016,-0.0057,3.2e-05,0.00033,4.5e-06,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00061,0.00061,0.0044,0.13,0.13,0.27,0.13,0.13,0.055,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10690000,0.98,-0.0053,-0.013,0.19,0.00049,0.0066,0.016,-0.0012,-0.0048,-0.017,-0.0016,-0.0057,3.2e-05,0.00033,5.2e-06,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00063,0.00063,0.0044,0.15,0.15,0.26,0.14,0.14,0.065,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10790000,0.98,-0.0054,-0.013,0.19,0.0023,0.003,0.014,-0.00076,-0.0047,-0.015,-0.0016,-0.0057,3.1e-05,0.00054,0.00046,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00062,0.00062,0.0044,0.1,0.1,0.17,0.091,0.091,0.062,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10890000,0.98,-0.0054,-0.013,0.19,0.0026,0.0064,0.01,-0.00051,-0.0043,-0.018,-0.0016,-0.0057,3.1e-05,0.00055,0.00046,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00064,0.00064,0.0044,0.12,0.12,0.16,0.098,0.098,0.068,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10990000,0.98,-0.0054,-0.013,0.19,0.002,0.012,0.016,-0.00041,-0.003,-0.011,-0.0015,-0.0057,3e-05,0.00072,0.00069,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0006,0.0006,0.0044,0.096,0.096,0.12,0.073,0.073,0.065,9.9e-05,9.9e-05,6.3e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 +11090000,0.98,-0.0056,-0.013,0.19,0.0031,0.017,0.02,-0.00019,-0.0016,-0.0074,-0.0015,-0.0057,3e-05,0.00072,0.00069,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00062,0.00062,0.0044,0.12,0.12,0.11,0.08,0.08,0.069,9.9e-05,9.9e-05,6.3e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 +11190000,0.98,-0.0059,-0.013,0.19,0.0047,0.017,0.026,0.0011,-0.0018,-0.00035,-0.0014,-0.0057,2.8e-05,0.00074,0.0015,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0044,0.096,0.096,0.083,0.063,0.063,0.066,9.1e-05,9.1e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11290000,0.98,-0.0058,-0.013,0.19,0.0051,0.018,0.026,0.0016,-5.3e-05,-0.00012,-0.0014,-0.0057,2.8e-05,0.00075,0.0015,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,0.12,0.12,0.077,0.07,0.07,0.069,9.1e-05,9.1e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11390000,0.98,-0.006,-0.013,0.19,0.0032,0.015,0.016,0.00095,-0.00089,-0.0086,-0.0014,-0.0058,2.6e-05,0.0013,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00049,0.00049,0.0044,0.097,0.097,0.062,0.057,0.057,0.066,8.3e-05,8.3e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11490000,0.98,-0.0059,-0.013,0.19,0.0025,0.016,0.02,0.0012,0.00069,-0.0025,-0.0014,-0.0058,2.6e-05,0.0013,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0005,0.0005,0.0044,0.12,0.12,0.057,0.065,0.065,0.067,8.3e-05,8.3e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11590000,0.98,-0.0062,-0.013,0.19,0.004,0.013,0.018,0.00091,-0.00038,-0.0036,-0.0013,-0.0058,2.4e-05,0.0017,0.0027,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00042,0.00042,0.0044,0.096,0.096,0.048,0.054,0.054,0.065,7.5e-05,7.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 +11690000,0.98,-0.0062,-0.012,0.19,0.0046,0.017,0.018,0.0014,0.0011,-0.0049,-0.0013,-0.0058,2.4e-05,0.0017,0.0027,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00044,0.00044,0.0044,0.12,0.12,0.044,0.062,0.062,0.066,7.5e-05,7.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 +11790000,0.98,-0.0066,-0.012,0.19,0.003,0.011,0.019,0.00076,-0.0018,-0.002,-0.0012,-0.0059,2e-05,0.0022,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00037,0.00037,0.0044,0.092,0.092,0.037,0.052,0.052,0.063,6.8e-05,6.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +11890000,0.98,-0.0067,-0.012,0.19,0.0058,0.012,0.017,0.0011,-0.00072,-0.0013,-0.0012,-0.0059,2e-05,0.0022,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00038,0.00038,0.0044,0.11,0.11,0.034,0.061,0.061,0.063,6.8e-05,6.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +11990000,0.98,-0.0069,-0.012,0.19,0.0086,0.012,0.015,0.0022,-0.0018,-0.0049,-0.0012,-0.0059,2e-05,0.0022,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00032,0.00032,0.0044,0.088,0.088,0.03,0.051,0.051,0.061,6.2e-05,6.2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12090000,0.98,-0.0068,-0.012,0.19,0.01,0.012,0.018,0.0031,-0.00066,0.0011,-0.0012,-0.0059,2e-05,0.0022,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00033,0.00033,0.0044,0.11,0.11,0.027,0.06,0.06,0.06,6.2e-05,6.2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12190000,0.98,-0.0067,-0.012,0.19,0.0082,0.011,0.017,0.0018,0.00038,0.0029,-0.0012,-0.0059,2e-05,0.0024,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00029,0.00029,0.0044,0.083,0.083,0.024,0.05,0.05,0.058,5.7e-05,5.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12290000,0.98,-0.0068,-0.012,0.19,0.0059,0.01,0.016,0.0026,0.0015,0.0039,-0.0012,-0.0059,2e-05,0.0024,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0003,0.0003,0.0044,0.098,0.098,0.022,0.059,0.059,0.058,5.7e-05,5.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12390000,0.98,-0.0069,-0.012,0.19,0.0044,0.0071,0.014,0.0017,0.00048,-0.0021,-0.0012,-0.0059,1.9e-05,0.0027,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00026,0.00026,0.0044,0.078,0.078,0.02,0.05,0.05,0.056,5.3e-05,5.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12490000,0.98,-0.0069,-0.012,0.19,0.0044,0.0081,0.018,0.0022,0.0012,-6.9e-05,-0.0012,-0.0059,1.9e-05,0.0027,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00027,0.00027,0.0044,0.091,0.091,0.018,0.059,0.059,0.055,5.3e-05,5.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12590000,0.98,-0.0071,-0.012,0.19,0.0081,0.0015,0.019,0.0033,-0.0014,0.0017,-0.0011,-0.006,1.7e-05,0.0027,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00024,0.00024,0.0044,0.072,0.072,0.017,0.049,0.049,0.054,5e-05,5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12690000,0.98,-0.007,-0.012,0.19,0.0087,-0.00069,0.019,0.0041,-0.0013,0.0033,-0.0011,-0.006,1.7e-05,0.0027,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00025,0.00025,0.0044,0.084,0.084,0.016,0.058,0.058,0.053,5e-05,5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12790000,0.98,-0.0073,-0.012,0.19,0.01,-0.0041,0.021,0.0042,-0.0044,0.0054,-0.0011,-0.006,1.6e-05,0.0028,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00023,0.00023,0.0044,0.068,0.068,0.014,0.049,0.049,0.052,4.7e-05,4.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12890000,0.98,-0.0073,-0.012,0.19,0.01,-0.005,0.022,0.0052,-0.0049,0.0085,-0.0011,-0.006,1.6e-05,0.0028,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00024,0.00024,0.0044,0.078,0.078,0.013,0.058,0.058,0.051,4.7e-05,4.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12990000,0.98,-0.0072,-0.012,0.19,0.0082,-0.003,0.022,0.0036,-0.0036,0.0096,-0.0011,-0.006,1.6e-05,0.0028,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00022,0.00022,0.0044,0.064,0.064,0.012,0.049,0.049,0.05,4.5e-05,4.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13090000,0.98,-0.0073,-0.012,0.19,0.0091,-0.0029,0.02,0.0044,-0.0038,0.0085,-0.0011,-0.006,1.6e-05,0.0029,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00023,0.00023,0.0044,0.073,0.073,0.012,0.057,0.057,0.049,4.5e-05,4.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13190000,0.98,-0.0073,-0.012,0.19,0.004,-0.0045,0.019,0.00097,-0.0046,0.0091,-0.0011,-0.006,1.6e-05,0.0029,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.06,0.06,0.011,0.049,0.049,0.047,4.3e-05,4.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13290000,0.98,-0.0073,-0.012,0.19,0.0036,-0.0055,0.016,0.0013,-0.005,0.0084,-0.0011,-0.006,1.6e-05,0.003,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00022,0.00022,0.0044,0.068,0.068,0.01,0.057,0.057,0.047,4.3e-05,4.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13390000,0.98,-0.0072,-0.012,0.19,0.0027,-0.0035,0.016,0.00086,-0.0038,0.0091,-0.0011,-0.006,1.6e-05,0.003,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.056,0.056,0.0097,0.049,0.049,0.046,4e-05,4e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13490000,0.98,-0.0072,-0.012,0.19,0.0032,-0.0018,0.015,0.0012,-0.0039,0.0053,-0.0011,-0.006,1.6e-05,0.0031,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00022,0.00021,0.0044,0.064,0.064,0.0093,0.057,0.057,0.045,4e-05,4e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13590000,0.98,-0.0072,-0.012,0.19,0.0077,-0.002,0.017,0.004,-0.0032,0.0038,-0.0011,-0.006,1.6e-05,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.053,0.053,0.0088,0.048,0.048,0.044,3.8e-05,3.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13690000,0.98,-0.0072,-0.012,0.19,0.0077,-0.0035,0.017,0.0048,-0.0035,0.0064,-0.0011,-0.006,1.6e-05,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.061,0.061,0.0085,0.056,0.056,0.044,3.8e-05,3.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13790000,0.98,-0.0071,-0.012,0.19,0.015,0.00072,0.017,0.0083,-0.00094,0.0059,-0.0011,-0.0059,1.7e-05,0.0037,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.051,0.051,0.0082,0.048,0.048,0.043,3.7e-05,3.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13890000,0.98,-0.007,-0.012,0.19,0.016,0.0014,0.018,0.0098,-0.00083,0.0081,-0.0011,-0.0059,1.7e-05,0.0037,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.058,0.058,0.008,0.056,0.056,0.042,3.7e-05,3.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13990000,0.98,-0.0071,-0.012,0.19,0.015,0.0016,0.017,0.0074,-0.0023,0.007,-0.0011,-0.006,1.6e-05,0.0035,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.049,0.049,0.0077,0.048,0.048,0.041,3.5e-05,3.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14090000,0.98,-0.0071,-0.012,0.19,0.013,-0.0029,0.018,0.0089,-0.0024,0.0034,-0.0011,-0.006,1.6e-05,0.0036,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.055,0.055,0.0076,0.055,0.055,0.041,3.5e-05,3.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14190000,0.98,-0.0071,-0.012,0.19,0.01,-0.0016,0.018,0.008,-0.0018,0.0036,-0.0011,-0.006,1.7e-05,0.0037,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.047,0.047,0.0074,0.048,0.048,0.04,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14290000,0.98,-0.007,-0.012,0.19,0.012,-0.0017,0.016,0.009,-0.002,0.0079,-0.0011,-0.006,1.7e-05,0.0037,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.053,0.053,0.0073,0.055,0.055,0.04,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14390000,0.98,-0.0071,-0.011,0.19,0.012,-0.0046,0.017,0.0084,-0.0031,0.012,-0.0011,-0.006,1.6e-05,0.0035,0.0032,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.045,0.045,0.0071,0.048,0.048,0.039,3.1e-05,3.1e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14490000,0.98,-0.0072,-0.011,0.19,0.01,-0.0044,0.021,0.0095,-0.0036,0.015,-0.0011,-0.006,1.6e-05,0.0035,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.051,0.051,0.0071,0.055,0.055,0.038,3.1e-05,3.1e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14590000,0.98,-0.0073,-0.011,0.19,0.0079,-0.0045,0.019,0.0059,-0.0042,0.011,-0.0011,-0.006,1.6e-05,0.0033,0.0031,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0044,0.044,0.044,0.007,0.047,0.047,0.038,2.9e-05,2.9e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14690000,0.98,-0.0073,-0.011,0.19,0.0071,-0.0045,0.019,0.0066,-0.0046,0.011,-0.0011,-0.006,1.6e-05,0.0033,0.0031,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.05,0.05,0.007,0.054,0.054,0.037,2.9e-05,2.9e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14790000,0.98,-0.0072,-0.011,0.19,0.0088,0.0025,0.019,0.0053,0.00078,0.014,-0.0012,-0.006,1.8e-05,0.0034,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0044,0.043,0.043,0.0069,0.047,0.047,0.037,2.7e-05,2.7e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14890000,0.98,-0.0071,-0.011,0.19,0.0074,9e-05,0.023,0.006,0.00092,0.014,-0.0012,-0.006,1.8e-05,0.0034,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.049,0.049,0.007,0.054,0.054,0.037,2.7e-05,2.7e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14990000,0.98,-0.0072,-0.011,0.19,0.0062,-0.0015,0.026,0.0047,-0.0007,0.017,-0.0012,-0.0061,1.7e-05,0.0032,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0045,0.042,0.042,0.0069,0.047,0.047,0.036,2.6e-05,2.6e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15090000,0.98,-0.0072,-0.011,0.19,0.0062,-0.00048,0.03,0.0054,-0.00084,0.019,-0.0012,-0.0061,1.7e-05,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0045,0.048,0.048,0.007,0.054,0.054,0.036,2.6e-05,2.6e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15190000,0.98,-0.0073,-0.011,0.19,0.0043,-0.0015,0.03,0.0042,-0.00076,0.021,-0.0012,-0.0061,1.7e-05,0.0032,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0045,0.041,0.041,0.007,0.047,0.047,0.036,2.4e-05,2.4e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15290000,0.98,-0.0074,-0.011,0.19,0.0049,-0.0026,0.03,0.0047,-0.00095,0.018,-0.0012,-0.0061,1.7e-05,0.0035,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0045,0.047,0.047,0.0071,0.054,0.054,0.035,2.4e-05,2.4e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15390000,0.98,-0.0075,-0.011,0.19,0.0051,-0.00023,0.029,0.0038,-0.00065,0.018,-0.0012,-0.0061,1.7e-05,0.0035,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00018,0.00018,0.0045,0.041,0.041,0.0071,0.047,0.047,0.035,2.2e-05,2.2e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15490000,0.98,-0.0075,-0.011,0.19,0.0045,-0.0028,0.029,0.0042,-0.00077,0.019,-0.0012,-0.0061,1.7e-05,0.0036,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00019,0.00019,0.0045,0.046,0.046,0.0072,0.053,0.053,0.035,2.2e-05,2.2e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15590000,0.98,-0.0078,-0.011,0.19,0.0081,-0.0066,0.029,0.0063,-0.0047,0.018,-0.0011,-0.0061,1.5e-05,0.004,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.04,0.04,0.0072,0.047,0.047,0.035,2.1e-05,2.1e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15690000,0.98,-0.0077,-0.011,0.19,0.0099,-0.0098,0.029,0.0072,-0.0055,0.019,-0.0011,-0.0061,1.5e-05,0.0041,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00019,0.00019,0.0045,0.045,0.045,0.0073,0.053,0.053,0.034,2.1e-05,2.1e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15790000,0.98,-0.0077,-0.011,0.19,0.0065,-0.0091,0.029,0.0055,-0.0043,0.02,-0.0011,-0.0061,1.6e-05,0.0041,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.04,0.04,0.0073,0.046,0.046,0.034,1.9e-05,1.9e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15890000,0.98,-0.0078,-0.011,0.19,0.0053,-0.0075,0.03,0.0061,-0.0051,0.02,-0.0011,-0.0061,1.6e-05,0.0042,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.045,0.045,0.0074,0.053,0.053,0.034,1.9e-05,1.9e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15990000,0.98,-0.0076,-0.011,0.19,0.0033,-0.006,0.027,0.0048,-0.0039,0.019,-0.0012,-0.0061,1.6e-05,0.0045,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.039,0.039,0.0074,0.046,0.046,0.034,1.7e-05,1.7e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +16090000,0.98,-0.0075,-0.011,0.19,0.0026,-0.0073,0.024,0.0051,-0.0046,0.019,-0.0012,-0.0061,1.6e-05,0.0046,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.044,0.044,0.0076,0.053,0.053,0.034,1.7e-05,1.7e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +16190000,0.98,-0.0074,-0.011,0.19,-0.0013,-0.0049,0.023,0.0027,-0.0034,0.016,-0.0012,-0.0061,1.7e-05,0.0047,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.039,0.039,0.0076,0.046,0.046,0.034,1.6e-05,1.6e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16290000,0.98,-0.0075,-0.011,0.19,-0.001,-0.0065,0.023,0.0026,-0.004,0.017,-0.0012,-0.0061,1.7e-05,0.0048,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.043,0.043,0.0077,0.053,0.053,0.034,1.6e-05,1.6e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16390000,0.98,-0.0074,-0.011,0.19,0.0015,-0.0057,0.023,0.0036,-0.003,0.017,-0.0012,-0.0061,1.7e-05,0.0053,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.038,0.038,0.0077,0.046,0.046,0.034,1.5e-05,1.5e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16490000,0.98,-0.0075,-0.011,0.19,0.0034,-0.0074,0.026,0.0039,-0.0037,0.021,-0.0012,-0.0061,1.7e-05,0.0052,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.043,0.043,0.0079,0.053,0.053,0.034,1.5e-05,1.5e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16590000,0.98,-0.0075,-0.011,0.19,0.0075,-0.0073,0.029,0.0034,-0.0029,0.021,-0.0012,-0.0061,1.8e-05,0.0054,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00016,0.0045,0.037,0.037,0.0079,0.046,0.046,0.034,1.3e-05,1.3e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16690000,0.98,-0.0076,-0.011,0.19,0.009,-0.012,0.029,0.0042,-0.0038,0.022,-0.0012,-0.0061,1.8e-05,0.0055,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00016,0.0045,0.042,0.042,0.008,0.053,0.053,0.034,1.3e-05,1.3e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16790000,0.98,-0.0074,-0.011,0.19,0.0098,-0.011,0.028,0.0033,-0.0028,0.022,-0.0012,-0.0061,1.8e-05,0.0056,0.0025,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00016,0.0045,0.037,0.037,0.008,0.046,0.046,0.034,1.2e-05,1.2e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +16890000,0.98,-0.0074,-0.011,0.19,0.0089,-0.011,0.029,0.0043,-0.0038,0.02,-0.0012,-0.0061,1.8e-05,0.0059,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00016,0.0045,0.041,0.041,0.0082,0.052,0.052,0.034,1.2e-05,1.2e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +16990000,0.98,-0.0074,-0.011,0.19,0.0086,-0.011,0.029,0.004,-0.0028,0.019,-0.0013,-0.0061,1.9e-05,0.0065,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.036,0.036,0.0082,0.046,0.046,0.034,1.1e-05,1.1e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17090000,0.98,-0.0075,-0.011,0.19,0.01,-0.013,0.028,0.005,-0.0041,0.018,-0.0013,-0.0061,1.9e-05,0.0067,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00015,0.0045,0.04,0.041,0.0083,0.052,0.052,0.034,1.1e-05,1.1e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17190000,0.98,-0.0076,-0.011,0.19,0.0089,-0.018,0.03,0.0033,-0.0075,0.021,-0.0012,-0.0061,1.9e-05,0.0065,0.0019,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.036,0.036,0.0083,0.046,0.046,0.034,9.9e-06,9.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17290000,0.98,-0.0076,-0.011,0.19,0.0099,-0.019,0.03,0.0043,-0.0094,0.021,-0.0012,-0.0061,1.9e-05,0.0067,0.0017,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.04,0.04,0.0084,0.052,0.052,0.034,9.9e-06,9.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17390000,0.98,-0.0074,-0.011,0.19,0.0066,-0.018,0.029,0.0057,-0.0059,0.021,-0.0013,-0.006,2e-05,0.0075,0.0023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.035,0.035,0.0084,0.046,0.046,0.034,8.9e-06,8.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17490000,0.98,-0.0074,-0.011,0.19,0.0047,-0.019,0.029,0.0062,-0.0078,0.022,-0.0013,-0.006,2e-05,0.0075,0.0022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.039,0.039,0.0085,0.052,0.052,0.034,8.9e-06,8.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17590000,0.98,-0.0073,-0.011,0.19,0.00081,-0.015,0.028,0.0024,-0.0058,0.02,-0.0013,-0.0061,2.1e-05,0.0074,0.0027,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.034,0.034,0.0085,0.046,0.046,0.034,8.1e-06,8.1e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17690000,0.98,-0.0074,-0.011,0.19,-0.00014,-0.016,0.029,0.0025,-0.0074,0.023,-0.0013,-0.0061,2.1e-05,0.0075,0.0027,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.038,0.038,0.0086,0.052,0.052,0.034,8.1e-06,8.1e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17790000,0.98,-0.0074,-0.011,0.19,0.0025,-0.014,0.029,0.0035,-0.0062,0.028,-0.0014,-0.006,2.2e-05,0.0078,0.0033,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.034,0.034,0.0086,0.046,0.046,0.034,7.3e-06,7.3e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17890000,0.98,-0.0073,-0.011,0.19,0.0046,-0.016,0.029,0.0039,-0.0077,0.032,-0.0014,-0.006,2.2e-05,0.0077,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.037,0.037,0.0086,0.052,0.052,0.035,7.3e-06,7.3e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17990000,0.98,-0.0071,-0.011,0.19,0.0041,-0.0093,0.029,0.0031,-0.002,0.033,-0.0014,-0.006,2.4e-05,0.0081,0.0044,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0045,0.033,0.033,0.0086,0.045,0.045,0.034,6.6e-06,6.6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18090000,0.98,-0.0072,-0.011,0.19,0.0036,-0.0098,0.028,0.0035,-0.0029,0.031,-0.0014,-0.006,2.4e-05,0.0084,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0045,0.036,0.036,0.0087,0.052,0.052,0.035,6.6e-06,6.6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18190000,0.98,-0.0072,-0.011,0.19,0.0036,-0.0088,0.028,0.0041,-0.0022,0.029,-0.0014,-0.006,2.4e-05,0.0089,0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0045,0.032,0.032,0.0086,0.045,0.045,0.035,6e-06,6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18290000,0.98,-0.0072,-0.011,0.19,0.0045,-0.0093,0.027,0.0045,-0.0031,0.027,-0.0014,-0.006,2.4e-05,0.0092,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0046,0.036,0.036,0.0087,0.051,0.051,0.035,6e-06,6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18390000,0.98,-0.0071,-0.011,0.19,0.0054,-0.008,0.027,0.0061,-0.0023,0.026,-0.0014,-0.006,2.4e-05,0.0098,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00012,0.0046,0.031,0.031,0.0086,0.045,0.045,0.035,5.4e-06,5.4e-06,6.3e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 +18490000,0.98,-0.0072,-0.011,0.19,0.0081,-0.008,0.026,0.0069,-0.0031,0.028,-0.0014,-0.006,2.4e-05,0.0098,0.0038,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0046,0.035,0.035,0.0087,0.051,0.051,0.035,5.4e-06,5.4e-06,6.3e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 +18590000,0.98,-0.007,-0.011,0.19,0.0065,-0.0073,0.026,0.0055,-0.0024,0.031,-0.0015,-0.006,2.5e-05,0.0097,0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.031,0.031,0.0087,0.045,0.045,0.035,4.9e-06,4.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18690000,0.98,-0.007,-0.011,0.19,0.0066,-0.0063,0.024,0.0062,-0.0031,0.029,-0.0015,-0.006,2.5e-05,0.0099,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.034,0.034,0.0087,0.051,0.051,0.035,4.9e-06,4.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18790000,0.98,-0.007,-0.011,0.19,0.0056,-0.006,0.024,0.0062,-0.0025,0.027,-0.0015,-0.006,2.5e-05,0.01,0.0038,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.03,0.03,0.0087,0.045,0.045,0.035,4.4e-06,4.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18890000,0.98,-0.0069,-0.011,0.19,0.0043,-0.0056,0.021,0.0067,-0.0031,0.023,-0.0015,-0.006,2.5e-05,0.011,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.033,0.033,0.0087,0.051,0.051,0.035,4.4e-06,4.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18990000,0.98,-0.0069,-0.011,0.19,0.0026,-0.0057,0.022,0.0055,-0.0025,0.026,-0.0015,-0.006,2.5e-05,0.011,0.0037,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.029,0.029,0.0086,0.045,0.045,0.035,4e-06,4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19090000,0.98,-0.007,-0.011,0.19,0.00059,-0.0062,0.023,0.0057,-0.0031,0.022,-0.0015,-0.006,2.5e-05,0.011,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.032,0.032,0.0087,0.051,0.051,0.036,4e-06,4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19190000,0.98,-0.0069,-0.011,0.19,-0.00089,-0.0059,0.022,0.0048,-0.0025,0.021,-0.0015,-0.006,2.5e-05,0.011,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.028,0.028,0.0086,0.045,0.045,0.036,3.7e-06,3.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19290000,0.98,-0.0068,-0.011,0.19,-0.0018,-0.0057,0.023,0.0047,-0.0031,0.021,-0.0015,-0.006,2.5e-05,0.011,0.0033,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00011,0.0046,0.031,0.031,0.0087,0.05,0.051,0.036,3.7e-06,3.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19390000,0.98,-0.0069,-0.011,0.19,-0.0022,-0.0022,0.024,0.004,-0.0012,0.019,-0.0015,-0.006,2.6e-05,0.011,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.028,0.028,0.0086,0.045,0.045,0.036,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19490000,0.98,-0.0069,-0.011,0.19,-0.003,-0.0022,0.023,0.0038,-0.0014,0.019,-0.0015,-0.006,2.6e-05,0.012,0.0033,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.03,0.03,0.0087,0.05,0.05,0.036,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19590000,0.98,-0.0069,-0.011,0.19,-0.0041,-0.0052,0.025,0.0044,-0.0024,0.019,-0.0015,-0.006,2.6e-05,0.012,0.003,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.027,0.027,0.0086,0.044,0.044,0.036,3e-06,3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19690000,0.98,-0.0069,-0.011,0.19,-0.0058,-0.0037,0.023,0.0039,-0.0028,0.019,-0.0015,-0.006,2.6e-05,0.012,0.0029,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.03,0.03,0.0086,0.05,0.05,0.036,3e-06,3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19790000,0.98,-0.007,-0.011,0.19,-0.0058,-0.0023,0.022,0.0063,-0.0023,0.014,-0.0015,-0.006,2.6e-05,0.013,0.0026,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.026,0.026,0.0086,0.044,0.044,0.036,2.8e-06,2.8e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 +19890000,0.98,-0.007,-0.011,0.19,-0.0058,-0.002,0.022,0.0057,-0.0025,0.013,-0.0015,-0.006,2.6e-05,0.013,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.029,0.029,0.0086,0.05,0.05,0.036,2.8e-06,2.8e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 +19990000,0.98,-0.0071,-0.011,0.19,-0.0055,-0.0019,0.019,0.0061,-0.00091,0.0097,-0.0015,-0.0059,2.6e-05,0.013,0.0023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.0001,0.0046,0.026,0.026,0.0085,0.044,0.044,0.036,2.5e-06,2.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20090000,0.98,-0.0071,-0.011,0.19,-0.005,-0.0042,0.019,0.0055,-0.0012,0.013,-0.0015,-0.0059,2.6e-05,0.013,0.0024,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.0001,0.0046,0.028,0.028,0.0086,0.05,0.05,0.036,2.5e-06,2.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20190000,0.98,-0.0071,-0.011,0.19,-0.004,-0.0016,0.02,0.0065,-0.0009,0.013,-0.0015,-0.0059,2.6e-05,0.014,0.0023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.025,0.025,0.0085,0.044,0.044,0.036,2.3e-06,2.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20290000,0.98,-0.0071,-0.011,0.19,-0.0072,-0.0027,0.02,0.006,-0.0011,0.013,-0.0015,-0.0059,2.6e-05,0.014,0.0023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.027,0.027,0.0085,0.049,0.049,0.036,2.3e-06,2.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20390000,0.98,-0.007,-0.011,0.19,-0.0079,-0.0015,0.021,0.0069,-0.00077,0.014,-0.0015,-0.0059,2.7e-05,0.014,0.0022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.024,0.024,0.0084,0.044,0.044,0.036,2.1e-06,2.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20490000,0.98,-0.007,-0.011,0.19,-0.012,-0.0025,0.02,0.0059,-0.00095,0.012,-0.0015,-0.0059,2.7e-05,0.014,0.0021,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.027,0.027,0.0085,0.049,0.049,0.036,2.1e-06,2.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20590000,0.98,-0.007,-0.011,0.19,-0.012,-0.0034,0.02,0.0069,-0.00079,0.011,-0.0015,-0.0059,2.7e-05,0.014,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,9.9e-05,0.0046,0.024,0.024,0.0084,0.044,0.044,0.036,2e-06,2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20690000,0.98,-0.0069,-0.011,0.19,-0.013,-0.0022,0.021,0.0057,-0.001,0.011,-0.0015,-0.0059,2.7e-05,0.014,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,9.9e-05,0.0046,0.026,0.026,0.0084,0.049,0.049,0.036,2e-06,2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20790000,0.98,-0.0063,-0.011,0.19,-0.016,0.00036,0.0055,0.0048,-0.00081,0.0097,-0.0015,-0.0059,2.7e-05,0.015,0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.9e-05,9.7e-05,0.0047,0.023,0.023,0.0084,0.043,0.043,0.036,1.8e-06,1.8e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20890000,0.98,0.0028,-0.0073,0.19,-0.022,0.012,-0.11,0.0029,-0.00014,0.0034,-0.0015,-0.0059,2.7e-05,0.015,0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.9e-05,9.8e-05,0.0047,0.026,0.026,0.0084,0.049,0.049,0.036,1.8e-06,1.8e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20990000,0.98,0.0061,-0.0038,0.19,-0.032,0.03,-0.25,0.0022,0.00051,-0.011,-0.0015,-0.0059,2.7e-05,0.015,0.0016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.7e-05,9.6e-05,0.0047,0.023,0.023,0.0083,0.043,0.043,0.036,1.7e-06,1.7e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21090000,0.98,0.0045,-0.0042,0.19,-0.046,0.046,-0.37,-0.0017,0.0044,-0.042,-0.0015,-0.0059,2.7e-05,0.015,0.0016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.8e-05,9.6e-05,0.0047,0.026,0.026,0.0084,0.048,0.048,0.036,1.7e-06,1.7e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21190000,0.98,0.0017,-0.0059,0.19,-0.049,0.05,-0.5,-0.0013,0.0035,-0.078,-0.0014,-0.0059,2.6e-05,0.015,0.00095,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.6e-05,9.4e-05,0.0047,0.023,0.023,0.0083,0.043,0.043,0.036,1.6e-06,1.6e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21290000,0.98,-0.00051,-0.0072,0.19,-0.05,0.054,-0.63,-0.0062,0.0087,-0.14,-0.0014,-0.0059,2.6e-05,0.015,0.00088,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.6e-05,9.5e-05,0.0047,0.026,0.026,0.0083,0.048,0.048,0.036,1.6e-06,1.6e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21390000,0.98,-0.002,-0.0079,0.19,-0.045,0.05,-0.75,-0.0051,0.011,-0.2,-0.0014,-0.0059,2.6e-05,0.015,0.00056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.4e-05,9.2e-05,0.0047,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0094,0.016,-0.29,-0.0014,-0.0059,2.6e-05,0.015,0.00046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.5e-05,9.3e-05,0.0047,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21590000,0.98,-0.0033,-0.0083,0.19,-0.032,0.043,-1,-0.008,0.016,-0.38,-0.0014,-0.0059,2.6e-05,0.015,0.00042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.2e-05,9.1e-05,0.0047,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21690000,0.98,-0.0036,-0.0082,0.19,-0.03,0.039,-1.1,-0.011,0.021,-0.49,-0.0014,-0.0059,2.6e-05,0.015,0.00025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.3e-05,9.1e-05,0.0047,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.032,-1.3,-0.0039,0.018,-0.61,-0.0014,-0.0058,2.7e-05,0.016,5.4e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9e-05,8.9e-05,0.0047,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-06,1.2e-06,6.3e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 +21890000,0.98,-0.0043,-0.0086,0.19,-0.018,0.028,-1.4,-0.0058,0.021,-0.75,-0.0014,-0.0058,2.7e-05,0.016,-7.9e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,9.1e-05,8.9e-05,0.0047,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-06,1.2e-06,6.3e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 +21990000,0.98,-0.005,-0.0088,0.19,-0.014,0.023,-1.4,-0.00042,0.017,-0.89,-0.0014,-0.0058,2.6e-05,0.016,0.00016,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.8e-05,8.7e-05,0.0047,0.022,0.022,0.0082,0.043,0.043,0.036,1.1e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22090000,0.98,-0.0057,-0.0097,0.19,-0.012,0.019,-1.4,-0.0018,0.019,-1,-0.0014,-0.0058,2.6e-05,0.016,4.1e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.9e-05,8.7e-05,0.0047,0.024,0.024,0.0082,0.048,0.048,0.036,1.2e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22190000,0.98,-0.0062,-0.0099,0.19,-0.0036,0.013,-1.4,0.006,0.013,-1.2,-0.0014,-0.0058,2.6e-05,0.016,7.9e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.7e-05,8.5e-05,0.0047,0.021,0.021,0.0081,0.043,0.043,0.036,1.1e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22290000,0.98,-0.0069,-0.01,0.19,0.0016,0.0079,-1.4,0.0059,0.014,-1.3,-0.0014,-0.0058,2.6e-05,0.016,-2.2e-05,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.7e-05,8.6e-05,0.0047,0.023,0.023,0.0081,0.048,0.048,0.036,1.1e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22390000,0.98,-0.0072,-0.01,0.19,0.0066,-0.0017,-1.4,0.013,0.0041,-1.5,-0.0014,-0.0058,2.6e-05,0.016,-0.00037,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.5e-05,8.4e-05,0.0047,0.021,0.021,0.0081,0.043,0.043,0.036,1e-06,1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22490000,0.98,-0.0074,-0.011,0.19,0.011,-0.0076,-1.4,0.014,0.0036,-1.6,-0.0014,-0.0058,2.6e-05,0.016,-0.00042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.6e-05,8.4e-05,0.0047,0.022,0.022,0.0081,0.047,0.047,0.036,1e-06,1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22590000,0.98,-0.0073,-0.011,0.19,0.02,-0.017,-1.4,0.026,-0.0053,-1.7,-0.0014,-0.0058,2.6e-05,0.016,-0.00058,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.4e-05,8.2e-05,0.0047,0.02,0.02,0.0081,0.042,0.042,0.036,9.4e-07,9.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22690000,0.98,-0.0072,-0.012,0.19,0.022,-0.021,-1.4,0.028,-0.0072,-1.9,-0.0014,-0.0058,2.6e-05,0.016,-0.00065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.4e-05,8.3e-05,0.0047,0.021,0.021,0.0081,0.047,0.047,0.036,9.5e-07,9.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22790000,0.98,-0.0072,-0.012,0.19,0.028,-0.029,-1.4,0.038,-0.017,-2,-0.0014,-0.0058,2.6e-05,0.017,-0.00077,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0047,0.019,0.019,0.0081,0.042,0.042,0.036,8.9e-07,8.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22890000,0.98,-0.0074,-0.012,0.19,0.032,-0.034,-1.4,0.041,-0.02,-2.2,-0.0014,-0.0058,2.6e-05,0.017,-0.00085,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0047,0.021,0.021,0.0081,0.047,0.047,0.036,8.9e-07,8.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22990000,0.98,-0.0073,-0.013,0.19,0.036,-0.039,-1.4,0.051,-0.031,-2.3,-0.0014,-0.0058,2.6e-05,0.017,-0.00098,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0047,0.019,0.019,0.0081,0.042,0.042,0.036,8.4e-07,8.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23090000,0.98,-0.0074,-0.013,0.19,0.041,-0.044,-1.4,0.055,-0.035,-2.5,-0.0014,-0.0058,2.6e-05,0.017,-0.001,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0047,0.02,0.02,0.0081,0.046,0.046,0.036,8.4e-07,8.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23190000,0.98,-0.0074,-0.013,0.19,0.047,-0.046,-1.4,0.066,-0.045,-2.6,-0.0014,-0.0058,2.7e-05,0.017,-0.0012,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.9e-05,0.0048,0.018,0.018,0.008,0.042,0.042,0.035,8e-07,8e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23290000,0.98,-0.0079,-0.013,0.19,0.052,-0.051,-1.4,0.071,-0.05,-2.8,-0.0014,-0.0058,2.7e-05,0.017,-0.0012,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.9e-05,0.0048,0.02,0.02,0.0081,0.046,0.046,0.036,8e-07,8e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23390000,0.98,-0.0078,-0.014,0.19,0.057,-0.053,-1.4,0.082,-0.055,-2.9,-0.0014,-0.0058,2.6e-05,0.017,-0.00096,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8e-05,7.8e-05,0.0048,0.018,0.018,0.008,0.041,0.041,0.036,7.6e-07,7.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23490000,0.98,-0.0079,-0.014,0.19,0.062,-0.055,-1.4,0.088,-0.061,-3,-0.0014,-0.0058,2.6e-05,0.017,-0.001,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.9e-05,0.0048,0.019,0.019,0.0081,0.046,0.046,0.036,7.6e-07,7.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23590000,0.98,-0.0081,-0.014,0.19,0.064,-0.058,-1.4,0.095,-0.07,-3.2,-0.0014,-0.0058,2.7e-05,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8e-05,7.8e-05,0.0048,0.017,0.017,0.008,0.041,0.041,0.035,7.2e-07,7.2e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23690000,0.98,-0.0088,-0.014,0.19,0.062,-0.06,-1.3,0.1,-0.076,-3.3,-0.0014,-0.0058,2.7e-05,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0048,0.018,0.018,0.0081,0.046,0.046,0.036,7.2e-07,7.2e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23790000,0.98,-0.011,-0.017,0.19,0.057,-0.058,-0.96,0.11,-0.081,-3.4,-0.0014,-0.0058,2.7e-05,0.017,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.7e-05,0.0048,0.016,0.016,0.008,0.041,0.041,0.035,6.9e-07,6.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23890000,0.98,-0.014,-0.021,0.19,0.053,-0.058,-0.52,0.12,-0.087,-3.5,-0.0014,-0.0058,2.7e-05,0.017,-0.0012,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.8e-05,0.0048,0.017,0.017,0.008,0.045,0.045,0.035,6.9e-07,6.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23990000,0.98,-0.016,-0.024,0.19,0.054,-0.058,-0.14,0.13,-0.089,-3.6,-0.0014,-0.0058,2.7e-05,0.018,-0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.8e-05,0.0048,0.015,0.015,0.008,0.041,0.041,0.036,6.6e-07,6.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24090000,0.98,-0.016,-0.023,0.19,0.061,-0.066,0.09,0.13,-0.095,-3.6,-0.0014,-0.0058,2.7e-05,0.018,-0.0017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.8e-05,0.0048,0.017,0.017,0.0081,0.045,0.045,0.036,6.6e-07,6.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24190000,0.98,-0.013,-0.019,0.19,0.072,-0.071,0.077,0.14,-0.1,-3.6,-0.0014,-0.0058,2.7e-05,0.019,-0.0024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.04,0.04,0.035,6.3e-07,6.3e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24290000,0.98,-0.01,-0.016,0.19,0.076,-0.074,0.055,0.15,-0.11,-3.6,-0.0014,-0.0058,2.7e-05,0.019,-0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.016,0.016,0.0081,0.044,0.044,0.036,6.4e-07,6.3e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24390000,0.98,-0.0095,-0.015,0.19,0.069,-0.069,0.071,0.15,-0.11,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.0032,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.04,0.04,0.035,6.1e-07,6.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24490000,0.98,-0.0098,-0.015,0.19,0.065,-0.066,0.069,0.16,-0.11,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.0032,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.016,0.016,0.008,0.044,0.044,0.035,6.1e-07,6.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24590000,0.98,-0.01,-0.015,0.19,0.061,-0.062,0.065,0.16,-0.11,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.0039,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.04,0.04,0.036,5.9e-07,5.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24690000,0.98,-0.011,-0.015,0.19,0.059,-0.061,0.064,0.17,-0.12,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.0039,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.016,0.016,0.0081,0.044,0.044,0.036,5.9e-07,5.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24790000,0.98,-0.011,-0.014,0.19,0.056,-0.059,0.056,0.17,-0.11,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0045,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.039,0.039,0.035,5.7e-07,5.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24890000,0.98,-0.011,-0.013,0.19,0.054,-0.062,0.045,0.18,-0.12,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0045,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.016,0.016,0.008,0.043,0.043,0.035,5.7e-07,5.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24990000,0.98,-0.011,-0.013,0.19,0.046,-0.058,0.038,0.18,-0.11,-3.6,-0.0015,-0.0058,2.5e-05,0.019,-0.0052,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.015,0.015,0.008,0.039,0.039,0.035,5.5e-07,5.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +25090000,0.98,-0.011,-0.013,0.19,0.042,-0.057,0.035,0.18,-0.12,-3.6,-0.0015,-0.0058,2.5e-05,0.019,-0.0053,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.016,0.016,0.0081,0.043,0.043,0.035,5.5e-07,5.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +25190000,0.98,-0.011,-0.013,0.19,0.037,-0.05,0.035,0.18,-0.11,-3.6,-0.0015,-0.0058,2.5e-05,0.019,-0.0057,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.015,0.015,0.008,0.039,0.039,0.035,5.3e-07,5.3e-07,6.3e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25290000,0.98,-0.011,-0.012,0.19,0.032,-0.052,0.029,0.18,-0.11,-3.6,-0.0015,-0.0058,2.5e-05,0.019,-0.0057,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.016,0.016,0.0081,0.043,0.043,0.036,5.3e-07,5.3e-07,6.3e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25390000,0.98,-0.011,-0.012,0.19,0.024,-0.044,0.028,0.18,-0.1,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0064,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.015,0.015,0.008,0.039,0.039,0.035,5.2e-07,5.1e-07,6.3e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25490000,0.98,-0.011,-0.012,0.19,0.019,-0.044,0.027,0.18,-0.11,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0064,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.016,0.016,0.008,0.043,0.043,0.035,5.2e-07,5.2e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25590000,0.98,-0.011,-0.012,0.19,0.014,-0.04,0.028,0.18,-0.098,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0067,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,5e-07,5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25690000,0.98,-0.011,-0.011,0.19,0.013,-0.039,0.017,0.18,-0.1,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0068,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.015,0.015,0.0081,0.043,0.043,0.035,5e-07,5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25790000,0.98,-0.011,-0.011,0.19,0.0021,-0.031,0.017,0.17,-0.093,-3.6,-0.0016,-0.0058,2.7e-05,0.019,-0.0072,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,4.9e-07,4.8e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25890000,0.98,-0.011,-0.011,0.19,-0.0036,-0.029,0.019,0.17,-0.095,-3.6,-0.0016,-0.0058,2.7e-05,0.019,-0.0072,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.043,0.043,0.036,4.9e-07,4.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25990000,0.98,-0.011,-0.011,0.19,-0.013,-0.022,0.013,0.16,-0.086,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0077,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,4.7e-07,4.7e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26090000,0.98,-0.01,-0.011,0.19,-0.018,-0.022,0.011,0.16,-0.088,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0077,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.035,4.7e-07,4.7e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26190000,0.98,-0.01,-0.011,0.19,-0.024,-0.015,0.0063,0.15,-0.081,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0079,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,4.6e-07,4.6e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26290000,0.98,-0.01,-0.012,0.19,-0.026,-0.014,0.00052,0.15,-0.083,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0079,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.036,4.6e-07,4.6e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26390000,0.98,-0.0098,-0.012,0.19,-0.032,-0.0067,0.0045,0.13,-0.075,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0082,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.5e-07,4.5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26490000,0.98,-0.0095,-0.011,0.19,-0.035,-0.0036,0.014,0.13,-0.075,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0081,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.035,4.5e-07,4.5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26590000,0.98,-0.009,-0.012,0.19,-0.036,0.0044,0.014,0.12,-0.068,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.4e-07,4.3e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26690000,0.98,-0.0088,-0.011,0.19,-0.038,0.0095,0.013,0.12,-0.067,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.035,4.4e-07,4.4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26790000,0.98,-0.0086,-0.011,0.19,-0.046,0.013,0.012,0.1,-0.062,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0085,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.3e-07,4.2e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26890000,0.98,-0.008,-0.011,0.19,-0.051,0.016,0.007,0.1,-0.06,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.036,4.3e-07,4.2e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26990000,0.98,-0.0075,-0.011,0.19,-0.058,0.023,0.0061,0.088,-0.054,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0088,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.2e-07,4.1e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27090000,0.98,-0.0073,-0.012,0.19,-0.06,0.03,0.0089,0.082,-0.052,-3.6,-0.0016,-0.0058,2.8e-05,0.02,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.005,0.015,0.015,0.0081,0.042,0.042,0.035,4.2e-07,4.1e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27190000,0.98,-0.0073,-0.012,0.19,-0.066,0.035,0.011,0.071,-0.046,-3.6,-0.0016,-0.0058,2.8e-05,0.02,-0.0091,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.005,0.014,0.014,0.008,0.038,0.038,0.035,4.1e-07,4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27290000,0.98,-0.0075,-0.013,0.19,-0.073,0.041,0.12,0.064,-0.042,-3.6,-0.0016,-0.0058,2.8e-05,0.02,-0.0091,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.005,0.015,0.015,0.0081,0.042,0.042,0.035,4.1e-07,4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27390000,0.98,-0.0089,-0.015,0.19,-0.078,0.047,0.45,0.055,-0.035,-3.5,-0.0016,-0.0058,3e-05,0.02,-0.0094,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,7.9e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,4e-07,3.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27490000,0.98,-0.01,-0.017,0.19,-0.082,0.053,0.76,0.047,-0.03,-3.5,-0.0016,-0.0058,3e-05,0.02,-0.0095,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.008,0.042,0.042,0.035,4e-07,4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27590000,0.98,-0.01,-0.016,0.19,-0.076,0.055,0.85,0.038,-0.026,-3.4,-0.0016,-0.0058,3e-05,0.02,-0.0096,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-07,3.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27690000,0.98,-0.0089,-0.013,0.19,-0.072,0.052,0.76,0.031,-0.02,-3.3,-0.0016,-0.0058,3e-05,0.02,-0.0097,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-07,3.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27790000,0.98,-0.0076,-0.011,0.19,-0.071,0.049,0.75,0.025,-0.018,-3.3,-0.0016,-0.0058,3e-05,0.02,-0.0091,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.8e-07,3.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +27890000,0.98,-0.0072,-0.012,0.19,-0.079,0.056,0.79,0.017,-0.012,-3.2,-0.0016,-0.0058,3e-05,0.02,-0.0092,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.036,3.8e-07,3.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +27990000,0.98,-0.0077,-0.012,0.19,-0.079,0.058,0.78,0.012,-0.011,-3.1,-0.0016,-0.0058,3.1e-05,0.02,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28090000,0.98,-0.0079,-0.012,0.19,-0.082,0.059,0.78,0.0041,-0.0049,-3,-0.0016,-0.0058,3.1e-05,0.02,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.008,0.041,0.041,0.035,3.8e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28190000,0.98,-0.0074,-0.012,0.19,-0.083,0.055,0.79,-0.0024,-0.0044,-3,-0.0016,-0.0058,3.1e-05,0.02,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28290000,0.98,-0.0069,-0.012,0.19,-0.088,0.059,0.79,-0.011,0.0013,-2.9,-0.0016,-0.0058,3.1e-05,0.02,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.035,3.7e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28390000,0.98,-0.0069,-0.013,0.19,-0.088,0.061,0.79,-0.015,0.0043,-2.8,-0.0015,-0.0058,3.3e-05,0.02,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-07,3.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28490000,0.98,-0.0072,-0.014,0.19,-0.09,0.065,0.79,-0.024,0.011,-2.8,-0.0015,-0.0058,3.3e-05,0.02,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.036,3.6e-07,3.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28590000,0.98,-0.0072,-0.014,0.19,-0.083,0.06,0.79,-0.028,0.0083,-2.7,-0.0015,-0.0058,3.3e-05,0.02,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28690000,0.98,-0.007,-0.013,0.19,-0.083,0.061,0.79,-0.036,0.014,-2.6,-0.0015,-0.0058,3.3e-05,0.02,-0.0085,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.008,0.041,0.041,0.035,3.6e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28790000,0.98,-0.0064,-0.013,0.19,-0.08,0.061,0.79,-0.038,0.016,-2.5,-0.0015,-0.0058,3.5e-05,0.02,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.013,0.013,0.008,0.037,0.037,0.035,3.5e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28890000,0.98,-0.0062,-0.012,0.19,-0.084,0.064,0.78,-0.046,0.022,-2.5,-0.0015,-0.0058,3.5e-05,0.02,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.036,3.5e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28990000,0.98,-0.006,-0.013,0.19,-0.08,0.06,0.78,-0.046,0.022,-2.4,-0.0015,-0.0058,3.7e-05,0.02,-0.0088,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29090000,0.98,-0.0058,-0.013,0.19,-0.082,0.062,0.78,-0.054,0.028,-2.3,-0.0015,-0.0058,3.7e-05,0.02,-0.0088,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.008,0.041,0.041,0.035,3.5e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29190000,0.98,-0.0058,-0.013,0.19,-0.078,0.061,0.78,-0.051,0.027,-2.2,-0.0015,-0.0058,4e-05,0.02,-0.009,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29290000,0.98,-0.006,-0.013,0.19,-0.08,0.067,0.78,-0.059,0.033,-2.2,-0.0015,-0.0058,4e-05,0.02,-0.009,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29390000,0.98,-0.0065,-0.012,0.19,-0.076,0.065,0.78,-0.057,0.034,-2.1,-0.0015,-0.0058,4.2e-05,0.02,-0.0092,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29490000,0.98,-0.0065,-0.012,0.19,-0.078,0.066,0.78,-0.065,0.041,-2,-0.0015,-0.0058,4.2e-05,0.02,-0.0092,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.0081,0.041,0.041,0.036,3.4e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29590000,0.98,-0.0064,-0.012,0.19,-0.074,0.064,0.78,-0.062,0.04,-1.9,-0.0015,-0.0058,4.4e-05,0.02,-0.0094,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29690000,0.98,-0.0064,-0.012,0.19,-0.078,0.063,0.78,-0.07,0.046,-1.9,-0.0015,-0.0058,4.4e-05,0.02,-0.0095,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29790000,0.98,-0.0063,-0.013,0.19,-0.074,0.056,0.78,-0.065,0.044,-1.8,-0.0014,-0.0058,4.6e-05,0.02,-0.0098,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29890000,0.98,-0.0058,-0.013,0.19,-0.074,0.057,0.77,-0.073,0.049,-1.7,-0.0014,-0.0058,4.6e-05,0.02,-0.0099,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29990000,0.98,-0.0059,-0.013,0.19,-0.069,0.052,0.77,-0.068,0.044,-1.6,-0.0014,-0.0058,4.7e-05,0.02,-0.01,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30090000,0.98,-0.0061,-0.013,0.19,-0.069,0.053,0.77,-0.075,0.05,-1.6,-0.0014,-0.0058,4.7e-05,0.02,-0.01,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30190000,0.98,-0.006,-0.013,0.19,-0.063,0.05,0.77,-0.068,0.048,-1.5,-0.0014,-0.0057,5e-05,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30290000,0.98,-0.0061,-0.013,0.19,-0.062,0.05,0.77,-0.074,0.053,-1.4,-0.0014,-0.0057,5e-05,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30390000,0.98,-0.0061,-0.013,0.19,-0.055,0.044,0.76,-0.066,0.049,-1.4,-0.0014,-0.0057,5.2e-05,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30490000,0.98,-0.0061,-0.013,0.19,-0.058,0.044,0.77,-0.072,0.054,-1.3,-0.0014,-0.0057,5.2e-05,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.036,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30590000,0.98,-0.0064,-0.014,0.19,-0.053,0.041,0.76,-0.065,0.05,-1.2,-0.0014,-0.0057,5.4e-05,0.021,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30690000,0.98,-0.0068,-0.014,0.19,-0.051,0.04,0.76,-0.07,0.054,-1.1,-0.0014,-0.0057,5.4e-05,0.021,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.013,0.008,0.041,0.041,0.035,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30790000,0.98,-0.0065,-0.013,0.19,-0.044,0.035,0.76,-0.063,0.052,-1.1,-0.0014,-0.0057,5.5e-05,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30890000,0.98,-0.0058,-0.013,0.19,-0.044,0.032,0.76,-0.067,0.056,-1,-0.0014,-0.0057,5.5e-05,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.041,0.041,0.035,3.1e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30990000,0.98,-0.006,-0.013,0.19,-0.037,0.026,0.76,-0.057,0.049,-0.94,-0.0014,-0.0057,5.7e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.0079,0.037,0.037,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31090000,0.98,-0.0062,-0.013,0.19,-0.036,0.025,0.76,-0.061,0.051,-0.86,-0.0014,-0.0057,5.7e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.041,0.04,0.036,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31190000,0.98,-0.0064,-0.013,0.19,-0.031,0.02,0.76,-0.052,0.046,-0.79,-0.0014,-0.0057,5.8e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.037,0.037,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31290000,0.98,-0.0066,-0.014,0.19,-0.028,0.018,0.76,-0.055,0.048,-0.72,-0.0014,-0.0057,5.8e-05,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31390000,0.98,-0.0064,-0.013,0.19,-0.022,0.012,0.76,-0.046,0.042,-0.65,-0.0014,-0.0057,5.9e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.0079,0.037,0.037,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31490000,0.98,-0.0061,-0.014,0.19,-0.022,0.0088,0.76,-0.049,0.043,-0.58,-0.0014,-0.0057,5.9e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31590000,0.98,-0.006,-0.014,0.19,-0.018,0.0066,0.76,-0.038,0.039,-0.51,-0.0014,-0.0057,6e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.012,0.012,0.0079,0.037,0.037,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31690000,0.98,-0.006,-0.015,0.19,-0.02,0.0055,0.76,-0.04,0.039,-0.44,-0.0014,-0.0057,6e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31790000,0.98,-0.0062,-0.015,0.19,-0.011,0.0029,0.76,-0.028,0.037,-0.36,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.012,0.012,0.008,0.037,0.037,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31890000,0.98,-0.0059,-0.015,0.19,-0.008,0.00063,0.76,-0.029,0.038,-0.3,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31990000,0.98,-0.0062,-0.015,0.19,-0.00014,-2.6e-05,0.75,-0.017,0.034,-0.23,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32090000,0.98,-0.0066,-0.014,0.19,-0.00068,-0.0034,0.76,-0.017,0.034,-0.16,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32190000,0.98,-0.0067,-0.015,0.19,0.0046,-0.0066,0.76,-0.006,0.033,-0.092,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.2e-05,8e-05,0.0052,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32290000,0.98,-0.0066,-0.015,0.19,0.0063,-0.0094,0.75,-0.0055,0.032,-0.024,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32390000,0.98,-0.0068,-0.015,0.19,0.012,-0.011,0.75,0.0057,0.03,0.051,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.2e-05,8e-05,0.0053,0.012,0.012,0.008,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32490000,0.98,-0.0096,-0.013,0.19,0.039,-0.074,-0.12,0.0089,0.023,0.054,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.2e-05,8e-05,0.0053,0.015,0.015,0.0078,0.04,0.04,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32590000,0.98,-0.0096,-0.013,0.19,0.04,-0.075,-0.12,0.021,0.02,0.035,-0.0014,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.1e-05,7.9e-05,0.0053,0.016,0.016,0.0075,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32690000,0.98,-0.0095,-0.013,0.19,0.035,-0.08,-0.12,0.025,0.012,0.02,-0.0014,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.1e-05,7.9e-05,0.0053,0.019,0.019,0.0074,0.041,0.041,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32790000,0.98,-0.0092,-0.013,0.19,0.034,-0.078,-0.12,0.034,0.01,0.0044,-0.0014,-0.0056,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.8e-05,7.6e-05,0.0053,0.02,0.02,0.0071,0.037,0.037,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32890000,0.98,-0.0092,-0.013,0.19,0.033,-0.084,-0.13,0.038,0.0019,-0.011,-0.0014,-0.0056,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.8e-05,7.7e-05,0.0053,0.023,0.024,0.007,0.041,0.041,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32990000,0.98,-0.0089,-0.013,0.19,0.031,-0.08,-0.13,0.045,-0.0012,-0.024,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.4e-05,7.2e-05,0.0053,0.024,0.024,0.0067,0.038,0.038,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33090000,0.98,-0.0089,-0.013,0.19,0.026,-0.084,-0.12,0.048,-0.0093,-0.031,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.4e-05,7.2e-05,0.0053,0.029,0.029,0.0066,0.042,0.042,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33190000,0.98,-0.0086,-0.013,0.19,0.022,-0.079,-0.12,0.054,-0.011,-0.037,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.8e-05,6.7e-05,0.0053,0.029,0.029,0.0064,0.038,0.038,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33290000,0.98,-0.0086,-0.013,0.19,0.019,-0.08,-0.12,0.056,-0.019,-0.045,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.8e-05,6.7e-05,0.0053,0.035,0.035,0.0063,0.043,0.043,0.034,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33390000,0.98,-0.0081,-0.013,0.19,0.014,-0.066,-0.12,0.059,-0.014,-0.053,-0.0014,-0.0056,6.4e-05,0.025,-0.017,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.1e-05,6e-05,0.0053,0.035,0.035,0.0062,0.039,0.039,0.034,2.6e-07,2.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33490000,0.98,-0.0081,-0.013,0.19,0.0096,-0.067,-0.12,0.06,-0.021,-0.063,-0.0014,-0.0056,6.4e-05,0.025,-0.017,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.2e-05,6e-05,0.0053,0.042,0.042,0.0061,0.044,0.044,0.034,2.7e-07,2.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33590000,0.98,-0.0077,-0.013,0.19,0.0057,-0.058,-0.11,0.063,-0.017,-0.069,-0.0014,-0.0056,6.4e-05,0.024,-0.019,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,5.4e-05,5.3e-05,0.0053,0.04,0.041,0.006,0.04,0.04,0.034,2.6e-07,2.6e-07,6.4e-06,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 +33690000,0.98,-0.0077,-0.013,0.19,0.001,-0.058,-0.11,0.063,-0.023,-0.077,-0.0014,-0.0056,6.4e-05,0.024,-0.019,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,5.5e-05,5.3e-05,0.0053,0.048,0.048,0.0059,0.046,0.046,0.034,2.6e-07,2.6e-07,6.4e-06,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 +33790000,0.98,-0.0075,-0.013,0.19,-0.0022,-0.048,-0.11,0.067,-0.018,-0.083,-0.0014,-0.0056,6.4e-05,0.023,-0.022,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.8e-05,4.7e-05,0.0053,0.045,0.045,0.0059,0.041,0.041,0.034,2.6e-07,2.6e-07,6.4e-06,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 +33890000,0.98,-0.0075,-0.013,0.19,-0.0064,-0.045,-0.11,0.066,-0.023,-0.089,-0.0014,-0.0056,6.4e-05,0.023,-0.022,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.8e-05,4.7e-05,0.0053,0.052,0.053,0.0058,0.047,0.047,0.033,2.6e-07,2.6e-07,6.4e-06,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 +33990000,0.98,-0.0072,-0.013,0.19,-0.006,-0.031,-0.1,0.069,-0.015,-0.092,-0.0014,-0.0056,6.5e-05,0.02,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.2e-05,4.1e-05,0.0054,0.047,0.047,0.0058,0.042,0.042,0.033,2.6e-07,2.6e-07,6.4e-06,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 +34090000,0.98,-0.0072,-0.013,0.19,-0.01,-0.031,-0.1,0.069,-0.018,-0.096,-0.0014,-0.0056,6.5e-05,0.02,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.2e-05,4.1e-05,0.0054,0.054,0.055,0.0058,0.049,0.049,0.033,2.6e-07,2.6e-07,6.4e-06,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 +34190000,0.98,-0.0071,-0.013,0.19,-0.011,-0.02,-0.098,0.072,-0.013,-0.099,-0.0014,-0.0056,6.5e-05,0.019,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.8e-05,3.7e-05,0.0054,0.048,0.048,0.0058,0.043,0.043,0.033,2.6e-07,2.6e-07,6.4e-06,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 +34290000,0.98,-0.0069,-0.013,0.19,-0.011,-0.02,-0.097,0.071,-0.015,-0.1,-0.0014,-0.0056,6.5e-05,0.019,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.8e-05,3.7e-05,0.0054,0.055,0.055,0.0058,0.05,0.05,0.033,2.6e-07,2.6e-07,6.4e-06,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 +34390000,0.98,-0.0069,-0.013,0.19,-0.013,-0.01,-0.092,0.073,-0.01,-0.11,-0.0014,-0.0056,6.5e-05,0.017,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.4e-05,3.3e-05,0.0054,0.047,0.047,0.0058,0.044,0.044,0.033,2.6e-07,2.6e-07,6.4e-06,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 +34490000,0.98,-0.0069,-0.013,0.19,-0.015,-0.0092,-0.089,0.071,-0.011,-0.11,-0.0014,-0.0056,6.5e-05,0.017,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.5e-05,3.3e-05,0.0054,0.053,0.054,0.0059,0.051,0.051,0.032,2.6e-07,2.6e-07,6.4e-06,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 +34590000,0.98,-0.0069,-0.013,0.19,-0.014,-0.0049,0.71,0.073,-0.0088,-0.081,-0.0014,-0.0056,6.5e-05,0.016,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.2e-05,3.1e-05,0.0054,0.044,0.044,0.0059,0.045,0.045,0.032,2.7e-07,2.6e-07,6.4e-06,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 +34690000,0.98,-0.0069,-0.012,0.19,-0.017,-0.0027,1.7,0.071,-0.0092,0.038,-0.0014,-0.0056,6.5e-05,0.016,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.2e-05,3.1e-05,0.0054,0.047,0.048,0.006,0.052,0.052,0.032,2.7e-07,2.6e-07,6.4e-06,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 +34790000,0.98,-0.0068,-0.012,0.19,-0.018,0.002,2.7,0.072,-0.0068,0.21,-0.0014,-0.0056,6.5e-05,0.017,-0.03,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.1e-05,3e-05,0.0054,0.04,0.04,0.0061,0.045,0.045,0.032,2.7e-07,2.7e-07,6.4e-06,0.021,0.021,0.0005,0,0,0,0,0,0,0,0 +34890000,0.98,-0.0068,-0.012,0.19,-0.022,0.0044,3.6,0.07,-0.0063,0.5,-0.0014,-0.0056,6.5e-05,0.018,-0.03,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.1e-05,3e-05,0.0054,0.044,0.044,0.0061,0.052,0.052,0.032,2.7e-07,2.7e-07,6.4e-06,0.021,0.021,0.0005,0,0,0,0,0,0,0,0 diff --git a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt index 58bdcee733..9e9edc1f0e 100644 --- a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt +++ b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 ECL Development Team. All rights reserved. +# Copyright (c) 2019-2023 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 @@ -12,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name ECL nor the names of its contributors may be +# 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. # diff --git a/src/modules/ekf2/test/sensor_simulator/airspeed.h b/src/modules/ekf2/test/sensor_simulator/airspeed.h index 0ac4aa5c89..261aa4fefc 100644 --- a/src/modules/ekf2/test/sensor_simulator/airspeed.h +++ b/src/modules/ekf2/test/sensor_simulator/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/baro.h b/src/modules/ekf2/test/sensor_simulator/baro.h index 018e5d88e0..2847935d45 100644 --- a/src/modules/ekf2/test/sensor_simulator/baro.h +++ b/src/modules/ekf2/test/sensor_simulator/baro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py b/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py index 0e656a684a..1b3dd9dc05 100644 --- a/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py +++ b/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py @@ -22,14 +22,14 @@ def getVioData(ulog: ULog) -> pd.DataFrame: def getOpticalFlowData(ulog: ULog) -> pd.DataFrame: - optical_flow = ulog.get_dataset("optical_flow").data + optical_flow = ulog.get_dataset("vehicle_optical_flow").data flow = pd.DataFrame({'timestamp': optical_flow['timestamp'], 'sensor' : 'flow', - 'pixel_flow_x_integral': optical_flow["pixel_flow_x_integral"], - 'pixel_flow_y_integral': optical_flow["pixel_flow_y_integral"], - 'gyro_x_rate_integral': optical_flow["gyro_x_rate_integral"], - 'gyro_y_rate_integral': optical_flow["gyro_y_rate_integral"], - 'gyro_z_rate_integral': optical_flow["gyro_z_rate_integral"], + 'pixel_flow_x': optical_flow["pixel_flow[0]"], + 'pixel_flow_y': optical_flow["pixel_flow[1]"], + 'delta_angle_x': optical_flow["delta_angle[0]"], + 'delta_angle_y': optical_flow["delta_angle[1]"], + 'delta_angle_z': optical_flow["delta_angle[2]"], 'quality': optical_flow["quality"] }) return flow diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_logger.h b/src/modules/ekf2/test/sensor_simulator/ekf_logger.h index 7a1c648d89..748adcc758 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_logger.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_logger.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 95ccb1e3cc..596d087cf3 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -205,6 +205,11 @@ bool EkfWrapper::isIntendingMag3DFusion() const return _ekf->control_status_flags().mag_3D; } +bool EkfWrapper::isMagHeadingConsistent() const +{ + return _ekf->control_status_flags().mag_heading_consistent; +} + void EkfWrapper::setMagFuseTypeNone() { _ekf_params->mag_fusion_type = MagFuseType::NONE; @@ -275,11 +280,6 @@ float EkfWrapper::getYawAngle() const return euler(2); } -matrix::Vector4f EkfWrapper::getQuaternionVariance() const -{ - return matrix::Vector4f(_ekf->orientation_covariances().diag()); -} - int EkfWrapper::getQuaternionResetCounter() const { float tmp[4]; diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 9e53505395..beb43e64e3 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -100,6 +100,7 @@ public: bool isIntendingMagHeadingFusion() const; bool isIntendingMag3DFusion() const; + bool isMagHeadingConsistent() const; void setMagFuseTypeNone(); void enableMagStrengthCheck(); void enableMagInclinationCheck(); @@ -117,7 +118,6 @@ public: Eulerf getEulerAngles() const; float getYawAngle() const; - matrix::Vector4f getQuaternionVariance() const; int getQuaternionResetCounter() const; void enableDragFusion(); diff --git a/src/modules/ekf2/test/sensor_simulator/flow.h b/src/modules/ekf2/test/sensor_simulator/flow.h index 4820fedfd8..2c0cdf94fc 100644 --- a/src/modules/ekf2/test/sensor_simulator/flow.h +++ b/src/modules/ekf2/test/sensor_simulator/flow.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index 1ecacb85a8..82d62df70f 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/imu.h b/src/modules/ekf2/test/sensor_simulator/imu.h index 18600c3eb7..ccf6856edf 100644 --- a/src/modules/ekf2/test/sensor_simulator/imu.h +++ b/src/modules/ekf2/test/sensor_simulator/imu.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/mag.h b/src/modules/ekf2/test/sensor_simulator/mag.h index 5cfed8b892..202ba63fb9 100644 --- a/src/modules/ekf2/test/sensor_simulator/mag.h +++ b/src/modules/ekf2/test/sensor_simulator/mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/range_finder.h b/src/modules/ekf2/test/sensor_simulator/range_finder.h index e41a923cb7..0dcd068232 100644 --- a/src/modules/ekf2/test/sensor_simulator/range_finder.h +++ b/src/modules/ekf2/test/sensor_simulator/range_finder.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/sensor.h b/src/modules/ekf2/test/sensor_simulator/sensor.h index 271960eb47..317f922978 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor.h +++ b/src/modules/ekf2/test/sensor_simulator/sensor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h index ce61f43ddf..ee6a651e3a 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/sensor_simulator/vio.h b/src/modules/ekf2/test/sensor_simulator/vio.h index 683341024b..28d98135fc 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.h +++ b/src/modules/ekf2/test/sensor_simulator/vio.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index c92a3170ec..28964035c9 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp index b1a9e14f8a..d851d6a7a6 100644 --- a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp +++ b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp @@ -36,7 +36,7 @@ #include "test_helper/comparison_helper.h" #include "../EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h" -#include "../EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h" +#include "../EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" #include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" #include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" @@ -137,11 +137,19 @@ TEST(AttitudeVariance, matlabVsSymforceZeroTilt) void increaseYawVar(const Vector24f &state_vector, SquareMatrix24f &P, const float yaw_var) { SquareMatrix q_cov; - sym::YawVarToLowerTriangularQuatCov(state_vector, yaw_var, &q_cov); + sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(0.f, 0.f, yaw_var), &q_cov); q_cov.copyLowerToUpperTriangle(); P.slice<4, 4>(0, 0) += q_cov; } +void setTiltVar(const Vector24f &state_vector, SquareMatrix24f &P, const float tilt_var) +{ + SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(tilt_var, tilt_var, 0.f), &q_cov); + q_cov.copyLowerToUpperTriangle(); + P.slice<4, 4>(0, 0) = q_cov; +} + float getYawVar(const Vector24f &state_vector, const SquareMatrix24f &P) { Vector24f H_YAW; @@ -210,3 +218,55 @@ TEST(AttitudeVariance, increaseYawWithTilt) const float var_2 = getYawVar(state_vector, P); EXPECT_NEAR(var_2, yaw_var_1 + yaw_var_2, 1e-6f); } + +TEST(AttitudeVariance, setRotVarNoTilt) +{ + Quatf q; + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float tilt_var = radians(1.2f); + setTiltVar(state_vector, P, tilt_var); + + Vector3f rot_var; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var); + + EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f); + EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f); + EXPECT_EQ(rot_var(2), 0.f); + + // Compare against known values (special case) + EXPECT_EQ(P(0, 0), 0.f); + EXPECT_EQ(P(1, 1), 0.25f * tilt_var); + EXPECT_EQ(P(2, 2), 0.25f * tilt_var); + EXPECT_EQ(P(3, 3), 0.25f * 0.f); // no yaw var +} + +TEST(AttitudeVariance, setRotVarPitch90) +{ + Quatf q(Eulerf(0.f, M_PI_F, 0.f)); + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float tilt_var = radians(1.2f); + setTiltVar(state_vector, P, tilt_var); + + Vector3f rot_var; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var); + + // TODO: FIXME, due to the nonlinearity of the quaternion parameters, + // setting the variance and getting it back is approximate. + // The correct way would be to keep the uncertainty as a 3D vector in the tangent plane + // instead of converting it to the parameter space + // EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f); + // EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f); + // EXPECT_EQ(rot_var(2), 0.f); +} diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index d7b00f1b8d..ba1c842ebe 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -245,22 +245,16 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) _sensor_simulator.runSeconds(1); - float hpos = 0.f; - float vpos = 0.f; - float hvel = 0.f; - float vvel = 0.f; - float baro_vpos = 0.f; - // After the change of origin, the pos and vel innovations should stay small - _ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos); - _ekf->getBaroHgtInnovRatio(baro_vpos); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f); - EXPECT_NEAR(hpos, 0.f, 0.05f); - EXPECT_NEAR(vpos, 0.f, 0.05f); - EXPECT_NEAR(baro_vpos, 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_baro_hgt().test_ratio, 0.f, 0.05f); - EXPECT_NEAR(hvel, 0.f, 0.02f); - EXPECT_NEAR(vvel, 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f); } TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) @@ -271,6 +265,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + EXPECT_FALSE(_ekf->global_origin_valid()); + _latitude_new = 45.0000005; _longitude_new = 111.0000005; _altitude_new = 1500.0; @@ -282,21 +278,79 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + // Global origin has been initialized but since there is no position aiding, the global + // position is still not valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + _sensor_simulator.runSeconds(1); - float hpos = 0.f; - float vpos = 0.f; - float hvel = 0.f; - float vvel = 0.f; - // After the change of origin, the pos and vel innovations should stay small - _ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f); - EXPECT_NEAR(hpos, 0.f, 0.05f); - EXPECT_NEAR(vpos, 0.f, 0.05f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f); + EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f); +} - EXPECT_NEAR(hvel, 0.f, 0.02f); - EXPECT_NEAR(vvel, 0.f, 0.02f); +TEST_F(EkfBasicsTest, global_position_from_local_ev) +{ + // GIVEN: external vision (local) aiding + _ekf_wrapper.enableExternalVisionPositionFusion(); + _sensor_simulator._vio.setPositionFrameToLocalNED(); + _sensor_simulator.startExternalVision(); + + _sensor_simulator.runSeconds(1); + + // THEN; since there is no origin, only the local position can be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _latitude_new = 45.0000005; + _longitude_new = 111.0000005; + _altitude_new = 1500.0; + + // BUT WHEN: the global origin is set (manually) + _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); + + // THEN: local and global positions are valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->local_position_is_valid()); +} + +TEST_F(EkfBasicsTest, global_position_from_opt_flow) +{ + // GIVEN: optical flow aiding + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); + _sensor_simulator.startFlow(); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startRangeFinder(); + + _sensor_simulator.runSeconds(1); + + // THEN; since there is no origin, only the local position can be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _latitude_new = 45.0000005; + _longitude_new = 111.0000005; + _altitude_new = 1500.0; + + // BUT WHEN: the global origin is set (manually) + _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); + + // THEN: local and global positions are valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->local_position_is_valid()); } // TODO: Add sampling tests diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 3be67e2da7..aa8376dc6c 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -61,9 +61,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 553a0af47c..0302849dd4 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index cfcc62d660..0bafab37f3 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -183,7 +183,6 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow) const float max_ground_distance = 50.f; _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); _sensor_simulator.startFlow(); - _sensor_simulator.startFlow(); _ekf_wrapper.enableFlowFusion(); _ekf->set_in_air_status(true); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 8921a2cb22..a18c3fdf02 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -222,8 +222,7 @@ TEST_F(EkfGpsTest, altitudeDrift) _sensor_simulator.runSeconds(dt); } - float baro_innov; - _ekf->getBaroHgtInnov(baro_innov); + float baro_innov = _ekf->aid_src_baro_hgt().innovation; BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus(); printf("baro innov = %f\n", (double)baro_innov); diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index 6222c05020..781133dde2 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 ECL Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -61,9 +61,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); @@ -304,9 +303,12 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) _sensor_simulator.runSeconds(7.5); // THEN: after a few seconds, the fusion should stop and - // the estimator should fall back to mag fusion + // the estimator doesn't fall back to mag fusion because it has + // been declared inconsistent with the filter states EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); - EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent()); + //TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop? + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); } TEST_F(EkfGpsHeadingTest, stopOnGround) @@ -331,9 +333,9 @@ TEST_F(EkfGpsHeadingTest, stopOnGround) _ekf_wrapper.setMagFuseTypeNone(); // WHEN: running without yaw aiding - const matrix::Vector4f quat_variance_before = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance_before = _ekf->getQuaternionVariance(); _sensor_simulator.runSeconds(20.0); - const matrix::Vector4f quat_variance_after = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance_after = _ekf->getQuaternionVariance(); // THEN: the yaw variance increases EXPECT_GT(quat_variance_after(3), quat_variance_before(3)); diff --git a/src/modules/ekf2/test/test_EKF_gyroscope.cpp b/src/modules/ekf2/test/test_EKF_gyroscope.cpp index e19bd2586b..8d3daac169 100644 --- a/src/modules/ekf2/test/test_EKF_gyroscope.cpp +++ b/src/modules/ekf2/test/test_EKF_gyroscope.cpp @@ -58,9 +58,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } @@ -70,22 +69,25 @@ public: { } - void testBias(const Vector3f &bias, float duration, float tolerance); + void testBias(const Vector3f &bias, float duration, const Vector3f &tolerance); }; -void EkfGyroscopeTest::testBias(const Vector3f &bias, float duration, float tolerance) +void EkfGyroscopeTest::testBias(const Vector3f &bias, float duration, const Vector3f &tolerance) { _sensor_simulator._imu.setGyroData(bias); _sensor_simulator.runSeconds(duration); + EXPECT_TRUE(_ekf->control_status_flags().vehicle_at_rest); const Vector3f estimated_bias = _ekf->getGyroBias(); - EXPECT_TRUE(matrix::isEqual(estimated_bias, bias, - tolerance)) << estimated_bias - bias; + + for (int i = 0; i < 3; i++) { + EXPECT_NEAR(estimated_bias(i), bias(i), tolerance(i)) << "index " << i; + } } TEST_F(EkfGyroscopeTest, biasEstimateZero) { - testBias(Vector3f(), 10, 0.f); + testBias(Vector3f(), 10, Vector3f()); } TEST_F(EkfGyroscopeTest, biasEstimatePositive) @@ -96,7 +98,8 @@ TEST_F(EkfGyroscopeTest, biasEstimatePositive) for (int i = 0; i < 4; i ++) { bias.setAll(biases[i]); - testBias(bias, 30, 0.0008f); + // The Z gyro bias takes more time to converge as the Z rotation variance is higher + testBias(bias, 30, Vector3f(0.0008f, 0.0008f, 0.004f)); } } @@ -107,6 +110,6 @@ TEST_F(EkfGyroscopeTest, biasEstimateNegative) for (int i = 0; i < 4; i ++) { bias.setAll(biases[i]); - testBias(bias, 30, 0.0008f); + testBias(bias, 30, Vector3f(0.0008f, 0.0008f, 0.004f)); } } diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index a9d70bb1d9..f83ad67cd4 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -198,13 +198,8 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f); // and the innovations are close to zero - float baro_innov = NAN; - _ekf->getBaroHgtInnov(baro_innov); - EXPECT_NEAR(baro_innov, 0.f, 0.2f); - - float rng_innov = NAN; - _ekf->getRngHgtInnov(rng_innov); - EXPECT_NEAR(rng_innov, 0.f, 0.2f); + EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f); + EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f); } TEST_F(EkfHeightFusionTest, baroRefFailOver) diff --git a/src/modules/ekf2/test/test_EKF_imuSampling.cpp b/src/modules/ekf2/test/test_EKF_imuSampling.cpp index c037c2c44b..bd300c5385 100644 --- a/src/modules/ekf2/test/test_EKF_imuSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_imuSampling.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index 716519c341..5dad09e2e8 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -78,7 +78,7 @@ public: void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f) { - const matrix::Vector4f quat_variance = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance = _ekf->getQuaternionVariance(); EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1); EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2); EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3); diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index b166aea45f..11a079958b 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 ECL Development Team. All rights reserved. + * Copyright (c) 2023 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,9 +57,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp index a3e0e77186..367ae41534 100644 --- a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp b/src/modules/ekf2/test/test_EKF_ringbuffer.cpp index 44cf3a2e43..0338bea816 100644 --- a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp +++ b/src/modules/ekf2/test/test_EKF_ringbuffer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp index 4220d6559f..72d0016160 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 ECL Development Team. All rights reserved. + * Copyright (c) 2020-2023 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,6 +40,7 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" class EkfTerrainTest : public ::testing::Test { @@ -182,3 +183,30 @@ TEST_F(EkfTerrainTest, testRngForTerrainFusion) const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); EXPECT_NEAR(estimated_distance_to_ground, rng_height, 0.01f); } + +TEST_F(EkfTerrainTest, testHeightReset) +{ + // GIVEN: rng for terrain but not flow + _ekf_wrapper.disableTerrainFlowFusion(); + _ekf_wrapper.enableTerrainRngFusion(); + + const float rng_height = 1.f; + const float flow_height = 1.f; + runFlowAndRngScenario(rng_height, flow_height); + + const float estimated_distance_to_ground = _ekf->getTerrainVertPos() - _ekf->getPosition()(2); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: the baro height is suddenly changed to trigger a height reset + const float new_baro_height = _sensor_simulator._baro.getData() + 50.f; + _sensor_simulator._baro.setData(new_baro_height); + _sensor_simulator.stopGps(); // prevent from switching to GNSS height + _sensor_simulator.runSeconds(6); + + // THEN: a height reset occured and the estimated distance to the ground remains constant + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); + EXPECT_NEAR(estimated_distance_to_ground, _ekf->getTerrainVertPos() - _ekf->getPosition()(2), 1e-3f); +} diff --git a/src/modules/ekf2/test/test_EKF_utils.cpp b/src/modules/ekf2/test/test_EKF_utils.cpp index 144d6e6e94..056be5ecff 100644 --- a/src/modules/ekf2/test/test_EKF_utils.cpp +++ b/src/modules/ekf2/test/test_EKF_utils.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_withReplayData.cpp b/src/modules/ekf2/test/test_EKF_withReplayData.cpp index b9ee8cea15..b97637440a 100644 --- a/src/modules/ekf2/test/test_EKF_withReplayData.cpp +++ b/src/modules/ekf2/test/test_EKF_withReplayData.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 987b92506e..a49cd5c66f 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 ECL Development Team. All rights reserved. + * Copyright (c) 2021-2023 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 diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index 2e10f69357..ce3ee5f624 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 ECL Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 diff --git a/src/modules/ekf2/test/test_helper/CMakeLists.txt b/src/modules/ekf2/test/test_helper/CMakeLists.txt index e5ac7d9a62..21ebcbcddc 100644 --- a/src/modules/ekf2/test/test_helper/CMakeLists.txt +++ b/src/modules/ekf2/test/test_helper/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 ECL Development Team. All rights reserved. +# Copyright (c) 2019-2023 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 @@ -12,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name ECL nor the names of its contributors may be +# 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. # diff --git a/src/modules/ekf2/test/test_helper/reset_logging_checker.h b/src/modules/ekf2/test/test_helper/reset_logging_checker.h index 965a636371..c36f288c01 100644 --- a/src/modules/ekf2/test/test_helper/reset_logging_checker.h +++ b/src/modules/ekf2/test/test_helper/reset_logging_checker.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 ECL Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 6c34aae7a3..8168f0bca5 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -221,8 +221,7 @@ void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp) { // Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, - _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _deltatime); // Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input _yaw_sp_aligned = true; diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp index 3ebc783581..85ba49ea0b 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -60,8 +60,7 @@ bool FlightTaskDescend::update() // Nudging if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index c4b9649046..063b78dcf6 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -112,6 +112,7 @@ void FlightTask::_evaluateVehicleLocalPosition() // yaw _yaw = _sub_vehicle_local_position.get().heading; + _unaided_yaw = _sub_vehicle_local_position.get().unaided_heading; _is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control; // position diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 95021fed59..3b9ddbd466 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -218,6 +218,7 @@ protected: matrix::Vector3f _velocity; /**< current vehicle velocity */ float _yaw{}; /**< current vehicle yaw heading */ + float _unaided_yaw{}; bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */ float _dist_to_bottom{}; /**< current height above ground level if dist_bottom is valid */ float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 0a2704d68a..ab90d10e12 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -64,6 +64,7 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.f; + _stick_yaw.reset(_yaw, _unaided_yaw); _setDefaultConstraints(); _updateConstraintsFromEstimator(); @@ -273,14 +274,15 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) { // Only reset the yaw setpoint when the heading is locked if (PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint += delta_psi; + _yaw_setpoint = wrap_pi(_yaw_setpoint + delta_psi); } + + _stick_yaw.ekfResetHandler(delta_psi); } void FlightTaskManualAltitude::_updateSetpoints() { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); _updateAltitudeLock(); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index fdac174e37..bf7ec2dc49 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -40,7 +40,6 @@ #pragma once #include "FlightTask.hpp" -#include #include "Sticks.hpp" #include "StickTiltXY.hpp" #include "StickYaw.hpp" @@ -115,6 +114,8 @@ private: void setGearAccordingToSwitch(); + bool _updateYawCorrection(); + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 8a2512279c..edc82d6c98 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -35,23 +35,80 @@ #include +using matrix::wrap_pi; + StickYaw::StickYaw(ModuleParams *parent) : ModuleParams(parent) {} -void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, - const float yaw, const bool is_yaw_good_for_control, const float deltatime) +void StickYaw::reset(const float yaw, const float unaided_yaw) { + if (PX4_ISFINITE(unaided_yaw)) { + _yaw_error_lpf.reset(wrap_pi(yaw - unaided_yaw)); + } +} + +void StickYaw::ekfResetHandler(const float delta_yaw) +{ + _yaw_error_lpf.reset(wrap_pi(_yaw_error_lpf.getState() + delta_yaw)); + _yaw_error_ref = wrap_pi(_yaw_error_ref + delta_yaw); +} + +void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, + const float yaw, const float deltatime, const float unaided_yaw) +{ + _yaw_error_lpf.setParameters(deltatime, _kYawErrorTimeConstant); + const float yaw_correction_prev = _yaw_correction; + const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw); + + if (reset_setpoint) { + yaw_setpoint = NAN; + } + _yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get()); yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get())); - yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control); + yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev); +} + +bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw) +{ + if (!PX4_ISFINITE(unaided_yaw)) { + _yaw_correction = 0.f; + return false; + } + + // Detect the convergence phase of the yaw estimate by monitoring its relative + // distance from an unaided yaw source. + const float yaw_error = wrap_pi(yaw - unaided_yaw); + + // Run it through a high-pass filter to detect transients + const float yaw_error_hpf = wrap_pi(yaw_error - _yaw_error_lpf.getState()); + _yaw_error_lpf.update(yaw_error); + + const bool was_converging = _yaw_estimate_converging; + _yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold; + + bool reset_setpoint = false; + + if (!_yaw_estimate_converging) { + _yaw_error_ref = yaw_error; + + if (was_converging) { + // Force a reset of the locking mechanism + reset_setpoint = true; + } + } + + _yaw_correction = wrap_pi(yaw_error - _yaw_error_ref); + + return reset_setpoint; } float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint, - const bool is_yaw_good_for_control) + const float yaw_correction_prev) const { // Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN. - if ((fabsf(yawspeed_setpoint) > FLT_EPSILON) || !is_yaw_good_for_control) { + if (fabsf(yawspeed_setpoint) > FLT_EPSILON) { // no fixed heading when rotating around yaw by stick return NAN; @@ -61,7 +118,7 @@ float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, co return yaw; } else { - return yaw_setpoint; + return wrap_pi(yaw_setpoint - yaw_correction_prev + _yaw_correction); } } } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index 8e9e4aea1e..185c490110 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -48,12 +48,23 @@ public: StickYaw(ModuleParams *parent); ~StickYaw() = default; - void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, - bool is_yaw_good_for_control, float deltatime); + void reset(float yaw, float unaided_yaw = NAN); + void ekfResetHandler(float delta_yaw); + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime, + float unaided_yaw = NAN); private: AlphaFilter _yawspeed_filter; + float _yaw_error_ref{0.f}; + float _yaw_correction{0.f}; + bool _yaw_estimate_converging{false}; + AlphaFilter _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter + static constexpr float _kYawErrorTimeConstant{1.f}; ///< time constant of the high-pass filter used to detect yaw convergence + static constexpr float _kYawErrorChangeThreshold{radians(1.f)}; ///< we consider the yaw estimate as "converging" when above this threshold + + bool updateYawCorrection(float yaw, float unaided_yaw); + /** * Lock yaw when not currently turning * When applying a yawspeed the vehicle is turning, when the speed is @@ -65,7 +76,7 @@ private: * @param yaw current yaw setpoint which then will be overwritten by the return value * @return yaw setpoint to execute to have a yaw lock at the correct moment in time */ - static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control); + float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 66d80ee90e..554eac360c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -44,10 +44,7 @@ * Attitude Roll Time Constant * * This defines the latency between a roll step input and the achieved setpoint - * (inverse to a P gain). Half a second is a good start value and fits for - * most average systems. Smaller systems may require smaller values, but as - * this will wear out servos faster, the value should only be decreased as - * needed. + * (inverse to a P gain). Smaller systems may require smaller values. * * @unit s * @min 0.2 @@ -62,10 +59,7 @@ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); * Attitude pitch time constant * * This defines the latency between a pitch step input and the achieved setpoint - * (inverse to a P gain). Half a second is a good start value and fits for - * most average systems. Smaller systems may require smaller values, but as - * this will wear out servos faster, the value should only be decreased as - * needed. + * (inverse to a P gain). Smaller systems may require smaller values. * * @unit s * @min 0.2 @@ -168,9 +162,6 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); /** * Wheel steering rate integrator limit * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * * @min 0.0 * @max 1.0 * @decimal 2 @@ -197,8 +188,6 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f); /** * Wheel steering rate feed forward * - * Direct feed forward from rate setpoint to control surface output - * * @unit %/rad/s * @min 0.0 * @max 10 @@ -228,8 +217,7 @@ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); * Maximum manually added yaw rate * * This is the maximally added yaw rate setpoint from the yaw stick in any attitude controlled flight mode. - * The controller already generates a yaw rate setpoint to coordinate a turn, and this value is added to it. - * This is an absolute value, which is applied symmetrically to the negative and positive side. + * It is added to the yaw rate setpoint generated by the controller for turn coordination. * * @unit deg/s * @min 0 @@ -242,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_YR_MAX, 30.f); /** * Maximum manual roll angle * - * Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode + * Applies to both directions in all manual modes with attitude stabilization * * @unit deg * @min 0.0 @@ -256,7 +244,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); /** * Maximum manual pitch angle * - * Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode + * Applies to both directions in all manual modes with attitude stabilization but without altitude control * * @unit deg * @min 0.0 diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index ec274f28da..eee05880fd 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -180,7 +180,11 @@ void FwAutotuneAttitudeControl::Run() Vector3f kid = pid_design::computePidGmvc(num_design, den, _sample_interval_avg, 0.2f, 0.f, 0.4f); _kiff(0) = kid(0); _kiff(1) = kid(1); - _attitude_p = 8.f / (M_PI_F * (_kiff(2) + _kiff(0))); // Maximum control power at an attitude error of pi/8 + + // To compute the attitude gain, use the following empirical rule: + // "An error of 60 degrees should produce the maximum control output" + // or K_att * (K_rate + K_ff) * rad(60) = 1 + _attitude_p = math::constrain(1.f / (math::radians(60.f) * (_kiff(0) + _kiff(2))), 1.f, 5.f); const Vector &coeff_var = _sys_id.getVariances(); const Vector3f rate_sp = _sys_id.areFiltersInitialized() @@ -638,19 +642,21 @@ const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal() if (_state == state::roll || _state == state::test) { // Scale the signal such that the attitude controller is // able to cancel it completely at an attitude error of pi/8 - signal_scaled = signal * M_PI_F / (8.f * _param_fw_r_tc.get()); + signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_r_tc.get()), math::radians(_param_fw_r_rmax.get())); rate_sp(0) = signal_scaled - _signal_filter.getState(); } if (_state == state::pitch || _state == state::test) { - signal_scaled = signal * M_PI_F / (8.f * _param_fw_p_tc.get()); + const float pitch_rate_max_deg = math::min(_param_fw_p_rmax_pos.get(), _param_fw_p_rmax_neg.get()); + signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_p_tc.get()), math::radians(pitch_rate_max_deg)); rate_sp(1) = signal_scaled - _signal_filter.getState(); } if (_state == state::yaw) { // Do not send a signal that produces more than a full deflection of the rudder - signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get())); + signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get()), + math::radians(_param_fw_y_rmax.get())); rate_sp(2) = signal_scaled - _signal_filter.getState(); } diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp index 213518ef72..9be5f55596 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -193,14 +193,18 @@ private: (ParamFloat) _param_fw_rr_p, (ParamFloat) _param_fw_rr_i, (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_r_rmax, (ParamFloat) _param_fw_r_tc, (ParamFloat) _param_fw_pr_p, (ParamFloat) _param_fw_pr_i, (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_p_rmax_pos, + (ParamFloat) _param_fw_p_rmax_neg, (ParamFloat) _param_fw_p_tc, (ParamFloat) _param_fw_yr_p, (ParamFloat) _param_fw_yr_i, - (ParamFloat) _param_fw_yr_ff + (ParamFloat) _param_fw_yr_ff, + (ParamFloat) _param_fw_y_rmax ) static constexpr float _publishing_dt_s = 100e-3f; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index da1938ffc6..a7f5bb5eab 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -76,6 +76,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : /* fetch initial parameter values */ parameters_update(); + + _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_trim.get()); } FixedwingPositionControl::~FixedwingPositionControl() @@ -399,26 +401,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint() float FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, - float calibrated_min_airspeed, const Vector2f &ground_speed) + float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation) { - if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { - calibrated_airspeed_setpoint = _param_fw_airspd_trim.get(); - } - - // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained - if (!_wind_valid) { - /* - * This error value ensures that a plane (as long as its throttle capability is - * not exceeded) travels towards a waypoint (and is not pushed more and more away - * by wind). Not countering this would lead to a fly-away. Only non-zero in presence - * of sufficient wind. "minimum ground speed undershoot". - */ - const float ground_speed_body = _body_velocity_x; - - if (ground_speed_body < _param_fw_gnd_spd_min.get()) { - calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body; - } - } + // --- airspeed *constraint adjustments --- float load_factor_from_bank_angle = 1.0f; @@ -441,27 +426,53 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio); // Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds. - if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { + if (!in_takeoff_situation && _airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() * _wind_vel.length(), _param_fw_airspd_max.get()); } + // --- airspeed *setpoint adjustments --- + + if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { + calibrated_airspeed_setpoint = _param_fw_airspd_trim.get(); + } + + // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained + if (!_wind_valid && !in_takeoff_situation) { + /* + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. Only non-zero in presence + * of sufficient wind. "minimum ground speed undershoot". + */ + const float ground_speed_body = _body_velocity_x; + + if (ground_speed_body < _param_fw_gnd_spd_min.get()) { + calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body; + } + } + calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed, _param_fw_airspd_max.get()); - // initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case - const bool slewed_airspeed_outside_of_limits = _airspeed_slew_rate_controller.getState() < calibrated_min_airspeed - || _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get(); - - if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || slewed_airspeed_outside_of_limits) { + if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) { _airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint); - - } else if (control_interval > FLT_EPSILON) { - // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s - calibrated_airspeed_setpoint = _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); } - return calibrated_airspeed_setpoint; + if (control_interval > FLT_EPSILON) { + // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s + _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); + } + + if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) { + _airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed); + } + + if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) { + _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get()); + } + + return _airspeed_slew_rate_controller.getState(); } void @@ -712,7 +723,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _skipping_takeoff_detection = false; if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) || - _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) { + _control_mode.flag_control_offboard_enabled) && (_position_setpoint_current_valid + || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + + // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. + // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -1025,21 +1040,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { const float acc_rad = _npfg.switchDistance(500.0f); - Vector2d curr_wp{0, 0}; - Vector2d prev_wp{0, 0}; - - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - - if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; - - } else { - // No valid previous waypoint, go along the line between aircraft and current waypoint - prev_wp = curr_pos; - } - float tecs_fw_thr_min; float tecs_fw_thr_max; @@ -1062,14 +1062,14 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) ) { - const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), - pos_sp_prev.lat, pos_sp_prev.lon); + const float d_curr_prev = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_prev.lat, + pos_sp_prev.lon); // Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { // Calculate distance to current waypoint - const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), - _current_latitude, _current_longitude); + const float d_curr = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, _current_latitude, + _current_longitude); // Save distance to waypoint if it is the smallest ever achieved, however make sure that // _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp @@ -1093,8 +1093,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co _param_fw_airspd_min.get(), ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); - Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1)); + Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); @@ -1107,8 +1106,12 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - } else { + } else if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + + } else { + navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1313,16 +1316,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo adjusted_min_airspeed = takeoff_airspeed; } - float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, - ground_speed); - if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { _runway_takeoff.init(now, _yaw, global_position); - _takeoff_ground_alt = _current_altitude; - _launch_current_yaw = _yaw; + _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway"); } @@ -1353,17 +1352,23 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP - Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)}; + float takeoff_bearing = _launch_current_yaw; if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { // the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded - takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local); + const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - start_pos_local; + + if (takeoff_bearing_vector.norm() > FLT_EPSILON) { + takeoff_bearing = atan2f(takeoff_bearing_vector(1), takeoff_bearing_vector(0)); + } } + float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, + true); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed, - _wind_vel, 0.0f); + navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); @@ -1433,6 +1438,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_global_position = global_position; _takeoff_ground_alt = _current_altitude; _launch_current_yaw = _yaw; + _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); } const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0), @@ -1440,11 +1446,15 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP - Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)}; + float takeoff_bearing = _launch_current_yaw; if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { // the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded - takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local); + const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - launch_local_position; + + if (takeoff_bearing_vector.norm() > FLT_EPSILON) { + takeoff_bearing = atan2f(takeoff_bearing_vector(1), takeoff_bearing_vector(0)); + } } /* Set control values depending on the detection state */ @@ -1452,10 +1462,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo && _param_fw_laun_detcn_on.get()) { /* Launch has been detected, hence we have to control the plane. */ + float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, + true); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel, - 0.0f); + navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1603,7 +1615,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); + navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1682,7 +1694,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); + navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1944,7 +1956,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, updateManualTakeoffStatus(); const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _param_fw_airspd_min.get(), ground_speed); + _param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -1985,7 +1997,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, updateManualTakeoffStatus(); float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _param_fw_airspd_min.get(), ground_speed); + _param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -2001,6 +2013,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } /* heading control */ + // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) { @@ -2025,28 +2038,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + _hdg_hold_position.lat = _current_latitude; + _hdg_hold_position.lon = _current_longitude; } - /* we have a valid heading hold position, are we too close? */ - const float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat, - _hdg_hold_curr_wp.lon); - - if (dist < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); - } - - Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon}; - Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon}; - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); - Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), - prev_wp(1)); + Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; @@ -2055,7 +2056,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } tecs_update_pitch_throttle(control_interval, - _current_altitude, + _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, min_pitch, radians(_param_fw_p_lim_max.get()), @@ -2147,6 +2148,13 @@ FixedwingPositionControl::Run() _current_longitude = gpos.lon; } + if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) { + _reference_altitude = _local_pos.ref_alt; + + } else { + _reference_altitude = 0.f; + } + _current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters // handle estimator reset events. we only adjust setpoins for manual modes @@ -2174,9 +2182,16 @@ FixedwingPositionControl::Run() || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp) || (_local_pos.vxy_reset_counter != _pos_reset_counter)) { - _global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon, + double reference_latitude = 0.; + double reference_longitude = 0.; + + if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) { + reference_latitude = _local_pos.ref_lat; + reference_longitude = _local_pos.ref_lon; + } + + _global_local_proj_ref.initReference(reference_latitude, reference_longitude, _local_pos.ref_timestamp); - _global_local_alt0 = _local_pos.ref_alt; } if (_control_mode.flag_control_offboard_enabled) { @@ -2205,7 +2220,7 @@ FixedwingPositionControl::Run() _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _pos_sp_triplet.current.lat = lat; _pos_sp_triplet.current.lon = lon; - _pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2]; + _pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2]; } } @@ -2616,27 +2631,6 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike); } -Vector2f -FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position, - const Vector2f &takeoff_waypoint) const -{ - Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position; - - if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) { - takeoff_bearing_vector.normalize(); - - } else { - // TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need - // for this check - - // takeoff in the direction of the airframe - takeoff_bearing_vector(0) = cosf(_yaw); - takeoff_bearing_vector(1) = sinf(_yaw); - } - - return takeoff_bearing_vector; -} - void FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) @@ -2806,7 +2800,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo local_position_setpoint.x = current_setpoint(0); local_position_setpoint.y = current_setpoint(1); - local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt; + local_position_setpoint.z = _reference_altitude - current_waypoint.alt; local_position_setpoint.yaw = NAN; local_position_setpoint.yawspeed = NAN; local_position_setpoint.vx = NAN; @@ -2840,47 +2834,97 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_ _orbit_status_pub.publish(orbit_status); } -void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B, +void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, const Vector2f &end_waypoint, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { - // similar to logic found in ECL_L1_Pos_Controller method of same name - // BUT no arbitrary max approach angle, approach entirely determined by generated - // bearing vectors + const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint; + const Vector2f start_waypoint_to_vehicle = vehicle_pos - start_waypoint; + const Vector2f end_waypoint_to_vehicle = vehicle_pos - end_waypoint; - const Vector2f vector_A_to_B = waypoint_B - waypoint_A; - const Vector2f vector_B_to_A = -vector_A_to_B; - const Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A; - const Vector2f vector_B_to_vehicle = vehicle_pos - waypoint_B; - Vector2f desired_path = vector_A_to_B; // this is the default path tangent, but is overridden below - - if (vector_A_to_B.norm() < FLT_EPSILON) { - // the waypoints are on top of each other and should be considered as a - // single waypoint, fly directly to it - if (vector_A_to_vehicle.norm() > FLT_EPSILON) { - desired_path = -vector_A_to_vehicle; - - } else { - // Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output. - return; - } - - } else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) { - // we are in front of waypoint A, fly directly to it until we are within switch distance. - if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) { - desired_path = -vector_A_to_vehicle; - } - - } else if (vector_B_to_A.dot(vector_B_to_vehicle) < -FLT_EPSILON) { - // we are behind waypoint B, fly directly to it - desired_path = -vector_B_to_vehicle; + if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) { + // degenerate case: the waypoints are on top of each other, this should only happen when someone uses this + // method incorrectly. just as a safe guard, call the singular waypoint navigation method. + navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); + return; } - // track the line segment - Vector2f unit_path_tangent{desired_path.normalized()}; + if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON) + && (start_waypoint_to_vehicle.norm() > _npfg.switchDistance(500.0f))) { + // we are in front of the start waypoint, fly directly to it until we are within switch distance + navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); + return; + } + + if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) { + // we are beyond the end waypoint, fly back to it + // NOTE: this logic ideally never gets executed, as a waypoint switch should happen before passing the + // end waypoint. however this included here as a safety precaution if any navigator (module) switch condition + // is missed for any reason. in the future this logic should all be handled in one place in a dedicated + // flight mode state machine. + navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); + return; + } + + // follow the line segment between the start and end waypoints + navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); +} + +void FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel) +{ + const Vector2f vehicle_to_waypoint = waypoint_pos - vehicle_pos; + + if (vehicle_to_waypoint.norm() < FLT_EPSILON) { + // degenerate case: the vehicle is on top of the single waypoint. (can happen). maintain the last npfg command. + return; + } + + const Vector2f unit_path_tangent = vehicle_to_waypoint.normalized(); + _closest_point_on_path = waypoint_pos; + + const float path_curvature = 0.f; + _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + + // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); - _closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f); -} // navigateWaypoints +} + +void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, + const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) +{ + const Vector2f line_segment = point_on_line_2 - point_on_line_1; + + if (line_segment.norm() <= FLT_EPSILON) { + // degenerate case: line segment has zero length. maintain the last npfg command. + return; + } + + const Vector2f unit_path_tangent = line_segment.normalized(); + + const Vector2f point_1_to_vehicle = vehicle_pos - point_on_line_1; + _closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; + + const float path_curvature = 0.f; + _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + + // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. + _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); +} + +void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const float line_bearing, + const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) +{ + const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)}; + + const Vector2f point_on_line_to_vehicle = vehicle_pos - point_on_line; + _closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; + + const float path_curvature = 0.f; + _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + + // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. + _target_bearing = line_bearing; +} void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos, float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel) @@ -2919,19 +2963,23 @@ void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, con _closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); -} // navigateLoiter - +} void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, const matrix::Vector2f &tangent_setpoint, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature) { + if (tangent_setpoint.norm() <= FLT_EPSILON) { + // degenerate case: no direction. maintain the last npfg command. + return; + } + const Vector2f unit_path_tangent{tangent_setpoint.normalized()}; _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = position_setpoint; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), position_setpoint, curvature); -} // navigatePathTangent +} void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel) @@ -2941,7 +2989,7 @@ void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_p _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = vehicle_pos; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); -} // navigateBearing +} int FixedwingPositionControl::task_spawn(int argc, char *argv[]) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index fe4e6aa580..e46c14c005 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -266,7 +266,8 @@ private: float _body_velocity_x{0.f}; MapProjection _global_local_proj_ref{}; - float _global_local_alt0{NAN}; + + float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point bool _landed{true}; @@ -287,8 +288,7 @@ private: bool _hdg_hold_enabled{false}; // heading hold enabled bool _yaw_lock_engaged{false}; // yaw is locked for heading hold - position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started - position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies + position_setpoint_s _hdg_hold_position{}; // position where heading hold started // [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands float _manual_control_setpoint_for_height_rate{0.0f}; @@ -670,10 +670,11 @@ private: * @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s] * @param calibrated_min_airspeed Minimum calibrated airspeed [m/s] * @param ground_speed Vehicle ground velocity vector (NE) [m/s] + * @param in_takeoff_situation Vehicle is currently in a takeoff situation * @return Adjusted calibrated airspeed setpoint [m/s] */ float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, - float calibrated_min_airspeed, const Vector2f &ground_speed); + float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation = false); void reset_takeoff_state(); void reset_landing_state(); @@ -732,15 +733,6 @@ private: */ float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const; - /** - * @brief Calculates the unit takeoff bearing vector from the launch position to takeoff waypont. - * - * @param launch_position Vehicle launch position in local coordinates (NE) [m] - * @param takeoff_waypoint Takeoff waypoint position in local coordinates (NE) [m] - * @return Unit takeoff bearing vector - */ - Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const; - /** * @brief Calculates the touchdown position for landing with optional manual lateral adjustments. * @@ -789,28 +781,68 @@ private: /* * Waypoint handling logic following closely to the ECL_L1_Pos_Controller - * method of the same name. Takes two waypoints and determines the relevant - * parameters for evaluating the NPFG guidance law, then updates control setpoints. + * method of the same name. Takes two waypoints, steering the vehicle to track + * the line segment between them. * - * @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates - * (lat,lon) [deg] - * @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates - * (lat,lon) [deg] - * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] + * @param[in] start_waypoint Segment starting position in local coordinates. (N,E) [m] + * @param[in] end_waypoint Segment end position in local coordinates. (N,E) [m] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateWaypoints(const matrix::Vector2f &waypoint_A, const matrix::Vector2f &waypoint_B, + void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint, const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); + /* + * Takes one waypoint and steers the vehicle towards this. + * + * NOTE: this *will lead to "flowering" behavior if no higher level state machine or + * switching condition changes the waypoint. + * + * @param[in] waypoint_pos Waypoint position in local coordinates. (N,E) [m] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + */ + void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, + const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); + + /* + * Line (infinite) following logic. Two points on the line are used to define the + * line in 2D space (first to second point determines the direction). Determines the + * relevant parameters for evaluating the NPFG guidance law, then updates control setpoints. + * + * @param[in] point_on_line_1 Arbitrary first position on line in local coordinates. (N,E) [m] + * @param[in] point_on_line_2 Arbitrary second position on line in local coordinates. (N,E) [m] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + */ + void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); + + /* + * Line (infinite) following logic. One point on the line and a line bearing are used to define + * the line in 2D space. Determines the relevant parameters for evaluating the NPFG guidance law, + * then updates control setpoints. + * + * @param[in] point_on_line Arbitrary position on line in local coordinates. (N,E) [m] + * @param[in] line_bearing Line bearing [rad] (from north) + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + */ + void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); + /* * Loitering (unlimited) logic. Takes loiter center, radius, and direction and * determines the relevant parameters for evaluating the NPFG guidance law, * then updates control setpoints. * * @param[in] loiter_center The position of the center of the loiter circle [m] - * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] * @param[in] radius Loiter radius [m] * @param[in] loiter_direction_counter_clockwise Specifies loiter direction * @param[in] ground_vel Vehicle ground velocity vector [m/s] @@ -824,8 +856,10 @@ private: * Path following logic. Takes poisiton, path tangent, curvature and * then updates control setpoints to follow a path setpoint. * - * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] - * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg] + * TODO: deprecate this function with a proper API to NPFG. + * + * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] position_setpoint closest point on a path in local coordinates. (N,E) [m] * @param[in] tangent_setpoint unit tangent vector of the path [m] * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] @@ -839,9 +873,9 @@ private: * Navigate on a fixed bearing. * * This only holds a certain (ground relative) direction and does not perform - * cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading. + * cross track correction. Helpful for semi-autonomous modes. * - * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] + * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m] * @param[in] bearing Bearing angle [rad] * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index 86d5875b0b..af04e328cf 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -249,9 +249,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that - * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * Maximum throttle limit in altitude controlled modes. + * Should be set accordingly to achieve FW_T_CLMB_MAX. * * @unit norm * @min 0.0 @@ -265,11 +264,9 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide - * some aerodynamic drag from a turning prop to improve the descent rate. + * Minimum throttle limit in altitude controlled modes. + * Usually set to 0 but can be increased to prevent the motor from stopping when + * descending, which can increase achievable descent rates. * * For aircraft with internal combustion engine this parameter should be set * for desired idle rpm. @@ -382,10 +379,7 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 1); * * When disabled, the landing configuration (flaps, landing airspeed, etc.) is only activated * on the final approach to landing. When enabled, it is already activated when entering the - * final loiter-down (loiter-to-alt) waypoint before the landing approach. This shifts the (often large) - * altitude and airspeed errors caused by the configuration change away from the ground such that - * these are not so critical. It also gives the controller enough time to adapt to the new - * configuration such that the landing approach starts with a cleaner initial state. + * final loiter-down (loiter-to-alt) waypoint before the landing approach. * * @boolean * @@ -989,8 +983,6 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); * system more robust against disturbances (turbulence) in high wind. * Only applies to AUTO flight mode. * - * airspeed_min_adjusted = FW_AIRSPD_MIN + FW_WIND_ARSP_SC * wind.length() - * * @min 0 * @decimal 2 * @increment 0.01 @@ -999,10 +991,9 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f); /** - * FW Launch detection + * Fixed-wing launch detection * * Enables automatic launch detection based on measured acceleration. Use for hand- or catapult-launched vehicles. - * Only available for fixed-wing vehicles. * Not compatible with runway takeoff. * * @boolean diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index f52ed4b59d..cffd790401 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -46,10 +46,9 @@ * The minimal airspeed (calibrated airspeed) the user is able to command. * Further, if the airspeed falls below this value, the TECS controller will try to * increase airspeed more aggressively. - * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL), - * with some margin between the stall speed and minimum airspeed. + * Should be set (with some margin) above the vehicle stall speed. * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight), - * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE). + * and is automatically adapated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE). * * @unit m/s * @min 0.5 @@ -142,9 +141,6 @@ PARAM_DEFINE_FLOAT(FW_PR_D, 0.f); /** * Pitch rate integrator gain. * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * * @unit %/rad * @min 0.0 * @max 10 @@ -157,9 +153,6 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); /** * Pitch rate integrator limit * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * * @min 0.0 * @max 1.0 * @decimal 2 @@ -169,7 +162,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); /** - * Roll rate proportional Gain + * Roll rate proportional gain * * @unit %/rad/s * @min 0.0 @@ -181,10 +174,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); /** - * Roll rate derivative Gain - * - * Roll rate differential gain. Small values help reduce fast oscillations. - * If value is too big oscillations will appear again. + * Roll rate derivative gain * * @unit %/rad/s * @min 0.0 @@ -193,13 +183,10 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * @increment 0.005 * @group FW Rate Control */ -PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f); +PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); /** - * Roll rate integrator Gain - * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. + * Roll rate integrator gain * * @unit %/rad * @min 0.0 @@ -211,9 +198,7 @@ PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f); PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f); /** - * Roll integrator anti-windup - * - * The portion of the integrator part in the control surface deflection is limited to this value. + * Roll integrator limit * * @min 0.0 * @max 1.0 @@ -238,9 +223,6 @@ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); /** * Yaw rate derivative gain * - * Yaw rate differential gain. Small values help reduce fast oscillations. - * If value is too big oscillations will appear again. - * * @unit %/rad/s * @min 0.0 * @max 10 @@ -253,9 +235,6 @@ PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f); /** * Yaw rate integrator gain * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * * @unit %/rad * @min 0.0 * @max 10 @@ -268,9 +247,6 @@ PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f); /** * Yaw rate integrator limit * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * * @min 0.0 * @max 1.0 * @decimal 2 @@ -282,9 +258,7 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); /** * Roll rate feed forward * - * Direct feed forward from rate setpoint to control surface output. Use this - * to obtain a tigher response of the controller without introducing - * noise amplification. + * Direct feed forward from rate setpoint to control surface output. * * @unit %/rad/s * @min 0.0 @@ -324,10 +298,7 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); /** - * Acro body x max rate. - * - * This is the rate the controller is trying to achieve if the user applies full roll - * stick input in acro mode. + * Acro body roll max rate setpoint * * @min 10 * @max 720 @@ -337,7 +308,7 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); /** - * Acro body pitch max rate setpoint. + * Acro body pitch max rate setpoint * * @min 10 * @max 720 @@ -347,7 +318,7 @@ PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); /** - * Acro body yaw max rate setpoint. + * Acro body yaw max rate setpoint * * @min 10 * @max 720 diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 24ee9b772f..92048efc31 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -50,7 +50,7 @@ * * @min 0 * @max 1 - * @group Landing target Estimator + * @group Landing Target Estimator * @value 0 Moving * @value 1 Stationary */ @@ -66,7 +66,7 @@ PARAM_DEFINE_INT32(LTEST_MODE, 0); * @min 0.01 * @decimal 2 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f); @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f); * @unit tan(rad)^2 * @decimal 4 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f); @@ -92,7 +92,7 @@ PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f); * @min 0.001 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); @@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); * @min 0.001 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f); @@ -117,7 +117,7 @@ PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f); * @min 0.01 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); @@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); * @min 0.01 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 508a542754..9ece702888 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -600,7 +600,10 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().z = xLP(X_z); // down } - _pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + const float heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + _pub_lpos.get().heading = heading; + _pub_lpos.get().heading_good_for_control = PX4_ISFINITE(heading); + _pub_lpos.get().unaided_heading = NAN; _pub_lpos.get().vx = xLP(X_vx); // north _pub_lpos.get().vy = xLP(X_vy); // east diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3e729489a9..e9f3c3004a 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -330,7 +330,7 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); - add_topic("optical_flow"); + add_topic("vehicle_optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); add_topic("vehicle_air_data"); diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 01ccb41a77..d295d43744 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2023 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 @@ -141,6 +141,7 @@ px4_add_module( mavlink_c timesync tunes + variable_length_ringbuffer version UNITY_BUILD ) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index e04677782a..81524c2b34 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit e04677782ab086974be52104e27860fc760eff36 +Subproject commit 81524c2b34aa08768f13091b1d94c421e64f96c3 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f7ee306dd1..80bcc8f5a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -81,6 +81,7 @@ #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t mavlink_event_buffer_mutex = PTHREAD_MUTEX_INITIALIZER; events::EventBuffer *Mavlink::_event_buffer = nullptr; Mavlink *mavlink_module_instances[MAVLINK_COMM_NUM_BUFFERS] {}; @@ -178,6 +179,7 @@ Mavlink::~Mavlink() perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); + perf_free(_forwarding_error_perf); } void @@ -375,15 +377,21 @@ Mavlink::destroy_all_instances() } } - LockGuard lg{mavlink_module_mutex}; + { + LockGuard lg{mavlink_module_mutex}; - // we know all threads have exited, so it's safe to delete objects. - for (Mavlink *inst_to_del : mavlink_module_instances) { - delete inst_to_del; + // we know all threads have exited, so it's safe to delete objects. + for (Mavlink *inst_to_del : mavlink_module_instances) { + delete inst_to_del; + } } - delete _event_buffer; - _event_buffer = nullptr; + { + LockGuard lg{mavlink_event_buffer_mutex}; + + delete _event_buffer; + _event_buffer = nullptr; + } PX4_INFO("all instances stopped"); return OK; @@ -1217,117 +1225,16 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } -int -Mavlink::message_buffer_init(int size) -{ - _message_buffer.size = size; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - _message_buffer.data = (char *)malloc(_message_buffer.size); - - int ret; - - if (_message_buffer.data == nullptr) { - ret = PX4_ERROR; - _message_buffer.size = 0; - - } else { - ret = OK; - } - - return ret; -} - -void -Mavlink::message_buffer_destroy() -{ - _message_buffer.size = 0; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - free(_message_buffer.data); -} - -int -Mavlink::message_buffer_count() -{ - int n = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (n < 0) { - n += _message_buffer.size; - } - - return n; -} - -bool -Mavlink::message_buffer_write(const void *ptr, int size) -{ - // bytes available to write - int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; - - if (available < 0) { - available += _message_buffer.size; - } - - if (size > available) { - // buffer overflow - return false; - } - - char *c = (char *) ptr; - int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer - - if (n < size) { - // message goes over end of the buffer - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); - _message_buffer.write_ptr = 0; - - } else { - n = 0; - } - - // now: n = bytes already written - int p = size - n; // number of bytes to write - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); - _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; - return true; -} - -int -Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) -{ - // bytes available to read - int available = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (available == 0) { - return 0; // buffer is empty - } - - int n = 0; - - if (available > 0) { - // read pointer is before write pointer, all available bytes can be read - n = available; - *is_part = false; - - } else { - // read pointer is after write pointer, read bytes from read_ptr to end of the buffer - n = _message_buffer.size - _message_buffer.read_ptr; - *is_part = _message_buffer.write_ptr > 0; - } - - *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); - return n; -} - void Mavlink::pass_message(const mavlink_message_t *msg) { - /* size is 8 bytes plus variable payload */ + /* size is 12 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_write(msg, size); - pthread_mutex_unlock(&_message_buffer_mutex); + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.push_back(reinterpret_cast(msg), size)) { + perf_count(_forwarding_error_perf); + } } MavlinkShell * @@ -1507,7 +1414,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GLOBAL_POSITION_INT", 5.0f); configure_stream_local("GPS2_RAW", 1.0f); configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); - configure_stream_local("GPS_RAW_INT", 1.0f); + configure_stream_local("GPS_RAW_INT", 5.0f); configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("HYGROMETER_SENSOR", 0.1f); @@ -2210,7 +2117,7 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* initialize send mutex */ + pthread_mutex_init(&_message_buffer_mutex, nullptr); pthread_mutex_init(&_send_mutex, nullptr); pthread_mutex_init(&_radio_status_mutex, nullptr); @@ -2220,13 +2127,12 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.allocate(2 * sizeof(mavlink_message_t) + 1)) { PX4_ERR("msg buf alloc fail"); return PX4_ERROR; } - - /* initialize message buffer mutex */ - pthread_mutex_init(&_message_buffer_mutex, nullptr); } /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ @@ -2543,7 +2449,7 @@ Mavlink::task_main(int argc, char *argv[]) /* handle new events */ if (check_events()) { if (_event_sub.updated()) { - LockGuard lg{mavlink_module_mutex}; + LockGuard lg{mavlink_event_buffer_mutex}; event_s orb_event; @@ -2568,50 +2474,21 @@ Mavlink::task_main(int argc, char *argv[]) _events.update(t); - /* pass messages from other UARTs */ + /* pass messages from other instances */ if (get_forwarding_on()) { - bool is_part; - uint8_t *read_ptr; - uint8_t *write_ptr; - - pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - if (available > 0) { - // Reconstruct message from buffer - - mavlink_message_t msg; - write_ptr = (uint8_t *)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - - message_buffer_mark_read(read_count); - - /* write second part of buffer if there is some */ - if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); - message_buffer_mark_read(available); - } - - pthread_mutex_unlock(&_message_buffer_mutex); + mavlink_message_t msg; + size_t available_bytes; + { + // We only send one message at a time, not to put too much strain on a + // link from forwarded messages. + LockGuard lg{_message_buffer_mutex}; + available_bytes = _message_buffer.pop_front(reinterpret_cast(&msg), sizeof(msg)); + // We need to make sure to release the lock here before sending the + // bytes out via IP or UART which could potentially take longer. + } + if (available_bytes > 0) { resend_message(&msg); } } @@ -2661,11 +2538,6 @@ Mavlink::task_main(int argc, char *argv[]) _socket_fd = -1; } - if (get_forwarding_on()) { - message_buffer_destroy(); - pthread_mutex_destroy(&_message_buffer_mutex); - } - if (_mavlink_ulog) { _mavlink_ulog->stop(); _mavlink_ulog = nullptr; @@ -2673,6 +2545,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_destroy(&_send_mutex); pthread_mutex_destroy(&_radio_status_mutex); + pthread_mutex_destroy(&_message_buffer_mutex); PX4_INFO("exiting channel %i", (int)_channel); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3..1ba71c6a2b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -419,11 +420,6 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - /** * Count transmitted bytes */ @@ -651,16 +647,9 @@ private: ping_statistics_s _ping_stats {}; - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; + pthread_mutex_t _message_buffer_mutex{}; + VariableLengthRingbuffer _message_buffer{}; - mavlink_message_buffer _message_buffer {}; - - pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; pthread_mutex_t _radio_status_mutex {}; @@ -682,6 +671,7 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")}; /**< loop interval performance counter */ perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")}; /**< send bytes error count */ + perf_counter_t _forwarding_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": forwarding error")}; /**< forwarding messages error count */ void mavlink_update_parameters(); @@ -711,18 +701,6 @@ private: */ int configure_streams_to_default(const char *configure_single_stream = nullptr); - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); } - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; } - void pass_message(const mavlink_message_t *msg); void publish_telemetry_status(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 43f7fab016..ef8f1c3b55 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -228,8 +228,13 @@ static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 == static_cast(ROTATION_PITCH_315), "Pitch: 315"); static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast(ROTATION_ROLL_90_PITCH_315), "Roll: 90, Pitch: 315"); + +// Note: Update the number (41, as of writing) below to the number of 'normal' rotation enums in MAVLink spec: +// https://mavlink.io/en/messages/common.html#MAV_SENSOR_ORIENTATION static_assert(41 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync"); +static_assert(MAV_SENSOR_ROTATION_CUSTOM == static_cast(ROTATION_CUSTOM), "Custom Rotation"); + static const StreamListItem streams_list[] = { #if defined(HEARTBEAT_HPP) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b20480e791..95ee75817d 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -74,12 +74,6 @@ uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink) -{ - init_offboard_mission(); -} - -void -MavlinkMissionManager::init_offboard_mission() { if (!_dataman_init) { _dataman_init = true; @@ -89,23 +83,29 @@ MavlinkMissionManager::init_offboard_mission() sizeof(mission_s)); if (success) { - _dataman_id = (dm_item_t)mission_state.dataman_id; - _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; - _current_seq = mission_state.current_seq; - _mission_update_counter = mission_state.mission_update_counter; + init_offboard_mission(mission_state); + load_geofence_stats(); + load_safepoint_stats(); } else { PX4_WARN("offboard mission init failed"); } - - load_geofence_stats(); - - load_safepoint_stats(); } _my_dataman_id = _dataman_id; } +void +MavlinkMissionManager::init_offboard_mission(mission_s mission_state) +{ + _dataman_id = (dm_item_t)mission_state.dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; + _current_seq = mission_state.current_seq; + _land_start_marker = mission_state.land_start_index; + _land_marker = mission_state.land_index; + _mission_update_counter = mission_state.mission_update_counter; +} + bool MavlinkMissionManager::load_geofence_stats() { @@ -142,7 +142,7 @@ MavlinkMissionManager::load_safepoint_stats() * Publish mission topic to notify navigator about changes. */ void -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman) { mission_s mission{}; mission.timestamp = hrt_absolute_time(); @@ -152,6 +152,8 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun mission.mission_update_counter = _mission_update_counter; mission.geofence_update_counter = _geofence_update_counter; mission.safe_points_update_counter = _safepoint_update_counter; + mission.land_start_index = _land_start_marker; + mission.land_index = _land_marker; /* update active mission state */ _dataman_id = dataman_id; @@ -160,6 +162,15 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun _my_dataman_id = _dataman_id; _offboard_mission_pub.publish(mission); + + if (write_to_dataman) { + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), + sizeof(mission_s)); + + if (!success) { + PX4_ERR("Can't update mission state in Dataman"); + } + } } int @@ -187,7 +198,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) return PX4_ERROR; } - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq); + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false); return PX4_OK; } @@ -216,7 +227,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) return PX4_ERROR; } - update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq); + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false); return PX4_OK; } @@ -460,8 +471,8 @@ MavlinkMissionManager::send() } else { _mavlink->send_statustext_critical("ERROR: wp index out of bounds\t"); - events::send(events::ID("mavlink_mission_wp_index_out_of_bounds"), events::Log::Error, - "Waypoint index out of bounds ({1} \\< {2})", mission_result.seq_current, mission_result.seq_total); + events::send(events::ID("mavlink_mission_wp_index_out_of_bounds"), events::Log::Error, + "Waypoint index out of bounds (current {1} \\>= total {2})", mission_result.seq_current, mission_result.seq_total); } } } @@ -873,6 +884,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: + _land_start_marker = -1; + _land_marker = -1; ++_mission_update_counter; /* alternate dataman ID anyway to let navigator know about changes */ @@ -914,6 +927,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission _transfer_current_seq = -1; + _transfer_land_start_marker = -1; + _transfer_land_marker = -1; } else if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -1068,6 +1083,21 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), sizeof(struct mission_item_s)); + // Check for land start marker + if ((mission_item.nav_cmd == MAV_CMD_DO_LAND_START) && (_transfer_land_start_marker == -1)) { + _transfer_land_start_marker = wp.seq; + } + + // Check for land index + if (((mission_item.nav_cmd == MAV_CMD_NAV_VTOL_LAND) || (mission_item.nav_cmd == MAV_CMD_NAV_LAND)) + && (_transfer_land_marker == -1)) { + _transfer_land_marker = wp.seq; + + if (_transfer_land_start_marker == -1) { + _transfer_land_start_marker = _transfer_land_marker; + } + } + if (!write_failed) { /* waypoint marked as current */ if (wp.current) { @@ -1162,6 +1192,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: ++_mission_update_counter; + _land_start_marker = _transfer_land_start_marker; + _land_marker = _transfer_land_marker; update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); break; @@ -1217,6 +1249,8 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: ++_mission_update_counter; + _land_start_marker = -1; + _land_marker = -1; update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); break; @@ -1231,6 +1265,8 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) case MAV_MISSION_TYPE_ALL: ++_mission_update_counter; + _land_start_marker = -1; + _land_marker = -1; update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); ret = update_geofence_count(0); @@ -1525,6 +1561,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->autocontinue = mission_item->autocontinue; + mavlink_mission_item->mission_type = _mission_type; /* default mappings for generic commands */ if (mission_item->frame == MAV_FRAME_MISSION) { @@ -1733,11 +1770,24 @@ void MavlinkMissionManager::check_active_mission() return; } - if (!(_my_dataman_id == _dataman_id)) { - PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); + if (_mission_sub.updated()) { + _mission_sub.update(); - _my_dataman_id = _dataman_id; - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], - MAV_MISSION_TYPE_MISSION); + if (_mission_sub.get().geofence_update_counter != _geofence_update_counter) { + load_geofence_stats(); + } + + if (_mission_sub.get().safe_points_update_counter != _safepoint_update_counter) { + load_safepoint_stats(); + } + + if ((_mission_sub.get().mission_update_counter != _mission_update_counter) + || (_my_dataman_id != (dm_item_t)_mission_sub.get().dataman_id)) { + PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); + init_offboard_mission(_mission_sub.get()); + _my_dataman_id = _dataman_id; + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], + MAV_MISSION_TYPE_MISSION); + } } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index d66eddc524..b4bb8bccaa 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -125,16 +125,21 @@ private: uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission + int32_t _transfer_land_start_marker{-1}; ///< index of land start mission item in current transmission (if unavailable, index of land mission item, -1 otherwise) + int32_t _transfer_land_marker{-1}; ///< index of land mission item in current transmission (-1 if unavailable) static bool _transfer_in_progress; ///< Global variable checking for current transmission uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; static uint16_t _mission_update_counter; static uint16_t _geofence_update_counter; static uint16_t _safepoint_update_counter; + int32_t _land_start_marker{-1}; ///< index of loaded land start mission item (if unavailable, index of land mission item, -1 otherwise) + int32_t _land_marker{-1}; ///< index of loaded land mission item (-1 if unavailable) MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz @@ -159,9 +164,9 @@ private: MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager &operator = (const MavlinkMissionManager &); - void init_offboard_mission(); + void init_offboard_mission(mission_s mission_state); - void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman = true); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d0a0791d12..de603c6b17 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2713,6 +2713,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; hil_local_pos.heading = euler.psi(); + hil_local_pos.heading_good_for_control = PX4_ISFINITE(euler.psi()); + hil_local_pos.unaided_heading = NAN; hil_local_pos.xy_global = true; hil_local_pos.z_global = true; hil_local_pos.vxy_max = INFINITY; @@ -3267,7 +3269,9 @@ MavlinkReceiver::run() _mission_manager.check_active_mission(); _mission_manager.send(); - _parameters_manager.send(); + if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) { + _parameters_manager.send(); + } if (_mavlink->ftp_enabled()) { _mavlink_ftp.send(); diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index deb5286dfd..723c535781 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -186,7 +186,13 @@ int MavlinkShell::shell_start_thread(int argc, char *argv[]) #ifdef __PX4_NUTTX dup2(1, 2); //redirect stderror to stdout - nsh_consolemain(0, NULL); + const int ret = nsh_consolemain(0, NULL); + + if (ret) { + PX4_ERR("Mavlink shell failed: %d%s", ret, (ret == -ENOMEM) ? " (out of memory)" : ""); + return ret; + } + #endif /* __PX4_NUTTX */ #ifdef __PX4_POSIX diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 709eefb12d..73068f0de3 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -142,16 +142,41 @@ private: static constexpr int mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); static constexpr int mavlink_cell_slots_extension = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0])); - uint16_t cell_voltages[mavlink_cell_slots + mavlink_cell_slots_extension]; - for (auto &voltage : cell_voltages) { - voltage = UINT16_MAX; + // Fill defaults first, voltage fields 1-10 + for (int i = 0; i < mavlink_cell_slots; ++i) { + bat_msg.voltages[i] = UINT16_MAX; + } + + // And extensions fields 11-14: 0 if unused for backwards compatibility and 0 truncation. + for (int i = 0; i < mavlink_cell_slots_extension; ++i) { + bat_msg.voltages_ext[i] = 0; } if (battery_status.connected) { - // We don't know the cell count or we don't know the indpendent cell voltages so we report the total voltage in the first cell + // We don't know the cell count or we don't know the indpendent cell voltages, + // so we report the total voltage in the first cell, or cell(s) if the voltage + // doesn't "fit" in the UINT16. if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) { - cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f; + // If it doesn't fit, we have to split it into UINT16-1 chunks and the remaining + // voltage for the subsequent field. + // This won't work for voltages of more than 655 volts. + const int num_fields_required = static_cast(battery_status.voltage_filtered_v * 1000.f) / (UINT16_MAX - 1) + 1; + + if (num_fields_required <= mavlink_cell_slots) { + float remaining_voltage = battery_status.voltage_filtered_v * 1000.f; + + for (int i = 0; i < num_fields_required - 1; ++i) { + bat_msg.voltages[i] = UINT16_MAX - 1; + remaining_voltage -= UINT16_MAX - 1; + } + + bat_msg.voltages[num_fields_required - 1] = remaining_voltage; + + } else { + // Leave it default/unknown. We're out of spec. + } + } else { static constexpr int uorb_cell_slots = @@ -161,22 +186,17 @@ private: uorb_cell_slots, mavlink_cell_slots + mavlink_cell_slots_extension); - for (int cell = 0; cell < cell_slots; cell++) { - cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f; + for (int i = 0; i < cell_slots; ++i) { + if (i < mavlink_cell_slots) { + bat_msg.voltages[i] = battery_status.voltage_cell_v[i] * 1000.f; + + } else if ((i - mavlink_cell_slots) < mavlink_cell_slots_extension) { + bat_msg.voltages_ext[i - mavlink_cell_slots] = battery_status.voltage_cell_v[i] * 1000.f; + } } } } - // voltage fields 1-10 - for (int cell = 0; cell < mavlink_cell_slots; cell++) { - bat_msg.voltages[cell] = cell_voltages[cell]; - } - - // voltage fields 11-14 into the extension - for (int cell = 0; cell < mavlink_cell_slots_extension; cell++) { - bat_msg.voltages_ext[cell] = cell_voltages[mavlink_cell_slots + cell]; - } - mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); updated = true; } diff --git a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp index 2cf17b69f2..b58852dbaf 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -101,8 +101,8 @@ private: msg.lon = global_pos.lon * 1e7; msg.alt = global_pos.alt_ellipsoid * 1000.f; - msg.h_acc = global_pos.eph * 1000.f; - msg.v_acc = global_pos.epv * 1000.f; + msg.h_acc = math::min(global_pos.eph * 1000.0f, (float)UINT16_MAX); + msg.v_acc = math::min(global_pos.epv * 1000.0f, (float)UINT16_MAX); msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index e09be516e1..bbdfe0d3c8 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -94,12 +94,18 @@ private: _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0); _vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint_1); + const float thrust_magnitude_0 = sqrtf(vehicle_thrust_setpoint_0.xyz[0] * vehicle_thrust_setpoint_0.xyz[0] + + vehicle_thrust_setpoint_0.xyz[1] * vehicle_thrust_setpoint_0.xyz[1] + + vehicle_thrust_setpoint_0.xyz[2] * vehicle_thrust_setpoint_0.xyz[2]); + + const float thrust_magnitude_1 = sqrtf(vehicle_thrust_setpoint_1.xyz[0] * vehicle_thrust_setpoint_1.xyz[0] + + vehicle_thrust_setpoint_1.xyz[1] * vehicle_thrust_setpoint_1.xyz[1] + + vehicle_thrust_setpoint_1.xyz[2] * vehicle_thrust_setpoint_1.xyz[2]); + // VFR_HUD throttle should only be used for operator feedback. // VTOLs switch between vehicle_thrust_setpoint_0 and vehicle_thrust_setpoint_1. During transition there isn't a // a single throttle value, but this should still be a useful heuristic for operator awareness. - msg.throttle = 100 * math::max( - -vehicle_thrust_setpoint_0.xyz[2], - vehicle_thrust_setpoint_1.xyz[0]); + msg.throttle = 100.f * math::max(thrust_magnitude_0, thrust_magnitude_1); } else { msg.throttle = 0.0f; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ca4482d487..31ca3eafa0 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -50,10 +50,12 @@ #include #include #include +#include #include #include #include #include +#include #include @@ -100,6 +102,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -118,12 +121,16 @@ private: float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air + SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up AlphaFilter _man_roll_input_filter; AlphaFilter _man_pitch_input_filter; hrt_abstime _last_run{0}; hrt_abstime _last_attitude_setpoint{0}; + bool _spooled_up{false}; ///< used to make sure the vehicle cannot take off during the spoolup time + bool _landed{true}; bool _reset_yaw_sp{true}; bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published bool _vehicle_type_rotary_wing{true}; @@ -152,7 +159,9 @@ private: (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ (ParamFloat) _param_mpc_thr_hover, /**< throttle at stationary hover */ - (ParamInt) _param_mpc_thr_curve /**< throttle curve behavior */ + (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ + + (ParamFloat) _param_com_spoolup_time ) }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 5925237219..35c65d5911 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -60,8 +60,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _vtol(vtol) { - parameters_updated(); + // Rate of change 5% per second -> 1.6 seconds to ramp to default 8% MPC_MANTHR_MIN + _manual_throttle_minimum.setSlewRate(0.05f); + // Rate of change 50% per second -> 2 seconds to ramp to 100% + _manual_throttle_maximum.setSlewRate(0.5f); } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -98,14 +101,21 @@ MulticopterAttitudeControl::parameters_updated() float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { - // throttle_stick_input is in range [0, 1] + float thrust = 0.f; + switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle - return math::interpolate(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get()); + thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, + _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); + break; - default: // 0 or other: rescale to hover throttle at 0.5 stick - return math::interpolateN(throttle_stick_input, {_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle + thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + break; } + + return math::min(thrust, _manual_throttle_maximum.getState()); } void @@ -178,9 +188,9 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, attitude_setpoint.pitch_body = euler_sp(1); attitude_setpoint.yaw_body = euler_sp(2); - attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f); - attitude_setpoint.timestamp = hrt_absolute_time(); + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); + attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); // update attitude controller setpoint immediately @@ -262,6 +272,16 @@ MulticopterAttitudeControl::Run() _vtol_in_transition_mode = vehicle_status.in_transition_mode; _vtol_tailsitter = vehicle_status.is_vtol_tailsitter; + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s; + } + } + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; } } @@ -324,6 +344,20 @@ MulticopterAttitudeControl::Run() _vehicle_rates_setpoint_pub.publish(rates_setpoint); } + if (_landed) { + _manual_throttle_minimum.update(0.f, dt); + + } else { + _manual_throttle_minimum.update(_param_mpc_manthr_min.get(), dt); + } + + if (_spooled_up) { + _manual_throttle_maximum.update(1.f, dt); + + } else { + _manual_throttle_maximum.setForcedValue(0.f); + } + // reset yaw setpoint during transitions, tailsitter.cpp generates // attitude setpoint for the transition _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control || (_vtol && _vtol_in_transition_mode); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 1371523c8d..955f3f4b44 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -171,9 +171,9 @@ void PositionControl::_velocityControl(const float dt) // Determine how much horizontal thrust is left after prioritizing vertical control const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2)); - float thrust_max_xy = 0; + float thrust_max_xy = 0.f; - if (thrust_max_xy_squared > 0) { + if (thrust_max_xy_squared > 0.f) { thrust_max_xy = sqrtf(thrust_max_xy_squared); } diff --git a/src/modules/mc_rate_control/mc_acro_params.c b/src/modules/mc_rate_control/mc_acro_params.c new file mode 100644 index 0000000000..ef9d315d0c --- /dev/null +++ b/src/modules/mc_rate_control/mc_acro_params.c @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 mc_acro_params.c + * + * Parameters for Acro mode behavior + */ + +/** + * Acro mode maximum roll rate + * + * Full stick deflection leads to this rate. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 100.f); + +/** + * Acro mode maximum pitch rate + * + * Full stick deflection leads to this rate. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 100.f); + +/** + * Acro mode maximum yaw rate + * + * Full stick deflection leads to this rate. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 100.f); + +/** + * Acro mode roll, pitch expo factor + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.f); + +/** + * Acro mode yaw expo factor + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.f); + +/** + * Acro mode roll, pitch super expo factor + * + * "Superexponential" factor for refining the input curve shape tuned using MC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.f); + +/** + * Acro mode yaw super expo factor + * + * "Superexponential" factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.f); diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 55e92f9be8..2571a70c1f 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -33,10 +33,8 @@ /** * @file mc_rate_control_params.c - * Parameters for multicopter attitude controller. * - * @author Lorenz Meier - * @author Anton Babushkin + * Parameters for multicopter rate controller */ /** @@ -281,110 +279,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); -/** - * Max acro roll rate - * - * default: 2 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); - -/** - * Max acro pitch rate - * - * default: 2 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); - -/** - * Max acro yaw rate - * - * default 1.5 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); - -/** - * Acro mode Expo factor for Roll and Pitch. - * - * Exponential factor for tuning the input curve shape. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); - -/** - * Acro mode Expo factor for Yaw. - * - * Exponential factor for tuning the input curve shape. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); - -/** - * Acro mode SuperExpo factor for Roll and Pitch. - * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. - * - * 0 Pure Expo function - * 0.7 reasonable shape enhancement for intuitive stick feel - * 0.95 very strong bent input curve only near maxima have effect - * - * @min 0 - * @max 0.95 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); - -/** - * Acro mode SuperExpo factor for Yaw. - * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. - * - * 0 Pure Expo function - * 0.7 reasonable shape enhancement for intuitive stick feel - * 0.95 very strong bent input curve only near maxima have effect - * - * @min 0 - * @max 0.95 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); - /** * Battery power level scaler * diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 4532c08d17..b996566def 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -34,22 +34,34 @@ add_subdirectory(GeofenceBreachAvoidance) add_subdirectory(MissionFeasibility) +set(NAVIGATOR_SOURCES + navigator_main.cpp + navigator_mode.cpp + mission_base.cpp + mission_block.cpp + mission.cpp + loiter.cpp + rtl.cpp + rtl_direct.cpp + rtl_direct_mission_land.cpp + rtl_mission_fast.cpp + rtl_mission_fast_reverse.cpp + takeoff.cpp + land.cpp + precland.cpp + mission_feasibility_checker.cpp + geofence.cpp) + +if(CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF) + set(NAVIGATOR_SOURCES + ${NAVIGATOR_SOURCES} + vtol_takeoff.cpp) +endif() + px4_add_module( MODULE modules__navigator MAIN navigator - SRCS - navigator_main.cpp - navigator_mode.cpp - mission_block.cpp - mission.cpp - loiter.cpp - rtl.cpp - takeoff.cpp - land.cpp - precland.cpp - mission_feasibility_checker.cpp - geofence.cpp - vtol_takeoff.cpp + SRCS ${NAVIGATOR_SOURCES} DEPENDS dataman_client geo diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp index f84e24e74b..802dacaaf6 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp @@ -159,7 +159,7 @@ GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_typ Vector2d test_point; // binary search for the distance from the drone to the geofence in the given direction - while (abs(current_max - current_min) > 0.5f) { + while (fabsf(current_max - current_min) > 0.5f) { test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance); if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) { diff --git a/src/modules/navigator/Kconfig b/src/modules/navigator/Kconfig index 422748e048..83e42cc594 100644 --- a/src/modules/navigator/Kconfig +++ b/src/modules/navigator/Kconfig @@ -10,3 +10,12 @@ menuconfig USER_NAVIGATOR depends on BOARD_PROTECTED && MODULES_NAVIGATOR ---help--- Put navigator in userspace memory + +menuconfig MODE_NAVIGATOR_VTOL_TAKEOFF + bool "Include VTOL takeoff mode support" + default n + depends on MODULES_NAVIGATOR + ---help--- + Add VTOL takeoff mode to enable support for MAV_CMD_NAV_VTOL_TAKEOFF. + The VTOL takes off in MC mode and transition to FW. The mode ends with + an infinite loiter diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index d280c942c6..08513bfa9f 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -62,8 +62,6 @@ Land::on_activation() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(false); - _navigator->set_position_setpoint_triplet_updated(); // reset cruising speed to default diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index a657f4ade4..66fadb10c4 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -51,7 +51,8 @@ Loiter::Loiter(Navigator *navigator) : void Loiter::on_activation() { - if (_navigator->get_reposition_triplet()->current.valid) { + if (_navigator->get_reposition_triplet()->current.valid + && hrt_elapsed_time(&_navigator->get_reposition_triplet()->current.timestamp) < 500_ms) { reposition(); } else { @@ -66,7 +67,8 @@ Loiter::on_activation() void Loiter::on_active() { - if (_navigator->get_reposition_triplet()->current.valid) { + if (_navigator->get_reposition_triplet()->current.valid + && hrt_elapsed_time(&_navigator->get_reposition_triplet()->current.timestamp) < 500_ms) { reposition(); } } @@ -80,7 +82,6 @@ Loiter::set_loiter_position() // Not setting loiter position if disarmed and landed, instead mark the current // setpoint as invalid and idle (both, just to be sure). - _navigator->set_can_loiter_at_sp(false); _navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; _navigator->set_position_setpoint_triplet_updated(); return; @@ -108,7 +109,6 @@ Loiter::set_loiter_position() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } @@ -134,7 +134,6 @@ Loiter::reposition() memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); // mark this as done diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c8b2979087..9daa37a733 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,7 +50,6 @@ #include #include -#include #include #include #include @@ -63,509 +62,43 @@ using namespace time_literals; +static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; + Mission::Mission(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) { - mission_init(); -} - -void -Mission::run() -{ - if ((_mission.count > 0) && (_current_mission_index != _load_mission_index)) { - - uint32_t start_index = _current_mission_index; - uint32_t end_index = start_index + DATAMAN_CACHE_SIZE; - - end_index = math::min(end_index, static_cast(_mission.count)); - - for (uint32_t index = start_index; index < end_index; ++index) { - - _dataman_cache.load(static_cast(_mission.dataman_id), index); - } - - _load_mission_index = _current_mission_index; - } - - _dataman_cache.update(); -} - -void Mission::mission_init() -{ - // init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start - - if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), sizeof(mission_s))) { - if ((_mission.timestamp != 0) - && (_mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { - if (_mission.count > 0) { - PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", _mission.dataman_id, _mission.count); - } - - } else { - // initialize mission state in dataman - _mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - _mission.timestamp = hrt_absolute_time(); - _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), sizeof(mission_s)); - } - } - - _current_mission_index = _mission.current_seq; - } void Mission::on_inactive() { - /* Without home a mission can't be valid yet anyway, let's wait. */ - if (!_navigator->home_global_position_valid()) { - return; - } + _vehicle_status_sub.update(); - if (_need_mission_save && _navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (_need_mission_save && _vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { save_mission_state(); } - if (_inited) { - if (_mission_sub.updated()) { - update_mission(); - - if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { - _mission_type = MISSION_TYPE_MISSION; - } - } - - /* reset the current mission if needed */ - if (need_to_reset_mission()) { - reset_mission(_mission); - _navigator->reset_cruising_speed(); - _current_mission_index = 0; - _navigator->reset_vroi(); - set_current_mission_item(); - } - - } else { - - /* load missions from storage */ - mission_s mission_state = {}; - - /* read current state */ - bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), - sizeof(mission_s)); - - if (success) { - _mission.dataman_id = mission_state.dataman_id; - _mission.count = mission_state.count; - _current_mission_index = mission_state.current_seq; - - // find and store landing start marker (if available) - find_mission_land_start(); - } - - /* On init let's check the mission, maybe there is already one available. */ - check_mission_valid(false); - - _inited = true; - } - - /* require takeoff after non-loiter or landing */ - if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) { - _need_takeoff = true; - } - - /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - - /* reset so MISSION_ITEM_REACHED isn't published */ - _navigator->get_mission_result()->seq_reached = -1; -} - -void -Mission::on_inactivation() -{ - _navigator->disable_camera_trigger(); - _navigator->stop_capturing_images(); - _navigator->release_gimbal_control(); - - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - - /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - - _inactivation_index = _current_mission_index; + MissionBase::on_inactive(); } void Mission::on_activation() { - if (_mission_waypoints_changed) { - // do not set the closest mission item in the normal mission mode - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { - _current_mission_index = index_closest_mission_item(); - } + _need_mission_save = true; - _mission_waypoints_changed = false; - } - - // we already reset the mission items - _execution_mode_changed = false; - - // reset the cache and fill it with the items up to the previous item. The cache contains - // commands that are valid for the whole mission, not just a sinlge waypoint. - if (_current_mission_index > 0) { - resetItemCache(); - updateCachedItemsUpToIndex(_current_mission_index - 1); - } - - unsigned resume_index; - - if (_inactivation_index > 0 && cameraWasTriggering() - && getPreviousPositionItemIndex(_mission, _inactivation_index - 1, resume_index)) { - // The mission we are resuming had camera triggering enabled. In order to not lose any images - // we restart the mission at the previous position item. - // We will replay the cached commands once we reach the previous position item and have yaw aligned. - set_current_mission_index(resume_index); - - _align_heading_necessary = true; - - } else { - set_mission_items(); - } - - _inactivation_index = -1; // reset - - // reset cruise speed - _navigator->reset_cruising_speed(); -} - -void -Mission::on_active() -{ - check_mission_valid(false); - - /* Check if stored mission plan has changed */ - const bool mission_sub_updated = _mission_sub.updated(); - - if (mission_sub_updated) { - _navigator->reset_triplets(); - update_mission(); - } - - /* mission is running (and we are armed), need reset after disarm */ - _need_mission_reset = true; - - _mission_changed = false; - - /* reset mission items if needed */ - if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) { - if (_mission_waypoints_changed) { - // do not set the closest mission item in the normal mission mode - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { - _current_mission_index = index_closest_mission_item(); - } - - _mission_waypoints_changed = false; - } - - _execution_mode_changed = false; - set_mission_items(); - } - - // check if heading alignment is necessary, and add it to the current mission item if necessary - if (_align_heading_necessary && is_mission_item_reached_or_completed()) { - mission_item_s next_position_mission_item = {}; - - // add yaw alignment requirement on the current mission item - if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_position_mission_item) - && !PX4_ISFINITE(_mission_item.yaw)) { - _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, - next_position_mission_item.lat, next_position_mission_item.lon)); - _mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode - } - - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); - - reset_mission_item_reached(); - - _navigator->set_position_setpoint_triplet_updated(); - _align_heading_necessary = false; - } - - // replay gimbal and camera commands immediately after resuming mission - if (haveCachedGimbalOrCameraItems()) { - replayCachedGimbalCameraItems(); - } - - // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set - if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { - replayCachedTriggerItems(); - } - - replayCachedSpeedChangeItems(); - - /* lets check if we reached the current mission item */ - if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { - /* If we just completed a takeoff which was inserted before the right waypoint, - there is no need to report that we reached it because we didn't. */ - if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) { - set_mission_item_reached(); - } - - if (_mission_item.autocontinue) { - /* switch to next waypoint if 'autocontinue' flag set */ - advance_mission(); - set_mission_items(); - } - - } else { - /* if waypoint position reached allow loiter on the setpoint */ - if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { - _navigator->set_can_loiter_at_sp(true); - } - } - - /* see if we need to update the current yaw heading */ - if (!_param_mis_mnt_yaw_ctl.get() - && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) - && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) - && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - || _mission_item.nav_cmd == NAV_CMD_LAND - || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND - || _work_item_type == WORK_ITEM_TYPE_ALIGN)) { - // Mount control is disabled If the vehicle is in ROI-mode, the vehicle - // needs to rotate such that ROI is in the field of view. - // ROI only makes sense for multicopters. - heading_sp_update(); - } - - // TODO: Add vtol heading update method if required. - // Question: Why does vtol ever have to update heading? - - /* check if landing needs to be aborted */ - if ((_mission_item.nav_cmd == NAV_CMD_LAND) - && (_navigator->abort_landing())) { - - do_abort_landing(); - } - - if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } + MissionBase::on_activation(); } bool -Mission::set_current_mission_index(uint16_t index) -{ - if (index == _current_mission_index) { - return true; // nothing to do, so return true - - } else if (_navigator->get_mission_result()->valid && (index < _mission.count)) { - - _current_mission_index = index; - - // we start from the first item so can reset the cache - if (_current_mission_index == 0) { - resetItemCache(); - } - - // a mission index is set manually which has the higher priority than the closest mission item - // as it is set by the user - _mission_waypoints_changed = false; - - // update mission items if already in active mission - if (_navigator->is_planned_mission()) { - // prevent following "previous - current" line - _navigator->get_position_setpoint_triplet()->previous.valid = false; - _navigator->get_position_setpoint_triplet()->current.valid = false; - _navigator->get_position_setpoint_triplet()->next.valid = false; - set_mission_items(); - } - - return true; - } - - return false; -} - -void -Mission::set_closest_item_as_current() -{ - _current_mission_index = index_closest_mission_item(); -} - -void -Mission::set_execution_mode(const uint8_t mode) -{ - if (_mission_execution_mode != mode) { - _execution_mode_changed = true; - _navigator->get_mission_result()->execution_mode = mode; - - - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: - if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - // command a transition if in vtol mc mode - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - _navigator->get_vstatus()->is_vtol && - !_navigator->get_land_detected()->landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous = pos_sp_triplet->current; - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); - issue_command(_mission_item); - } - - if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { - _mission_type = MISSION_TYPE_MISSION; - } - - if (_current_mission_index > _mission.count - 1) { - _current_mission_index = _mission.count - 1; - - } else if (_current_mission_index > 0) { - --_current_mission_index; - } - - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - } - - break; - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: - if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) || - (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) { - // handle switch from reverse to forward mission - if (_current_mission_index < 0) { - _current_mission_index = 0; - resetItemCache(); // reset cache as we start from the beginning - - } else if (_current_mission_index < _mission.count - 1) { - ++_current_mission_index; - } - - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - } - - break; - - } - - _mission_execution_mode = mode; - } -} - -bool -Mission::find_mission_land_start() -{ - /* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index - * return false if not found - */ - - const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; - struct mission_item_s missionitem = {}; - struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START - - _land_start_available = false; - - bool found_land_start_marker = false; - - for (size_t i = 1; i < _mission.count; i++) { - missionitem_prev = missionitem; // store the last mission item before reading a new one - - bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s), 500_ms); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - found_land_start_marker = true; - } - - if (found_land_start_marker && !_land_start_available && item_contains_position(missionitem)) { - // use the position of any waypoint after the land start marker which specifies a position. - _landing_start_lat = missionitem.lat; - _landing_start_lon = missionitem.lon; - _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + - _navigator->get_home_position()->alt : missionitem.altitude; - _landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius) - && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) : - _navigator->get_loiter_radius(); - _land_start_available = true; - _land_start_index = i; // set it to the first item containing a position after the land start marker was found - } - - if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || - (missionitem.nav_cmd == NAV_CMD_LAND)) { - - _landing_lat = missionitem.lat; - _landing_lon = missionitem.lon; - _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : - missionitem.altitude; - - // don't have a valid land start yet, use the landing item itself then - if (!_land_start_available) { - _land_start_index = i; - _landing_start_lat = _landing_lat; - _landing_start_lon = _landing_lon; - _landing_start_alt = _landing_alt; - _land_start_available = true; - } - - } - } - - return _land_start_available; -} - -bool -Mission::land_start() -{ - // if not currently landing, jump to do_land_start - if (_land_start_available) { - // check if we're currently already in mission mode and on landing part, then simply return true. - // note: it's not enough to check landing(), as that is not reset until set_current_mission_index(get_land_start_index()) - if (_navigator->on_mission_landing()) { - return true; - - } else { - set_current_mission_index(get_land_start_index()); - return landing(); - } - } - - return false; -} - -bool -Mission::landing() +Mission::isLanding() { // vehicle is currently landing if // mission valid, still flying, and in the landing portion of mission (past land start marker) - - const bool mission_valid = _navigator->get_mission_result()->valid; - bool on_landing_stage = _land_start_available && _current_mission_index > get_land_start_index(); + bool on_landing_stage = get_land_start_available() && _mission.current_seq > get_land_start_index(); // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the // distance to the WP is below the loiter radius + acceptance. - if (_current_mission_index == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + if (_mission.current_seq == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); @@ -577,801 +110,59 @@ Mission::landing() on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); } - return mission_valid && on_landing_stage; -} - -void -Mission::update_mission() -{ - - bool failed = !_navigator->get_mission_result()->valid; - - _dataman_cache.invalidate(); - _load_mission_index = -1; - - /* Reset vehicle_roi - * Missions that do not explicitly configure ROI would not override - * an existing ROI setting from previous missions */ - _navigator->reset_vroi(); - - const mission_s old_mission = _mission; - - if (_mission_sub.copy(&_mission)) { - - bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), - sizeof(mission_s)); - - if (!success) { - PX4_ERR("Can't update mission state in Dataman"); - } - - /* determine current index */ - if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { - _current_mission_index = _mission.current_seq; - - } else { - /* if less items available, reset to first item */ - if (_current_mission_index >= (int)_mission.count) { - _current_mission_index = 0; - - } else if (_current_mission_index < 0) { - /* if not initialized, set it to 0 */ - _current_mission_index = 0; - } - - /* otherwise, just leave it */ - } - - if (old_mission.mission_update_counter != _mission.mission_update_counter) { - check_mission_valid(true); - - failed = !_navigator->get_mission_result()->valid; - - if (!failed) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; - - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; - - /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_changed = true; - } - - /* check if the mission waypoints changed while the vehicle is in air - * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != old_mission.count) || - (_mission.dataman_id != old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { - _mission_waypoints_changed = true; - } - } - - } else { - PX4_ERR("mission update failed"); - failed = true; - } - - if (failed) { - // only warn if the check failed on merit - if ((int)_mission.count > 0) { - PX4_WARN("mission check failed"); - } - - // reset the mission - _mission.count = 0; - _mission.current_seq = 0; - _current_mission_index = 0; - } - - // we start from the first item so can reset the cache - if (_current_mission_index == 0) { - resetItemCache(); - } - - // reset as when we update mission we don't want to proceed at previous index - _inactivation_index = -1; - - // find and store landing start marker (if available) - find_mission_land_start(); - - set_current_mission_item(); -} - - -void -Mission::advance_mission() -{ - /* do not advance mission item if we're processing sub mission work items */ - if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) { - return; - } - - switch (_mission_type) { - case MISSION_TYPE_MISSION: - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - _current_mission_index++; - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - // find next position item in reverse order - dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); - - for (int32_t i = _current_mission_index - 1; i >= 0; i--) { - struct mission_item_s missionitem = {}; - - bool success = _dataman_cache.loadWait(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s), 100_ms); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (item_contains_position(missionitem)) { - _current_mission_index = i; - return; - } - } - - // finished flying back the mission - _current_mission_index = -1; - break; - } - - default: - _current_mission_index++; - } - - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::set_mission_items() -{ - /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = false; - - /* mission item that comes after current if available */ - struct mission_item_s mission_item_next_position; - struct mission_item_s mission_item_after_next_position; - bool has_next_position_item = false; - bool has_after_next_position_item = false; - - work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - - if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, - &mission_item_after_next_position, &has_after_next_position_item)) { - /* if mission type changed, notify */ - if (_mission_type != MISSION_TYPE_MISSION) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission\t" : - "Executing Mission\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_execute_rev"), events::Log::Info, "Executing Reverse Mission"); - - } else { - events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); - } - - user_feedback_done = true; - } - - _mission_type = MISSION_TYPE_MISSION; - - } else { - if (_mission_type != MISSION_TYPE_NONE) { - - if (_navigator->get_land_detected()->landed) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed\t" : - "Mission finished, landed\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_finished_rev"), events::Log::Info, "Reverse Mission finished, landed"); - - } else { - events::send(events::ID("mission_finished"), events::Log::Info, "Mission finished, landed"); - } - - } else { - /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering\t" : - "Mission finished, loitering\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_finished_rev_loiter"), events::Log::Info, "Reverse Mission finished, loitering"); - - } else { - events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); - } - - /* use last setpoint for loiter */ - _navigator->set_can_loiter_at_sp(true); - } - - user_feedback_done = true; - } - - _mission_type = MISSION_TYPE_NONE; - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - if (_navigator->get_land_detected()->landed) { - _mission_item.nav_cmd = NAV_CMD_IDLE; - - } else { - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - setLoiterItemFromCurrentPositionSetpoint(&_mission_item); - - } else { - setLoiterItemFromCurrentPosition(&_mission_item); - } - - } - - /* update position setpoint triplet */ - pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - - /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); - - // set mission finished - _navigator->get_mission_result()->finished = true; - _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); - - if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - * https://en.wikipedia.org/wiki/Loiter_(aeronautics) - */ - - if (_navigator->get_land_detected()->landed) { - /* landed, refusing to take off without a mission */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); - events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, refusing takeoff"); - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); - events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, loitering"); - } - - user_feedback_done = true; - } - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); - - return; - } - - /*********************************** handle mission item *********************************************/ - - /* handle mission items depending on the mode */ - - const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current; - - if (item_contains_position(_mission_item)) { - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - /* force vtol land */ - if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { - _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; - } - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* do takeoff before going to setpoint if needed and not already in takeoff */ - /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ - if (do_need_vertical_takeoff() && - _work_item_type == WORK_ITEM_TYPE_DEFAULT && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; - has_next_position_item = true; - - float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home\t", - (double)(takeoff_alt - _navigator->get_home_position()->alt)); - events::send(events::ID("mission_takeoff_to"), events::Log::Info, - "Takeoff to {1:.1m_v} above home", takeoff_alt - _navigator->get_home_position()->alt); - - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - /* hold heading for takeoff items */ - _mission_item.yaw = _navigator->get_local_position()->heading; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - - } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - // if the vehicle is already in fixed wing mode then the current mission item - // will be accepted immediately and the work items will be skipped - _work_item_type = WORK_ITEM_TYPE_TAKEOFF; - - - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - } - - /* if we just did a normal takeoff navigate to the actual waypoint now */ - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_TAKEOFF && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - } - - /* if we just did a VTOL takeoff, prepare transition */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_TAKEOFF && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { - - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - - /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - - _mission_item.force_heading = true; - - new_work_item_type = WORK_ITEM_TYPE_ALIGN; - - /* set position setpoint to current while aligning */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - } - - /* heading is aligned now, prepare transition */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_ALIGN && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { - - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - - /* check if the vtol_takeoff waypoint is on top of us */ - if (do_need_move_to_takeoff()) { - new_work_item_type = WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; - } - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - _mission_item.yaw = _navigator->get_local_position()->heading; - - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - /* move to land wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && !_navigator->get_land_detected()->landed) { - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - has_next_position_item = true; - - float altitude = _navigator->get_global_position()->alt; - - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - _mission_item.vtol_back_transition = true; - } - - /* transition to MC */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_navigator->get_land_detected()->landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _navigator->get_global_position()->alt; - _mission_item.altitude_is_relative = false; - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; - - // make previous setpoint invalid, such that there will be no prev-current line following - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - pos_sp_triplet->previous.valid = false; - } - - /* move to landing waypoint before descent if necessary */ - if (do_need_move_to_land() && - (_work_item_type == WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - has_next_position_item = true; - - /* - * Ignoring waypoint altitude: - * Set altitude to the same as we have now to prevent descending too fast into - * the ground. Actual landing will descend anyway until it touches down. - * XXX: We might want to change that at some point if it is clear to the user - * what the altitude means on this waypoint type. - */ - float altitude = _navigator->get_global_position()->alt; - - if (pos_sp_triplet->current.valid - && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - // make previous setpoint invalid, such that there will be no prev-current line following. - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - pos_sp_triplet->previous.valid = false; - - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - } - - /* we just moved to the landing waypoint, now descend */ - if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - - } - - /* ignore yaw for landing items */ - /* XXX: if specified heading for landing is desired we could add another step before the descent - * that aligns the vehicle first */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { - _mission_item.yaw = NAN; - } - - - // for fast forward convert certain types to simple waypoint - // XXX: add other types which should be ignored in fast forward - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD && - ((_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) || - (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT))) { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - if (item_contains_position(_mission_item)) { - // convert mission item to a simple waypoint - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "MissionReverse: Got a non-position mission item, ignoring it\t"); - events::send(events::ID("mission_ignore_non_position_item"), events::Log::Info, - "MissionReverse: Got a non-position mission item, ignoring it"); - } - - break; - } - } - - } else { - /* handle non-position mission items such as commands */ - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* turn towards next waypoint before MC to FW transition */ - if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_navigator->get_land_detected()->landed - && has_next_position_item) { - - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - - new_work_item_type = WORK_ITEM_TYPE_ALIGN; - - set_align_mission_item(&_mission_item, &mission_item_next_position); - - /* set position setpoint to target during the transition */ - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); - } - - /* yaw is aligned now */ - if (_work_item_type == WORK_ITEM_TYPE_ALIGN && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - - pos_sp_triplet->previous = pos_sp_triplet->current; - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - // ignore certain commands in mission fast forward - if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) && - (_mission_item.nav_cmd == NAV_CMD_DELAY)) { - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - // nothing to do, all commands are ignored - break; - } - } - - if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; - } - } - - /*********************************** set setpoints and check next *********************************************/ - // The logic in this section establishes the tracking between the current waypoint - // which we are approaching and the next waypoint, which will tell us in which direction - // we will change our trajectory right after reaching it. - - // Because actions, gates and jump labels can be interleaved with waypoints, - // we are searching around the current mission item in the list to find the closest - // gate and the closest waypoint. We then store them separately. - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - // Check if the mission item is a gate along the current trajectory - if (item_contains_gate(_mission_item)) { - - // The mission item is a gate, let's check if the next item in the list provides - // a position to go towards. - - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (has_next_position_item) { - // We have a position, convert it to the setpoint and update setpoint triplet - mission_apply_limitation(mission_item_next_position); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); - } - - // ELSE: The current position setpoint stays unchanged. - - } else { - // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - } - - // ELSE: The current position setpoint stays unchanged. - } - - // Only set the previous position item if the current one really changed - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) && - !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { - pos_sp_triplet->previous = current_setpoint_copy; - } - - /* issue command if ready (will do nothing for position mission items) */ - issue_command(_mission_item); - - /* set current work item type */ - _work_item_type = new_work_item_type; - - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - - _need_takeoff = true; - } - - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); - - if (_mission_type == MISSION_TYPE_MISSION) { - set_current_mission_item(); - } - - // If the mission item under evaluation contains a gate, we need to check if we have a next position item so - // the controller can fly the correct line between the current and next setpoint - if (item_contains_gate(_mission_item)) { - if (has_after_next_position_item) { - /* got next mission item, update setpoint triplet */ - mission_apply_limitation(mission_item_next_position); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); - - } else { - pos_sp_triplet->next.valid = false; - } - - } else { - // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout - // This is done by setting the position triplet's next position's valid flag to false, - // which makes the FlightTask disregard the next position - // TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior - // seems hacky, handle this more properly. - const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item)); - - if (_mission_item.autocontinue && !brake_for_hold) { - /* try to process next mission item */ - if (has_next_position_item) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); - - } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; - } - - } else { - /* vehicle will be paused on current waypoint, don't set next item */ - pos_sp_triplet->next.valid = false; - } - } - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); + return _navigator->get_mission_result()->valid && on_landing_stage; } bool -Mission::do_need_vertical_takeoff() +Mission::set_current_mission_index(uint16_t index) { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (index == _mission.current_seq) { + return true; + } - float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - - if (_navigator->get_land_detected()->landed) { - /* force takeoff if landed (additional protection) */ - _need_takeoff = true; - - } else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) { - /* if in-air and already above takeoff height, don't do takeoff */ - _need_takeoff = false; - - } else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius() - && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { - /* if in-air but below takeoff height and we have a takeoff item */ - _need_takeoff = true; + if (_navigator->get_mission_result()->valid && (index < _mission.count)) { + if (goToItem(index, true) != PX4_OK) { + // Keep the old mission index (it was not updated by the interface) and report back. + return false; } - /* check if current mission item is one that requires takeoff before */ - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + _is_current_planned_mission_item_valid = true; - _need_takeoff = false; - return true; + // we start from the first item so can reset the cache + if (_mission.current_seq == 0) { + resetItemCache(); } + + // update mission items if already in active mission + if (isActive()) { + // prevent following "previous - current" line + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + } + + // User has actively set new index, reset. + _inactivation_index = -1; + + return true; } return false; } +bool Mission::setNextMissionItem() +{ + return (goToNextItem(true) == PX4_OK); +} + bool Mission::do_need_move_to_land() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); } @@ -1382,11 +173,11 @@ Mission::do_need_move_to_land() bool Mission::do_need_move_to_takeoff() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); } @@ -1394,351 +185,416 @@ Mission::do_need_move_to_takeoff() return false; } -void -Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) +void Mission::setActiveMissionItems() { - if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - mission_item->lat = setpoint->lat; - mission_item->lon = setpoint->lon; - mission_item->altitude = setpoint->alt; + /* Get mission item that comes after current if available */ + constexpr static size_t max_num_next_items{2u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items; - } else { - mission_item->lat = _navigator->get_global_position()->lat; - mission_item->lon = _navigator->get_global_position()->lon; - mission_item->altitude = _navigator->get_global_position()->alt; - } + getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); - mission_item->altitude_is_relative = false; -} + mission_item_s next_mission_items[max_num_next_items]; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); -void -Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next) -{ - mission_item->nav_cmd = NAV_CMD_WAYPOINT; - copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); - mission_item->altitude_is_relative = false; - mission_item->autocontinue = true; - mission_item->time_inside = 0.0f; - mission_item->yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - mission_item_next->lat, mission_item_next->lon); - mission_item->force_heading = true; -} + for (size_t i = 0U; i < num_found_items; i++) { + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i], + reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); -float -Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) -{ - /* calculate takeoff altitude */ - float takeoff_alt = get_absolute_altitude_for_item(*mission_item); - - /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ - if (_navigator->get_land_detected()->landed) { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt()); - - } else { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt()); - } - - return takeoff_alt; -} - -void -Mission::heading_sp_update() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = - _navigator->get_position_setpoint_triplet(); - - // Only update if current triplet is valid - if (pos_sp_triplet->current.valid) { - - double point_from_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon - }; - double point_to_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon - }; - float yaw_offset = 0.0f; - - // Depending on ROI-mode, update heading - switch (_navigator->get_vroi().mode) { - case vehicle_roi_s::ROI_LOCATION: { - // ROI is a fixed location. Vehicle needs to point towards that location - point_to_latlon[0] = _navigator->get_vroi().lat; - point_to_latlon[1] = _navigator->get_vroi().lon; - // No yaw offset required - yaw_offset = 0.0f; - break; - } - - case vehicle_roi_s::ROI_WPNEXT: { - // ROI is current waypoint. Vehcile needs to point towards current waypoint - point_to_latlon[0] = pos_sp_triplet->current.lat; - point_to_latlon[1] = pos_sp_triplet->current.lon; - // Add the gimbal's yaw offset - yaw_offset = _navigator->get_vroi().yaw_offset; - break; - } - - case vehicle_roi_s::ROI_NONE: - case vehicle_roi_s::ROI_WPINDEX: - case vehicle_roi_s::ROI_TARGET: - case vehicle_roi_s::ROI_ENUM_END: - default: { - return; - } - } - - // Get desired heading and update it. - // However, only update if distance to desired heading is - // larger than acceptance radius to prevent excessive yawing - float d_current = get_distance_to_next_waypoint(point_from_latlon[0], - point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); - - if (d_current > _navigator->get_acceptance_radius()) { - float yaw = matrix::wrap_pi( - get_bearing_to_next_waypoint(point_from_latlon[0], - point_from_latlon[1], point_to_latlon[0], - point_to_latlon[1]) + yaw_offset); - - _mission_item.yaw = yaw; - pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; + if (success) { + next_mission_items[i] = next_mission_item; } else { - if (!pos_sp_triplet->current.yaw_valid) { - _mission_item.yaw = _navigator->get_local_position()->heading; - pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; - } + num_found_items = i; + break; + } + } + + /*********************************** handle mission item *********************************************/ + WorkItemType new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; + + if (item_contains_position(_mission_item)) { + /* force vtol land */ + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } - // we set yaw directly so we can run this in parallel to the FOH update - publish_navigator_mission_item(); - _navigator->set_position_setpoint_triplet_updated(); - } -} + handleTakeoff(new_work_item_type, next_mission_items, num_found_items); -void -Mission::do_abort_landing() -{ - // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT + handleLanding(new_work_item_type, next_mission_items, num_found_items); - if (_mission_item.nav_cmd != NAV_CMD_LAND) { - return; + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 + if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout + // This is done by setting the position triplet's next position's valid flag to false, + // which makes the FlightTask disregard the next position + // TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior + // seems hacky, handle this more properly. + const bool brake_for_hold = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item)); + + if (_mission_item.autocontinue && !brake_for_hold) { + /* try to process next mission item */ + if (num_found_items >= 1u) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + + } else { + /* vehicle will be paused on current waypoint, don't set next item */ + pos_sp_triplet->next.valid = false; + } + + } else if (item_contains_gate(_mission_item)) { + // The mission item is a gate, let's check if the next item in the list provides + // a position to go towards. + + if (num_found_items > 0u) { + // We have a position, convert it to the setpoint and update setpoint triplet + mission_apply_limitation(next_mission_items[0u]); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); + } + + if (num_found_items >= 2u) { + /* got next mission item, update setpoint triplet */ + mission_apply_limitation(next_mission_items[1u]); + mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next); + + } else { + pos_sp_triplet->next.valid = false; + } + + } else { + handleVtolTransition(new_work_item_type, next_mission_items, num_found_items); } - const float alt_landing = get_absolute_altitude_for_item(_mission_item); - const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), - _navigator->get_global_position()->alt); + // Only set the previous position item if the current one really changed + if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) && + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + pos_sp_triplet->previous = current_setpoint_copy; + } - // turn current landing waypoint into an indefinite loiter - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = alt_sp; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; + issue_command(_mission_item); - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + /* set current work item type */ + _work_item_type = new_work_item_type; - // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during - // the landing abort hold. otherwise, the "next" setpoint would still register as a "LAND" point, and trigger - // the early landing configuration (flaps and landing airspeed) during the hold. - _navigator->get_position_setpoint_triplet()->next.lat = (double)NAN; - _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; - _navigator->get_position_setpoint_triplet()->next.alt = NAN; + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } publish_navigator_mission_item(); // for logging _navigator->set_position_setpoint_triplet_updated(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", - (int)(alt_sp - alt_landing)); - events::send(events::ID("mission_holding_above_landing"), events::Log::Info, - "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); - - // reset mission index to start of landing - if (_land_start_available) { - _current_mission_index = get_land_start_index(); - - } else { - // move mission index back (landing approach point) - _current_mission_index -= 1; - } - - // send reposition cmd to get out of mission - vehicle_command_s vcmd = {}; - - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vcmd.param1 = -1; - vcmd.param2 = 1; - vcmd.param5 = _mission_item.lat; - vcmd.param6 = _mission_item.lon; - vcmd.param7 = alt_sp; - - _navigator->publish_vehicle_cmd(&vcmd); } -bool -Mission::prepare_mission_items(struct mission_item_s *mission_item, - struct mission_item_s *next_position_mission_item, bool *has_next_position_item, - struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item) +void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) { - *has_next_position_item = false; - bool first_res = false; - int offset = 1; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - offset = -1; - } + /* do climb before going to setpoint if needed and not already executing climb */ + /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ + if (PX4_ISFINITE(_mission_init_climb_altitude_amsl) && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { - if (read_mission_item(0, mission_item)) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; - first_res = true; + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + next_mission_items[0u].nav_cmd = NAV_CMD_WAYPOINT; - /* trying to find next position mission item */ - while (read_mission_item(offset, next_position_mission_item)) { - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - offset--; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Climb to %.1f meters above home\t", + (double)(_mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt)); + events::send(events::ID("mission_climb_before_start"), events::Log::Info, + "Climb to {1:.1m_v} above home", _mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt); - } else { - offset++; - } - - if (item_contains_position(*next_position_mission_item)) { - *has_next_position_item = true; - break; - } - } - - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && - after_next_position_mission_item && has_after_next_position_item) { - /* trying to find next next position mission item */ - while (read_mission_item(offset, after_next_position_mission_item)) { - offset++; - - if (item_contains_position(*after_next_position_mission_item)) { - *has_after_next_position_item = true; - break; - } - } - } - } - - return first_res; -} - -bool -Mission::read_mission_item(int offset, struct mission_item_s *mission_item) -{ - /* select mission */ - const int current_index = _current_mission_index; - int index_to_read = current_index + offset; - - int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read; - const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; - - /* do not work on empty missions */ - if (_mission.count == 0) { - return false; - } - - /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after - * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ - for (int i = 0; i < 10; i++) { - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_mission.count) { - /* mission item index out of bounds - if they are equal, we just reached the end */ - if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission item index out of bound, index: %d, max: %" PRIu16 ".\t", - *mission_index_ptr, _mission.count); - events::send(events::ID("mission_index_out_of_bound"), events::Log::Error, - "Mission item index out of bound, index: {1}, max: {2}", *mission_index_ptr, _mission.count); - } - - return false; - } - - /* read mission item to temp storage first to not overwrite current mission item if data damaged */ - struct mission_item_s mission_item_tmp; - - /* read mission item from datamanager */ - bool success = _dataman_cache.loadWait(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), - sizeof(mission_item_s), 500_ms); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); - events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, - "Waypoint {1} could not be read from storage", *mission_index_ptr); - return false; - } - - /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { - const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL; - - /* do DO_JUMP as many times as requested if not in reverse mode */ - if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) { - - /* only raise the repeat count if this is for the current mission item - * but not for the read ahead mission item */ - if (offset == 0) { - (mission_item_tmp.do_jump_current_count)++; - - /* save repeat count */ - success = _dataman_client.writeSync(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), - sizeof(struct mission_item_s)); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the dataman */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); - events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, - "DO JUMP waypoint could not be written"); - return false; - } - - report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count); - } - - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index_ptr = mission_item_tmp.do_jump_mission_index; - - } else { - if (offset == 0 && execute_jumps) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed.\t"); - events::send(events::ID("mission_do_jump_rep_completed"), events::Log::Info, - "DO JUMP repetitions completed"); - } - - /* no more DO_JUMPS, therefore just try to continue with next mission item */ - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - (*mission_index_ptr)--; - - } else { - (*mission_index_ptr)++; - } - } + if (_land_detected_sub.get().landed) { + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; } else { - /* if it's not a DO_JUMP, then we were successful */ - memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); - return true; + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + /* hold heading for takeoff items */ + _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.altitude = _mission_init_climb_altitude_amsl; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + _mission_init_climb_altitude_amsl = NAN; + + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + + } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // if the vehicle is already in fixed wing mode then the current mission item + // will be accepted immediately and the work items will be skipped + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a normal takeoff navigate to the actual waypoint now */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) { + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a VTOL takeoff, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + _mission_item.force_heading = true; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; + + /* set position setpoint to current while aligning */ + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + } + + /* heading is aligned now, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + /* check if the vtol_takeoff waypoint is on top of us */ + if (do_need_move_to_takeoff()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + } + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + + /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + } +} + +void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* move to land wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) + && !_land_detected_sub.get().landed) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + } + + /* transition to MC */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_land_detected_sub.get().landed) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + } + + /* move to landing waypoint before descent if necessary */ + if (do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + /* + * Ignoring waypoint altitude: + * Set altitude to the same as we have now to prevent descending too fast into + * the ground. Actual landing will descend anyway until it touches down. + * XXX: We might want to change that at some point if it is clear to the user + * what the altitude means on this waypoint type. + */ + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid + && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); } } - /* we have given up, we don't want to cycle forever */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.\t"); - events::send(events::ID("mission_do_jump_cycle"), events::Log::Error, "DO JUMP is cycling, giving up"); - return false; + /* we just moved to the landing waypoint, now descend */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + } + } + + /* ignore yaw for landing items */ + /* XXX: if specified heading for landing is desired we could add another step before the descent + * that aligns the vehicle first */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + _mission_item.yaw = NAN; + } +} + +void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* turn towards next waypoint before MC to FW transition */ + if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_land_detected_sub.get().landed + && (num_found_items > 0u)) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; + + set_align_mission_item(&_mission_item, &next_mission_items[0u]); + + /* set position setpoint to target during the transition */ + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); + } + + /* yaw is aligned now */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + pos_sp_triplet->previous = pos_sp_triplet->current; + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } } void Mission::save_mission_state() { - if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Save only while disarmed, as this is a blocking operation _need_mission_save = true; return; @@ -1753,11 +609,11 @@ Mission::save_mission_state() if (success) { /* data read successfully, check dataman ID and items count */ - if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) { + if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count + && mission_state.mission_update_counter && _mission.mission_update_counter) { /* navigator may modify only sequence, write modified state only if it changed */ - if (mission_state.current_seq != _current_mission_index) { - mission_state.current_seq = _current_mission_index; - mission_state.timestamp = hrt_absolute_time(); + if (mission_state.current_seq != _mission.current_seq) { + mission_state = _mission; success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), sizeof(mission_s)); @@ -1771,10 +627,7 @@ Mission::save_mission_state() } else { /* invalid data, this must not happen and indicates error in mission publisher */ - mission_state.timestamp = hrt_absolute_time(); - mission_state.dataman_id = _mission.dataman_id; - mission_state.count = _mission.count; - mission_state.current_seq = _current_mission_index; + mission_state = _mission; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Invalid mission state.\t"); /* EVENT @@ -1792,381 +645,3 @@ Mission::save_mission_state() } } } - -void -Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) -{ - /* inform about the change */ - _navigator->get_mission_result()->item_do_jump_changed = true; - _navigator->get_mission_result()->item_changed_index = index; - _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; - - _navigator->set_mission_result_updated(); -} - -void -Mission::set_mission_item_reached() -{ - _navigator->get_mission_result()->seq_reached = _current_mission_index; - _navigator->set_mission_result_updated(); - reset_mission_item_reached(); -} - -void -Mission::set_current_mission_item() -{ - _navigator->get_mission_result()->finished = false; - _navigator->get_mission_result()->seq_current = _current_mission_index; - - _navigator->set_mission_result_updated(); - - save_mission_state(); -} - -void -Mission::check_mission_valid(bool force) -{ - if ((!_home_inited && _navigator->home_global_position_valid()) || force) { - - MissionFeasibilityChecker _missionFeasibilityChecker(_navigator, _dataman_client); - - _navigator->get_mission_result()->valid = - _missionFeasibilityChecker.checkMissionFeasible(_mission); - - _navigator->get_mission_result()->seq_total = _mission.count; - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); - _home_inited = _navigator->home_global_position_valid(); - - // find and store landing start marker (if available) - find_mission_land_start(); - } -} - -void -Mission::reset_mission(struct mission_s &mission) -{ - if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s))) { - if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { - /* set current item to 0 */ - mission.current_seq = 0; - - /* reset jump counters */ - if (mission.count > 0) { - const dm_item_t dataman_id = (dm_item_t)mission.dataman_id; - - for (unsigned index = 0; index < mission.count; index++) { - struct mission_item_s item; - const ssize_t len = sizeof(struct mission_item_s); - - bool success = _dataman_client.readSync(dataman_id, index, reinterpret_cast(&item), sizeof(mission_item_s), - 500_ms); - - if (!success) { - PX4_WARN("could not read mission item during reset"); - break; - } - - if (item.nav_cmd == NAV_CMD_DO_JUMP) { - item.do_jump_current_count = 0; - - success = _dataman_client.writeSync(dataman_id, index, reinterpret_cast(&item), len); - - if (!success) { - PX4_WARN("could not save mission item during reset"); - break; - } - } - } - } - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t"); - events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission"); - - /* initialize mission state in dataman */ - mission.timestamp = hrt_absolute_time(); - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.count = 0; - mission.current_seq = 0; - } - - _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); - } -} - -bool -Mission::need_to_reset_mission() -{ - // reset mission when disarmed, mission was actually started and we reached the last mission item - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset - && (_current_mission_index == _mission.count - 1)) { - _need_mission_reset = false; - return true; - } - - return false; -} - -int32_t -Mission::index_closest_mission_item() -{ - int32_t min_dist_index(0); - float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); - - dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); - - for (size_t i = 0; i < _mission.count; i++) { - struct mission_item_s missionitem = {}; - - bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), - sizeof(mission_item_s), 500_ms); - - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (item_contains_position(missionitem)) { - // do not consider land waypoints for a fw - if (!((missionitem.nav_cmd == NAV_CMD_LAND) && - (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && - (!_navigator->get_vstatus()->is_vtol))) { - float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon, - get_absolute_altitude_for_item(missionitem), - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (dist < min_dist) { - min_dist = dist; - min_dist_index = i; - } - } - } - } - - // for mission reverse also consider the home position - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - float dist = get_distance_to_point_global_wgs84( - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon, - _navigator->get_home_position()->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (dist < min_dist) { - min_dist = dist; - min_dist_index = -1; - } - } - - return min_dist_index; -} - -bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const -{ - return ((p1->valid == p2->valid) && - (p1->type == p2->type) && - (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && - (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && - (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && - (fabs(p1->lat - p2->lat) < DBL_EPSILON) && - (fabs(p1->lon - p2->lon) < DBL_EPSILON) && - (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && - ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && - (p1->yaw_valid == p2->yaw_valid) && - (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && - (p1->yawspeed_valid == p2->yawspeed_valid) && - (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && - (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && - (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && - (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && - ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) - && !PX4_ISFINITE(p2->cruising_throttle)))); - -} - -void Mission::publish_navigator_mission_item() -{ - navigator_mission_item_s navigator_mission_item{}; - - navigator_mission_item.instance_count = _navigator->mission_instance_count(); - navigator_mission_item.sequence_current = _current_mission_index; - navigator_mission_item.nav_cmd = _mission_item.nav_cmd; - navigator_mission_item.latitude = _mission_item.lat; - navigator_mission_item.longitude = _mission_item.lon; - navigator_mission_item.altitude = _mission_item.altitude; - - navigator_mission_item.time_inside = get_time_inside(_mission_item); - navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; - navigator_mission_item.loiter_radius = _mission_item.loiter_radius; - navigator_mission_item.yaw = _mission_item.yaw; - - navigator_mission_item.frame = _mission_item.frame; - navigator_mission_item.frame = _mission_item.origin; - - navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; - navigator_mission_item.force_heading = _mission_item.force_heading; - navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; - navigator_mission_item.autocontinue = _mission_item.autocontinue; - navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; - - navigator_mission_item.timestamp = hrt_absolute_time(); - - _navigator_mission_item_pub.publish(navigator_mission_item); -} - -bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactivation_index, - unsigned &prev_pos_index) const -{ - - for (int index = inactivation_index; index >= 0; index--) { - mission_item_s mission_item; - const dm_item_t dm_current = (dm_item_t)mission.dataman_id; - bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast(&mission_item), - sizeof(mission_item), 500_ms); - - if (!success) { - break; - } - - if (MissionBlock::item_contains_position(mission_item)) { - prev_pos_index = index; - return true; - } - } - - return false; -} - -bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) -{ - const dm_item_t dm_current = (dm_item_t)mission.dataman_id; - - while (start_index < mission.count) { - // start_index is expected to be after _current_mission_index, and the item should therefore be cached - bool success = _dataman_cache.loadWait(dm_current, start_index, reinterpret_cast(&mission_item), - sizeof(mission_item), 500_ms); - - if (success && MissionBlock::item_contains_position(mission_item)) { - return true; - } - - start_index++; - } - - return false; -} - -void Mission::cacheItem(const mission_item_s &mission_item) -{ - switch (mission_item.nav_cmd) { - case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: - _last_gimbal_configure_item = mission_item; - break; - - case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: - _last_gimbal_control_item = mission_item; - break; - - case NAV_CMD_SET_CAMERA_MODE: - _last_camera_mode_item = mission_item; - break; - - case NAV_CMD_DO_SET_CAM_TRIGG_DIST: - case NAV_CMD_DO_TRIGGER_CONTROL: - case NAV_CMD_IMAGE_START_CAPTURE: - case NAV_CMD_IMAGE_STOP_CAPTURE: - _last_camera_trigger_item = mission_item; - break; - - case NAV_CMD_DO_CHANGE_SPEED: - _last_speed_change_item = mission_item; - break; - - case NAV_CMD_DO_VTOL_TRANSITION: - // delete speed changes after a VTOL transition - _last_speed_change_item = {}; - break; - - default: - break; - } -} - -void Mission::replayCachedGimbalCameraItems() -{ - if (_last_gimbal_configure_item.nav_cmd > 0) { - issue_command(_last_gimbal_configure_item); - _last_gimbal_configure_item = {}; // delete cached item - } - - if (_last_gimbal_control_item.nav_cmd > 0) { - issue_command(_last_gimbal_control_item); - _last_gimbal_control_item = {}; // delete cached item - } - - if (_last_camera_mode_item.nav_cmd > 0) { - issue_command(_last_camera_mode_item); - _last_camera_mode_item = {}; // delete cached item - } -} - -void Mission::replayCachedTriggerItems() -{ - if (_last_camera_trigger_item.nav_cmd > 0) { - issue_command(_last_camera_trigger_item); - _last_camera_trigger_item = {}; // delete cached item - } -} - -void Mission::replayCachedSpeedChangeItems() -{ - if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { - issue_command(_last_speed_change_item); - _last_speed_change_item = {}; // delete cached item - } -} - -void Mission::resetItemCache() -{ - _last_gimbal_configure_item = {}; - _last_gimbal_control_item = {}; - _last_camera_mode_item = {}; - _last_camera_trigger_item = {}; -} - -bool Mission::haveCachedGimbalOrCameraItems() -{ - return _last_gimbal_configure_item.nav_cmd > 0 || - _last_gimbal_control_item.nav_cmd > 0 || - _last_camera_mode_item.nav_cmd > 0; -} - -bool Mission::cameraWasTriggering() -{ - return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL - && (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) || - (_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || - (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST - && _last_camera_trigger_item.params[0] > FLT_EPSILON); -} - -void Mission::updateCachedItemsUpToIndex(const int end_index) -{ - for (int i = 0; i <= end_index; i++) { - mission_item_s mission_item; - const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; - bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), - sizeof(mission_item), 500_ms); - - if (success) { - cacheItem(mission_item); - } - } -} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 28d1d56304..e8c33e1f0e 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -46,97 +46,32 @@ #pragma once -#include "mission_block.h" -#include "mission_feasibility_checker.h" -#include "navigator_mode.h" +#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "mission_base.h" +#include "navigation.h" class Navigator; -class Mission : public MissionBlock, public ModuleParams +class Mission : public MissionBase { public: Mission(Navigator *navigator); - ~Mission() override = default; + ~Mission() = default; - /** - * @brief function to call regularly to do background work - */ - void run(); - - void on_inactive() override; - void on_inactivation() override; - void on_activation() override; - void on_active() override; + virtual void on_inactive() override; + virtual void on_activation() override; bool set_current_mission_index(uint16_t index); - bool land_start(); - bool landing(); + uint16_t get_land_start_index() const { return _mission.land_start_index; } + bool get_land_start_available() const { return hasMissionLandStart(); } - uint16_t get_land_start_index() const { return _land_start_index; } - bool get_land_start_available() const { return _land_start_available; } - bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; } - bool get_mission_changed() const { return _mission_changed ; } - bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; } - double get_landing_start_lat() { return _landing_start_lat; } - double get_landing_start_lon() { return _landing_start_lon; } - float get_landing_start_alt() { return _landing_start_alt; } + bool isLanding(); - double get_landing_lat() { return _landing_lat; } - double get_landing_lon() { return _landing_lon; } - float get_landing_alt() { return _landing_alt; } - float get_landing_loiter_rad() { return _landing_loiter_radius; } - - void set_closest_item_as_current(); - - /** - * Set a new mission mode and handle the switching between the different modes - * - * For a list of the different modes refer to mission_result.msg - */ - void set_execution_mode(const uint8_t mode); private: - void mission_init(); - - /** - * Update mission topic - */ - void update_mission(); - - /** - * Move on to next mission item or switch to loiter - */ - void advance_mission(); - - /** - * @brief Configures mission items in current setting - * - * Configure the mission items depending on current mission item index and settings such - * as terrain following, etc. - */ - void set_mission_items(); - - /** - * Returns true if we need to do a takeoff at the current state - */ - bool do_need_vertical_takeoff(); + bool setNextMissionItem() override; /** * Returns true if we need to move to waypoint location before starting descent @@ -148,231 +83,24 @@ private: */ bool do_need_move_to_takeoff(); - /** - * Copies position from setpoint if valid, otherwise copies current position - */ - void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint); - - /** - * Create mission item to align towards next waypoint - */ - void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next); - /** * Calculate takeoff height for mission item considering ground clearance */ float calculate_takeoff_altitude(struct mission_item_s *mission_item); - /** - * Updates the heading of the vehicle. Rotary wings only. - */ - void heading_sp_update(); - - /** - * Abort landing - */ - void do_abort_landing(); - - /** - * Read the current and the next mission item. The next mission item read is the - * next mission item that contains a position. - * - * @return true if current mission item available - */ - bool prepare_mission_items(mission_item_s *mission_item, - mission_item_s *next_position_mission_item, bool *has_next_position_item, - mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr); - - /** - * Read current (offset == 0) or a specific (offset > 0) mission item - * from the dataman and watch out for DO_JUMPS - * - * @return true if successful - */ - bool read_mission_item(int offset, struct mission_item_s *mission_item); - /** * Save current mission state to dataman */ void save_mission_state(); - /** - * Inform about a changed mission item after a DO_JUMP - */ - void report_do_jump_mission_changed(int index, int do_jumps_remaining); + void setActiveMissionItems() override; - /** - * Set a mission item as reached - */ - void set_mission_item_reached(); + void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - /** - * Set the current mission item - */ - void set_current_mission_item(); + void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - /** - * Check whether a mission is ready to go - */ - void check_mission_valid(bool force); + void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items); - /** - * Reset mission - */ - void reset_mission(struct mission_s &mission); - - /** - * Returns true if we need to reset the mission (call this only when inactive) - */ - bool need_to_reset_mission(); - - /** - * Find and store the index of the landing sequence (DO_LAND_START) - */ - bool find_mission_land_start(); - - /** - * Return the index of the closest mission item to the current global position. - */ - int32_t index_closest_mission_item(); - - bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; - - void publish_navigator_mission_item(); - - - /** - * @brief Get the index associated with the last item that contains a position - * @param mission The mission to search - * @param start_index The index to start searching from - * @param prev_pos_index The index of the previous position item containing a position - * @return true if a previous position item was found - */ - bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index) const; - - /** - * @brief Get the next item after start_index that contains a position - * - * @param mission The mission to search - * @param start_index The index to start searching from - * @param mission_item The mission item to populate - * @return true if successful - */ - bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item); - - /** - * @brief Cache the mission items containing gimbal, camera mode and trigger commands - * - * @param mission_item The mission item to cache if applicable - */ - void cacheItem(const mission_item_s &mission_item); - - /** - * @brief Update the cached items up to the given index - * - * @param end_index The index to update up to - */ - void updateCachedItemsUpToIndex(int end_index); - - /** - * @brief Replay the cached gimbal and camera mode items - */ - void replayCachedGimbalCameraItems(); - - /** - * @brief Replay the cached trigger items - * - */ - void replayCachedTriggerItems(); - - /** - * @brief Replay the cached speed change items and delete them afterwards - * - */ - void replayCachedSpeedChangeItems(); - - /** - * @brief Reset the item cache - */ - void resetItemCache(); - - /** - * @brief Check if there are cached gimbal or camera mode items to be replayed - * - * @return true if there are cached items - */ - bool haveCachedGimbalOrCameraItems(); - - /** - * @brief Check if the camera was triggering - * - * @return true if there was a camera trigger command in the cached items that didn't disable triggering - */ - bool cameraWasTriggering(); - - DEFINE_PARAMETERS( - (ParamFloat) _param_mis_dist_1wp, - (ParamInt) _param_mis_mnt_yaw_ctl - ) - - uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; - - uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ - mission_s _mission {}; - - static constexpr uint32_t DATAMAN_CACHE_SIZE = 10; - DatamanCache _dataman_cache{"mission_dm_cache_miss", DATAMAN_CACHE_SIZE}; - DatamanClient &_dataman_client = _dataman_cache.client(); - int32_t _load_mission_index{-1}; - int32_t _current_mission_index{-1}; - - // track location of planned mission landing - bool _land_start_available{false}; - uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */ - double _landing_start_lat{0.0}; - double _landing_start_lon{0.0}; - float _landing_start_alt{0.0f}; - - double _landing_lat{0.0}; - double _landing_lon{0.0}; - float _landing_alt{0.0f}; - - float _landing_loiter_radius{0.f}; - - bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_MISSION - } _mission_type{MISSION_TYPE_NONE}; - - bool _inited{false}; - bool _home_inited{false}; - bool _need_mission_reset{false}; bool _need_mission_save{false}; - bool _mission_waypoints_changed{false}; - bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ - - // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message - enum work_item_type { - WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ - WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ - WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ - WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ - WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF, - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, - WORK_ITEM_TYPE_PRECISION_LAND - } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ - - uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ - bool _execution_mode_changed{false}; - - int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. - bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment. - - mission_item_s _last_gimbal_configure_item {}; - mission_item_s _last_gimbal_control_item {}; - mission_item_s _last_camera_mode_item {}; - mission_item_s _last_camera_trigger_item {}; - mission_item_s _last_speed_change_item {}; }; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp new file mode 100644 index 0000000000..41ce19c2cc --- /dev/null +++ b/src/modules/navigator/mission_base.cpp @@ -0,0 +1,1240 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 mission_base.cpp + * + * Mission base mode class that can be used for modes interacting with a mission. + * + */ + +#include + +#include "mission_base.h" + +#include "px4_platform_common/defines.h" + +#include "mission_feasibility_checker.h" +#include "navigator.h" + +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : + MissionBlock(navigator), + ModuleParams(navigator), + _dataman_cache_size_signed(dataman_cache_size_signed) +{ + _dataman_cache.resize(abs(dataman_cache_size_signed)); + _is_current_planned_mission_item_valid = (initMission() == PX4_OK); + + updateDatamanCache(); + + _mission_pub.advertise(); +} + +int MissionBase::initMission() +{ + mission_s mission; + int ret_val{PX4_ERROR}; + + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), + sizeof(mission_s)); + + if (success) { + if (isMissionValid(mission)) { + _mission = mission; + ret_val = PX4_OK; + + } else { + resetMission(); + } + + } else { + PX4_ERR("Could not initialize Mission: Dataman read failed"); + resetMission(); + } + + return ret_val; +} + +void +MissionBase::updateDatamanCache() +{ + if ((_mission.count > 0) && (_mission.current_seq != _load_mission_index)) { + + int32_t start_index = _mission.current_seq; + int32_t end_index = start_index + _dataman_cache_size_signed; + + end_index = math::max(math::min(end_index, static_cast(_mission.count)), INT32_C(0)); + + for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) { + + _dataman_cache.load(static_cast(_mission.dataman_id), index); + } + + _load_mission_index = _mission.current_seq; + } + + _dataman_cache.update(); +} + +void MissionBase::updateMavlinkMission() +{ + if (_mission_sub.updated()) { + mission_s new_mission; + _mission_sub.update(&new_mission); + + if (isMissionValid(new_mission)) { + /* Relevant mission items updated externally*/ + if (checkMissionDataChanged(new_mission)) { + bool mission_items_changed = (new_mission.mission_update_counter != _mission.mission_update_counter); + + if (new_mission.current_seq < 0) { + new_mission.current_seq = math::max(math::min(_mission.current_seq, static_cast(new_mission.count) - 1), + INT32_C(0)); + } + + _mission = new_mission; + + onMissionUpdate(mission_items_changed); + } + } + } +} + +void MissionBase::onMissionUpdate(bool has_mission_items_changed) +{ + _is_current_planned_mission_item_valid = _mission.count > 0; + + if (has_mission_items_changed) { + _dataman_cache.invalidate(); + _load_mission_index = -1; + + check_mission_valid(); + + // only warn if the check failed on merit + if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) { + PX4_WARN("mission check failed"); + resetMission(); + } + } + + if (isActive()) { + _mission_has_been_activated = true; + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + + } else { + if (has_mission_items_changed) { + _mission_has_been_activated = false; + } + } + + // reset as when we update mission we don't want to proceed at previous index + _inactivation_index = -1; +} + +void +MissionBase::on_inactive() +{ + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _global_pos_sub.update(); + + parameters_update(); + + updateMavlinkMission(); + + /* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */ + if (_navigator->home_global_position_valid() && !_initialized_mission_checked) { + check_mission_valid(); + _initialized_mission_checked = true; + } + + if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + _system_disarmed_while_inactive = true; + } +} + +void +MissionBase::on_inactivation() +{ + _navigator->disable_camera_trigger(); + + _navigator->stop_capturing_images(); + _navigator->release_gimbal_control(); + + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } + + /* reset so current mission item gets restarted if mission was paused */ + _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* reset so MISSION_ITEM_REACHED isn't published */ + _navigator->get_mission_result()->seq_reached = -1; + + _mission_type = MissionType::MISSION_TYPE_NONE; + + _inactivation_index = _mission.current_seq; +} + +void +MissionBase::on_activation() +{ + /* reset the current mission to the start sequence if needed.*/ + checkMissionRestart(); + + _mission_has_been_activated = true; + _system_disarmed_while_inactive = false; + + check_mission_valid(); + + update_mission(); + + // reset the cache and fill it with the items up to the previous item. The cache contains + // commands that are valid for the whole mission, not just a single waypoint. + if (_mission.current_seq > 0) { + resetItemCache(); + updateCachedItemsUpToIndex(_mission.current_seq - 1); + } + + int32_t resume_index = _inactivation_index > 0 ? _inactivation_index : 0; + + if (_inactivation_index > 0 && cameraWasTriggering()) { + size_t num_found_items{0U}; + getPreviousPositionItems(_inactivation_index - 1, &resume_index, num_found_items, 1U); + + if (num_found_items == 1U) { + // The mission we are resuming had camera triggering enabled. In order to not lose any images + // we restart the mission at the previous position item. + // We will replay the cached commands once we reach the previous position item and have yaw aligned. + setMissionIndex(resume_index); + + _align_heading_necessary = true; + } + } + + checkClimbRequired(_mission.current_seq); + set_mission_items(); + + _inactivation_index = -1; // reset + + // reset cruise speed + _navigator->reset_cruising_speed(); +} + +void +MissionBase::on_active() +{ + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _global_pos_sub.update(); + + parameters_update(); + + updateMavlinkMission(); + updateDatamanCache(); + + // check if heading alignment is necessary, and add it to the current mission item if necessary + if (_align_heading_necessary && is_mission_item_reached_or_completed()) { + + // add yaw alignment requirement on the current mission item + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items == 1U && !PX4_ISFINITE(_mission_item.yaw)) { + mission_item_s next_position_mission_item; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, + next_position_mission_item.lat, next_position_mission_item.lon)); + _mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + reset_mission_item_reached(); + + _navigator->set_position_setpoint_triplet_updated(); + _align_heading_necessary = false; + } + + // replay gimbal and camera commands immediately after resuming mission + if (haveCachedGimbalOrCameraItems()) { + replayCachedGimbalCameraItems(); + } + + // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set + if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { + replayCachedTriggerItems(); + } + + replayCachedSpeedChangeItems(); + + /* lets check if we reached the current mission item */ + if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { + /* If we just completed a takeoff which was inserted before the right waypoint, + there is no need to report that we reached it because we didn't. */ + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_CLIMB) { + set_mission_item_reached(); + } + + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + } + } + + /* see if we need to update the current yaw heading */ + if (!_param_mis_mnt_yaw_ctl.get() + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) + && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + || _mission_item.nav_cmd == NAV_CMD_LAND + || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING)) { + // Mount control is disabled If the vehicle is in ROI-mode, the vehicle + // needs to rotate such that ROI is in the field of view. + // ROI only makes sense for multicopters. + heading_sp_update(); + } + + // TODO: Add vtol heading update method if required. + // Question: Why does vtol ever have to update heading? + + /* check if landing needs to be aborted */ + if ((_mission_item.nav_cmd == NAV_CMD_LAND) + && (_navigator->abort_landing())) { + + do_abort_landing(); + } + + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void MissionBase::update_mission() +{ + if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_navigator->get_mission_result()->valid) { + if (_land_detected_sub.get().landed) { + /* landed, refusing to take off without a mission */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); + events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, + "No valid mission available, refusing takeoff"); + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); + events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, + "No valid mission available, loitering"); + } + + _mission_type = MissionType::MISSION_TYPE_NONE; + + } else { + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing Mission\t"); + events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); + } + + _mission_type = MissionType::MISSION_TYPE_MISSION; + } + + /* Reset vehicle_roi + * Missions that do not explicitly configure ROI would not override + * an existing ROI setting from previous missions */ + _navigator->reset_vroi(); + + if (_navigator->get_mission_result()->valid) { + /* reset work item if new mission has been accepted */ + _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; + + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; + } + + // we start from the first item so can reset the cache + if (_mission.current_seq == 0) { + resetItemCache(); + } + + set_mission_result(); +} + +void +MissionBase::advance_mission() +{ + /* do not advance mission item if we're processing sub mission work items */ + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + return; + } + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + _is_current_planned_mission_item_valid = setNextMissionItem(); + + if (!_is_current_planned_mission_item_valid) { + // Mission ended + if (_land_detected_sub.get().landed) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, landed\t"); + + events::send(events::ID("mission_finished"), events::Log::Info, "Mission finished, landed"); + + } else { + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, loitering\t"); + + events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); + } + + // Reset jump counter if the mission was completed + if ((_mission.current_seq + 1) == _mission.count) { + resetMissionJumpCounter(); + } + } + } +} + +void +MissionBase::set_mission_items() +{ + if (_is_current_planned_mission_item_valid) { + /* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */ + loadCurrentMissionItem(); + + setActiveMissionItems(); + + } else { + setEndOfMissionItems(); + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + } +} + +void MissionBase::loadCurrentMissionItem() +{ + const dm_item_t dm_item = static_cast(_mission.dataman_id); + bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast(&_mission_item), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item could not be set.\t"); + events::send(events::ID("mission_item_set_failed"), events::Log::Error, + "Mission item could not be set"); + } +} + +void MissionBase::setEndOfMissionItems() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + if (_land_detected_sub.get().landed) { + _mission_item.nav_cmd = NAV_CMD_IDLE; + + } else { + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + + } else { + setLoiterItemFromCurrentPosition(&_mission_item); + } + } + + /* update position setpoint triplet */ + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + // set mission finished + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + _mission_type = MissionType::MISSION_TYPE_NONE; +} + +void +MissionBase::set_mission_item_reached() +{ + _navigator->get_mission_result()->seq_reached = _mission.current_seq; + _navigator->set_mission_result_updated(); + + reset_mission_item_reached(); +} + +void +MissionBase::set_mission_result() +{ + _navigator->get_mission_result()->finished = false; + _navigator->get_mission_result()->seq_current = _mission.current_seq > 0 ? _mission.current_seq : 0; + + _navigator->set_mission_result_updated(); +} + +bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const +{ + return ((p1->valid == p2->valid) && + (p1->type == p2->type) && + (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && + (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && + (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && + (fabs(p1->lat - p2->lat) < DBL_EPSILON) && + (fabs(p1->lon - p2->lon) < DBL_EPSILON) && + (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && + ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && + (p1->yaw_valid == p2->yaw_valid) && + (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && + (p1->yawspeed_valid == p2->yawspeed_valid) && + (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && + (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && + (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && + (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && + ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) + && !PX4_ISFINITE(p2->cruising_throttle)))); + +} + +void +MissionBase::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + + _navigator->set_mission_result_updated(); +} + +void +MissionBase::checkMissionRestart() +{ + if (_system_disarmed_while_inactive && _mission_has_been_activated && (_mission.count > 0U) + && ((_mission.current_seq + 1) == _mission.count)) { + setMissionIndex(0); + _inactivation_index = -1; // reset + _is_current_planned_mission_item_valid = isMissionValid(_mission); + resetMissionJumpCounter(); + _navigator->reset_cruising_speed(); + _navigator->reset_vroi(); + set_mission_result(); + } +} + +void +MissionBase::check_mission_valid() +{ + if (_navigator->get_mission_result()->instance_count != _mission.mission_update_counter) { + MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client); + + bool is_mission_valid = + missionFeasibilityChecker.checkMissionFeasible(_mission); + + _navigator->get_mission_result()->valid = is_mission_valid; + _navigator->get_mission_result()->instance_count = _mission.mission_update_counter; + _navigator->get_mission_result()->seq_total = _mission.count; + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->failure = false; + set_mission_result(); + } +} + +void +MissionBase::heading_sp_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = + _navigator->get_position_setpoint_triplet(); + + // Only update if current triplet is valid + if (pos_sp_triplet->current.valid) { + + double point_from_latlon[2] = { _global_pos_sub.get().lat, + _global_pos_sub.get().lon + }; + double point_to_latlon[2] = { _global_pos_sub.get().lat, + _global_pos_sub.get().lon + }; + float yaw_offset = 0.0f; + + // Depending on ROI-mode, update heading + switch (_navigator->get_vroi().mode) { + case vehicle_roi_s::ROI_LOCATION: { + // ROI is a fixed location. Vehicle needs to point towards that location + point_to_latlon[0] = _navigator->get_vroi().lat; + point_to_latlon[1] = _navigator->get_vroi().lon; + // No yaw offset required + yaw_offset = 0.0f; + break; + } + + case vehicle_roi_s::ROI_WPNEXT: { + // ROI is current waypoint. Vehcile needs to point towards current waypoint + point_to_latlon[0] = pos_sp_triplet->current.lat; + point_to_latlon[1] = pos_sp_triplet->current.lon; + // Add the gimbal's yaw offset + yaw_offset = _navigator->get_vroi().yaw_offset; + break; + } + + case vehicle_roi_s::ROI_NONE: + case vehicle_roi_s::ROI_WPINDEX: + case vehicle_roi_s::ROI_TARGET: + case vehicle_roi_s::ROI_ENUM_END: + default: { + return; + } + } + + // Get desired heading and update it. + // However, only update if distance to desired heading is + // larger than acceptance radius to prevent excessive yawing + float d_current = get_distance_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); + + if (d_current > _navigator->get_acceptance_radius()) { + float yaw = matrix::wrap_pi( + get_bearing_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], + point_to_latlon[1]) + yaw_offset); + + _mission_item.yaw = yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + + } else { + if (!pos_sp_triplet->current.yaw_valid) { + _mission_item.yaw = _navigator->get_local_position()->heading; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + } + } + + // we set yaw directly so we can run this in parallel to the FOH update + publish_navigator_mission_item(); + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void +MissionBase::do_abort_landing() +{ + // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + return; + } + + if (_mission_item.nav_cmd != NAV_CMD_LAND) { + return; + } + + const float alt_landing = get_absolute_altitude_for_item(_mission_item); + const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), + _global_pos_sub.get().alt); + + // turn current landing waypoint into an indefinite loiter + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = alt_sp; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during + // the landing abort hold. otherwise, the "next" setpoint would still register as a "LAND" point, and trigger + // the early landing configuration (flaps and landing airspeed) during the hold. + _navigator->get_position_setpoint_triplet()->next.lat = (double)NAN; + _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; + _navigator->get_position_setpoint_triplet()->next.alt = NAN; + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", + (int)(alt_sp - alt_landing)); + events::send(events::ID("mission_holding_above_landing"), events::Log::Info, + "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); + + // reset mission index to start of landing + if (hasMissionLandStart()) { + _is_current_planned_mission_item_valid = true; + setMissionIndex(_mission.land_start_index); + + } else { + // move mission index back (landing approach point) + _is_current_planned_mission_item_valid = goToPreviousItem(false); + } + + // send reposition cmd to get out of mission + vehicle_command_s vcmd = {}; + + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.param1 = -1; + vcmd.param2 = 1; + vcmd.param5 = _mission_item.lat; + vcmd.param6 = _mission_item.lon; + vcmd.param7 = alt_sp; + + _navigator->publish_vehicle_cmd(&vcmd); +} + +void MissionBase::publish_navigator_mission_item() +{ + navigator_mission_item_s navigator_mission_item{}; + + navigator_mission_item.instance_count = _navigator->mission_instance_count(); + navigator_mission_item.sequence_current = _mission.current_seq; + navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.latitude = _mission_item.lat; + navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.altitude = _mission_item.altitude; + + navigator_mission_item.time_inside = get_time_inside(_mission_item); + navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; + navigator_mission_item.loiter_radius = _mission_item.loiter_radius; + navigator_mission_item.yaw = _mission_item.yaw; + + navigator_mission_item.frame = _mission_item.frame; + navigator_mission_item.frame = _mission_item.origin; + + navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; + navigator_mission_item.force_heading = _mission_item.force_heading; + navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; + navigator_mission_item.autocontinue = _mission_item.autocontinue; + navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; + + navigator_mission_item.timestamp = hrt_absolute_time(); + + _navigator_mission_item_pub.publish(navigator_mission_item); +} + +bool MissionBase::isMissionValid(mission_s &mission) const +{ + bool ret_val{false}; + + if (((mission.current_seq < mission.count) || (mission.count == 0U && mission.current_seq <= 0)) && + (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && + (mission.timestamp != 0u)) { + ret_val = true; + + } + + return ret_val; +} + +int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, + bool write_jumps, bool mission_direction_backward) +{ + if (mission_index >= _mission.count || mission_index < 0) { + return PX4_ERROR; + } + + const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; + int32_t new_mission_index{mission_index}; + mission_item_s new_mission; + + for (uint16_t jump_count = 0u; jump_count < MAX_JUMP_ITERATION; jump_count++) { + /* read mission item from datamanager */ + bool success = _dataman_cache.loadWait(dataman_id, new_mission_index, reinterpret_cast(&new_mission), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); + events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, + "Waypoint {1} could not be read from storage", new_mission_index); + return PX4_ERROR; + } + + if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) { + if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) { + PX4_ERR("Do Jump mission index is out of bounds."); + return PX4_ERROR; + } + + if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) { + if (write_jumps) { + new_mission.do_jump_current_count++; + success = _dataman_cache.writeWait(dataman_id, new_mission_index, reinterpret_cast(&new_mission), + sizeof(struct mission_item_s)); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); + events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, + "DO JUMP waypoint could not be written"); + // Still continue searching for next non jump item. + } + + report_do_jump_mission_changed(new_mission_index, new_mission.do_jump_repeat_count - new_mission.do_jump_current_count); + } + + new_mission_index = new_mission.do_jump_mission_index; + + } else { + if (mission_direction_backward) { + new_mission_index--; + + } else { + new_mission_index++; + } + } + + } else { + break; + } + } + + mission_index = new_mission_index; + mission = new_mission; + + return PX4_OK; +} + +int MissionBase::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward) +{ + mission_item_s mission_item; + + if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == PX4_OK) { + setMissionIndex(index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +void MissionBase::setMissionIndex(int32_t index) +{ + if (index != _mission.current_seq) { + _mission.current_seq = index; + _mission.timestamp = hrt_absolute_time(); + _mission_pub.publish(_mission); + } +} + +void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items) +{ + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index < 0) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + next_mission_index--; + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == PX4_OK; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items_index[item_idx] = next_mission_index; + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items) +{ + // Make sure vector does not contain any preexisting elements. + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index >= _mission.count) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == PX4_OK; + next_mission_index++; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items_index[item_idx] = math::max(next_mission_index - 1, + static_cast(0)); // subtract 1 to get the index of the first position item + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +int MissionBase::goToNextItem(bool execute_jump) +{ + if (_mission.current_seq + 1 >= (_mission.count)) { + return PX4_ERROR; + } + + return goToItem(_mission.current_seq + 1, execute_jump); +} + +int MissionBase::goToPreviousItem(bool execute_jump) +{ + if (_mission.current_seq <= 0) { + return PX4_ERROR; + } + + return goToItem(_mission.current_seq - 1, execute_jump, true); +} + +int MissionBase::goToPreviousPositionItem(bool execute_jump) +{ + size_t num_found_items{0U}; + int32_t previous_position_item_index; + getPreviousPositionItems(_mission.current_seq, &previous_position_item_index, num_found_items, 1); + + if (num_found_items == 1U) { + setMissionIndex(previous_position_item_index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int MissionBase::goToNextPositionItem(bool execute_jump) +{ + size_t num_found_items{0U}; + int32_t next_position_item_index; + getNextPositionItems(_mission.current_seq + 1, &next_position_item_index, num_found_items, 1); + + if (num_found_items == 1U) { + setMissionIndex(next_position_item_index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, float home_alt, + const vehicle_status_s &vehicle_status) +{ + int32_t min_dist_index(-1); + float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) { + mission_item_s mission; + + bool success = _dataman_cache.loadWait(dataman_id, mission_item_index, reinterpret_cast(&mission), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not set mission closest to position.\t"); + events::send(events::ID("mission_failed_set_closest"), events::Log::Error, + "Could not set mission closest to position"); + return PX4_ERROR; + } + + if (MissionBlock::item_contains_position(mission)) { + // do not consider land waypoints for a fw + if (!((mission.nav_cmd == NAV_CMD_LAND) && + (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (!vehicle_status.is_vtol))) { + float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon, + MissionBlock::get_absolute_altitude_for_item(mission, home_alt), + lat, + lon, + alt, + &dist_xy, &dist_z); + + if (dist < min_dist) { + min_dist = dist; + min_dist_index = mission_item_index; + } + } + } + } + + setMissionIndex(min_dist_index); + + return PX4_OK; +} + +void MissionBase::resetMission() +{ + /* we do not need to reset mission if is already.*/ + if (_mission.count == 0u && isMissionValid(_mission)) { + return; + } + + /* Set a new mission*/ + mission_s new_mission{_mission}; + new_mission.timestamp = hrt_absolute_time(); + new_mission.current_seq = 0; + new_mission.land_start_index = -1; + new_mission.land_index = -1; + new_mission.count = 0u; + new_mission.mission_update_counter = _mission.mission_update_counter + 1; + new_mission.dataman_id = _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0; + + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&new_mission), + sizeof(mission_s)); + + if (success) { + _mission = new_mission; + _mission_pub.publish(_mission); + + } else { + PX4_ERR("Mission Initialization failed."); + } +} + +void MissionBase::resetMissionJumpCounter() +{ + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) { + mission_item_s mission_item; + + bool success = _dataman_client.readSync(dataman_id, mission_index, reinterpret_cast(&mission_item), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission could not reset jump count.\t"); + events::send(events::ID("mission_failed_set_jump_count"), events::Log::Error, + "Mission could not reset jump count"); + break; + } + + if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) { + mission_item.do_jump_current_count = 0u; + + bool write_success = _dataman_cache.writeWait(dataman_id, mission_index, reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); + + if (!write_success) { + PX4_ERR("Could not write mission item for jump count reset."); + break; + } + } + } +} + +void MissionBase::cacheItem(const mission_item_s &mission_item) +{ + switch (mission_item.nav_cmd) { + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + _last_gimbal_configure_item = mission_item; + break; + + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + _last_gimbal_control_item = mission_item; + break; + + case NAV_CMD_SET_CAMERA_MODE: + _last_camera_mode_item = mission_item; + break; + + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_DO_TRIGGER_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + _last_camera_trigger_item = mission_item; + break; + + case NAV_CMD_DO_CHANGE_SPEED: + _last_speed_change_item = mission_item; + break; + + case NAV_CMD_DO_VTOL_TRANSITION: + // delete speed changes after a VTOL transition + _last_speed_change_item = {}; + break; + + default: + break; + } +} + +void MissionBase::replayCachedGimbalCameraItems() +{ + if (_last_gimbal_configure_item.nav_cmd > 0) { + issue_command(_last_gimbal_configure_item); + _last_gimbal_configure_item = {}; // delete cached item + } + + if (_last_gimbal_control_item.nav_cmd > 0) { + issue_command(_last_gimbal_control_item); + _last_gimbal_control_item = {}; // delete cached item + } + + if (_last_camera_mode_item.nav_cmd > 0) { + issue_command(_last_camera_mode_item); + _last_camera_mode_item = {}; // delete cached item + } +} + +void MissionBase::replayCachedTriggerItems() +{ + if (_last_camera_trigger_item.nav_cmd > 0) { + issue_command(_last_camera_trigger_item); + _last_camera_trigger_item = {}; // delete cached item + } +} + +void MissionBase::replayCachedSpeedChangeItems() +{ + if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { + issue_command(_last_speed_change_item); + _last_speed_change_item = {}; // delete cached item + } +} + +void MissionBase::resetItemCache() +{ + _last_gimbal_configure_item = {}; + _last_gimbal_control_item = {}; + _last_camera_mode_item = {}; + _last_camera_trigger_item = {}; +} + +bool MissionBase::haveCachedGimbalOrCameraItems() +{ + return _last_gimbal_configure_item.nav_cmd > 0 || + _last_gimbal_control_item.nav_cmd > 0 || + _last_camera_mode_item.nav_cmd > 0; +} + +bool MissionBase::cameraWasTriggering() +{ + return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL + && (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST + && _last_camera_trigger_item.params[0] > FLT_EPSILON); +} + +void MissionBase::updateCachedItemsUpToIndex(const int end_index) +{ + for (int i = 0; i <= end_index; i++) { + mission_item_s mission_item; + const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (success) { + cacheItem(mission_item); + } + } +} + +void MissionBase::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} + +void MissionBase::checkClimbRequired(int32_t mission_item_index) +{ + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(mission_item_index, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items > 0U) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s mission; + _mission_init_climb_altitude_amsl = NAN; // default to NAN, overwrite below if applicable + + const bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, reinterpret_cast(&mission), + sizeof(mission), MAX_DATAMAN_LOAD_WAIT); + + const bool is_fw_and_takeoff = mission.nav_cmd == NAV_CMD_TAKEOFF + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + // for FW when on a Takeoff item do not require climb before mission, as we need to keep course to takeoff item straight + if (success && !is_fw_and_takeoff) { + const float altitude_amsl_next_position_item = MissionBlock::get_absolute_altitude_for_item(mission); + const float error_below_setpoint = altitude_amsl_next_position_item - + _navigator->get_global_position()->alt; + + if (error_below_setpoint > _navigator->get_altitude_acceptance_radius()) { + + _mission_init_climb_altitude_amsl = altitude_amsl_next_position_item; + } + } + } +} + +bool MissionBase::checkMissionDataChanged(mission_s new_mission) +{ + /* count and land_index are the same if the mission_counter did not change. We do not care about changes in geofence or rally counters.*/ + return ((new_mission.dataman_id != _mission.dataman_id) || + (new_mission.mission_update_counter != _mission.mission_update_counter) || + (new_mission.current_seq != _mission.current_seq)); +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h new file mode 100644 index 0000000000..4a9a33464f --- /dev/null +++ b/src/modules/navigator/mission_base.h @@ -0,0 +1,446 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 mission_base.h + * + * Mission base mode class that can be used for modes interacting with a mission. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mission_block.h" +#include "navigation.h" + +using namespace time_literals; + +class Navigator; + +class MissionBase : public MissionBlock, public ModuleParams +{ +public: + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + ~MissionBase() override = default; + + virtual void on_inactive() override; + virtual void on_inactivation() override; + virtual void on_activation() override; + virtual void on_active() override; + +protected: + + /** + * @brief Maximum time to wait for dataman loading + * + */ + static constexpr hrt_abstime MAX_DATAMAN_LOAD_WAIT{500_ms}; + + // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message + enum class WorkItemType { + WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ + WORK_ITEM_TYPE_CLIMB, /**< takeoff before moving to waypoint */ + WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ + WORK_ITEM_TYPE_ALIGN_HEADING, /**< align for next waypoint */ + WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF, + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, + WORK_ITEM_TYPE_PRECISION_LAND + } _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ + + enum class MissionType { + MISSION_TYPE_NONE, + MISSION_TYPE_MISSION + } _mission_type{MissionType::MISSION_TYPE_NONE}; + + /** + * @brief Get the Previous Mission Position Items + * + * @param[in] start_index is the index from where to start searching the previous mission position items + * @param[out] items_index is an array of indexes indicating the previous mission position items found + * @param[out] num_found_items are the amount of previous position items found + * @param[in] max_num_items are the maximum amount of previous position items to be searched + */ + void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items); + /** + * @brief Get the next mission item containing a position setpoint + * + * @param[in] start_index is the index from where to start searching (first possible return index) + * @param[out] items_index is an array of indexes indicating the next mission position items found + * @param[out] num_found_items are the amount of next position items found + * @param[in] max_num_items are the maximum amount of next position items to be searched + */ + void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items); + /** + * @brief Has Mission a Land Start or Land Item + * + * @return true If mission has a land start of land item + * @return false otherwise + */ + bool hasMissionLandStart() const { return _mission.land_start_index > 0;}; + /** + * @brief Go to next Mission Item + * Go to next non jump mission item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextItem(bool execute_jump); + /** + * @brief Go to previous Mission Item + * Go to previous non jump mission item + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousItem(bool execute_jump); + /** + * @brief Go to Mission Item + * + * @param[in] index Index of the mission item to go to + * @param[in] execute_jump Flag indicating if a jump should be executed of ignored + * @param[in] mission_direction_backward Flag indicating if a mission is flown backward + * @return PX4_OK if the mission item exists, PX4_ERR otherwise + */ + int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false); + /** + * @brief Go To Previous Mission Position Item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousPositionItem(bool execute_jump); + /** + * @brief Go To Next Mission Position Item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextPositionItem(bool execute_jump); + /** + * @brief Go to Mission Land Start Item + * + * @return PX4_OK if land start item exists and is loaded, PX4_ERR otherwise + */ + int goToMissionLandStart(); + /** + * @brief Set the Mission to closest mission position item from current position + * + * @param[in] lat latitude of the current position + * @param[in] lon longitude of the current position + * @param[in] alt altitude of the current position + * @param[in] home_alt altitude of the home position + * @param[in] vehicle_status vehicle status struct + * @return PX4_OK if closest item is found and loaded, PX4_ERR otherwise + */ + int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status); + /** + * @brief Initialize Mission + * + * @return PX4_OK if mission could be loaded, PX4_ERR otherwise + */ + int initMission(); + /** + * @brief Reset Mission + * + */ + void resetMission(); + /** + * @brief Reset Mission Jump Counter of Mission Jump Items + * + */ + void resetMissionJumpCounter(); + /** + * @brief Get the Non Jump Mission Item + * + * @param[out] mission_index Index of the mission item + * @param[out] mission The return mission item + * @param execute_jump Flag indicating if a jump item should be executed or ignored + * @param write_jumps Flag indicating if the jump counter should be updated + * @param mission_direction_backward Flag indicating if the mission is flown backwards + * @return PX4_OK if mission item could be loaded, PX4_ERR otherwise + */ + int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps, + bool mission_direction_backward = false); + /** + * @brief Is Mission Parameters Valid + * + * @param mission Mission struct + * @return true is mission parameters are valid + * @return false otherwise + */ + bool isMissionValid(mission_s &mission) const; + + /** + * On mission update + * Change behaviour after external mission update. + * @param[in] has_mission_items_changed flag if the mission items have been changed. + */ + void onMissionUpdate(bool has_mission_items_changed); + + /** + * Update mission topic + */ + void update_mission(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * @brief Configures mission items in current setting + * + * Configure the mission items depending on current mission item index and settings such + * as terrain following, etc. + */ + void set_mission_items(); + + /** + * @brief Load current mission item + * + * Load current mission item from dataman cache. + */ + void loadCurrentMissionItem(); + + /** + * Set the mission result + */ + void set_mission_result(); + + /** + * @brief Reset the item cache + */ + void resetItemCache(); + + /** + * @brief Set the actions to be performed on Active Mission Item + * + */ + virtual void setActiveMissionItems() = 0; + /** + * @brief Set the Next Mission Item after old mission item has been completed + * + * @return true if the next mission item could be set + * @return false otherwise + */ + virtual bool setNextMissionItem() = 0; + /** + * @brief Set action at the end of the mission + * + */ + void setEndOfMissionItems(); + /** + * @brief Publish navigator mission item + * + */ + void publish_navigator_mission_item(); + /** + * @brief I position setpoint equal + * + * @param p1 First position setpoint to compare + * @param p2 Second position setpoint to compare + * @return true if both setpoints are equal + * @return false otherwise + */ + bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + + bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ + bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ + bool _initialized_mission_checked{false}; /**< Flag indicating if the initialized mission has been checked by the mission validator*/ + bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/ + mission_s _mission; /**< Currently active mission*/ + float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ + int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. + + DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/ + DatamanClient &_dataman_client = _dataman_cache.client(); /**< Dataman client*/ + + uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription*/ + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/ + uORB::Publication _mission_pub{ORB_ID(mission)}; /**< Mission publication*/ +private: + /** + * @brief Maximum number of jump mission items iterations + * + */ + static constexpr uint16_t MAX_JUMP_ITERATION{10u}; + /** + * @brief Update Dataman cache + * + */ + void updateDatamanCache(); + /** + * @brief Update mission subscription + * + */ + void updateMavlinkMission(); + + /** + * Check whether a mission is ready to go + */ + void check_mission_valid(); + + /** + * Reset mission + */ + void checkMissionRestart(); + + /** + * Set a mission item as reached + */ + void set_mission_item_reached(); + + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + + /** + * Abort landing + */ + void do_abort_landing(); + + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + + /** + * @brief Cache the mission items containing gimbal, camera mode and trigger commands + * + * @param mission_item The mission item to cache if applicable + */ + void cacheItem(const mission_item_s &mission_item); + + /** + * @brief Update the cached items up to the given index + * + * @param end_index The index to update up to + */ + void updateCachedItemsUpToIndex(int end_index); + + /** + * @brief Replay the cached gimbal and camera mode items + */ + void replayCachedGimbalCameraItems(); + + /** + * @brief Replay the cached trigger items + * + */ + void replayCachedTriggerItems(); + + /** + * @brief Replay the cached speed change items and delete them afterwards + * + */ + void replayCachedSpeedChangeItems(); + + /** + * @brief Check if there are cached gimbal or camera mode items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalOrCameraItems(); + + /** + * @brief Check if the camera was triggering + * + * @return true if there was a camera trigger command in the cached items that didn't disable triggering + */ + bool cameraWasTriggering(); + + /** + * @brief Set the Mission Index + * + * @param[in] index Index of the mission item + */ + void setMissionIndex(int32_t index); + + /** + * @brief Parameters update + * + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + /** + * @brief Check if a climb is necessary to align with mission altitude prior to starting the mission + * + * @param mission_item_index The index of the mission item to check if a climb is necessary + */ + void checkClimbRequired(int32_t mission_item_index); + + /** + * @brief check if relevant data in the new mission have changed. + * @param[in] new_mission new mission received over uorb + * @return true if the relevant mission data has changed, false otherwise + */ + bool checkMissionDataChanged(mission_s new_mission); + + int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ + int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ + + bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment. + + mission_item_s _last_gimbal_configure_item {}; + mission_item_s _last_gimbal_control_item {}; + mission_item_s _last_camera_mode_item {}; + mission_item_s _last_camera_trigger_item {}; + mission_item_s _last_speed_change_item {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mis_dist_1wp, + (ParamInt) _param_mis_mnt_yaw_ctl + ) + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 565b2f2150..71fcb756a5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -901,15 +901,54 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const +{ + return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt); +} + +float +MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt) { if (mission_item.altitude_is_relative) { - return mission_item.altitude + _navigator->get_home_position()->alt; + return mission_item.altitude + home_alt; } else { return mission_item.altitude; } } +void +MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const +{ + if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + mission_item->lat = setpoint->lat; + mission_item->lon = setpoint->lon; + mission_item->altitude = setpoint->alt; + + } else { + mission_item->lat = _navigator->get_global_position()->lat; + mission_item->lon = _navigator->get_global_position()->lon; + mission_item->altitude = _navigator->get_global_position()->alt; + } + + mission_item->altitude_is_relative = false; +} + +void +MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const +{ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); + mission_item->altitude_is_relative = false; + mission_item->autocontinue = true; + mission_item->time_inside = 0.0f; + mission_item->yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + mission_item_next->lat, mission_item_next->lon); + mission_item->force_heading = true; +} + void MissionBlock::initialize() { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 450b57ba7b..1a669512af 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,6 +91,15 @@ public: */ static bool item_contains_gate(const mission_item_s &item); + /** + * Get the absolute altitude for mission item + * + * @param mission_item the mission item of interest + * @param home_alt the home altitude in [m AMSL]. + * @return Mission item altitude in [m AMSL] + */ + static float get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt); + /** * Check if the mission item contains a marker * @@ -124,6 +133,18 @@ public: _payload_deploy_timeout_s = timeout_s; } + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const; + + /** + * Create mission item to align towards next waypoint + */ + void set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const; + protected: /** * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 9b411355bc..edfbac54bd 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -44,13 +44,13 @@ */ /** - * Take-off altitude + * Default take-off altitude * - * This is the minimum altitude the system will take off to. + * This is the relative altitude the system will take off to + * if not otherwise specified. * * @unit m * @min 0 - * @max 80 * @decimal 1 * @increment 0.5 * @group Mission diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 41b33edab2..1ddb1e8891 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -49,7 +49,9 @@ #include "navigator_mode.h" #include "rtl.h" #include "takeoff.h" +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF #include "vtol_takeoff.h" +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF #include "navigation.h" @@ -77,6 +79,7 @@ #include #include #include +#include #include #include #include @@ -142,7 +145,6 @@ public: /** * Setters */ - void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } void set_mission_result_updated() { _mission_result_updated = true; } @@ -171,8 +173,6 @@ public: Geofence &get_geofence() { return _geofence; } - bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } - float get_loiter_radius() { return _param_nav_loiter_rad.get(); } /** @@ -249,32 +249,12 @@ public: orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - void increment_mission_instance_count() { _mission_result.instance_count++; } - int mission_instance_count() const { return _mission_result.instance_count; } void set_mission_failure_heading_timeout(); - bool is_planned_mission() const { return _navigation_mode == &_mission; } - - bool on_mission_landing() { return (_mission.landing() && _navigation_mode == &_mission); } - - bool start_mission_landing() { return _mission.land_start(); } - bool get_mission_start_land_available() { return _mission.get_land_start_available(); } - int get_mission_landing_index() { return _mission.get_land_start_index(); } - - double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); } - double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); } - float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); } - - double get_mission_landing_lat() { return _mission.get_landing_lat(); } - double get_mission_landing_lon() { return _mission.get_landing_lon(); } - float get_mission_landing_alt() { return _mission.get_landing_alt(); } - - float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); } - // RTL bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } @@ -285,7 +265,7 @@ public: // Param access int get_loiter_min_alt() const { return _param_min_ltr_alt.get(); } int get_landing_abort_min_alt() const { return _param_mis_lnd_abrt_alt.get(); } - float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } + float get_param_mis_takeoff_alt() const { return _param_mis_takeoff_alt.get(); } int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } @@ -342,8 +322,6 @@ private: vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ - bool _rtl_activated{false}; - // Publications geofence_result_s _geofence_result{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ @@ -358,17 +336,16 @@ private: hrt_abstime _last_geofence_check = 0; bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */ - bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ - bool _shouldEngageMissionForLanding{false}; - Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */ +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9d9ed8c035..fd0f076569 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,7 +75,9 @@ Navigator::Navigator() : _mission(this), _loiter(this), _takeoff(this), +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _vtol_takeoff(this), +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _land(this), _precland(this), _rtl(this) @@ -87,7 +89,9 @@ Navigator::Navigator() : _navigation_mode_array[3] = &_takeoff; _navigation_mode_array[4] = &_land; _navigation_mode_array[5] = &_precland; +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _navigation_mode_array[6] = &_vtol_takeoff; +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF /* iterate through navigation modes and initialize _mission_item for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { @@ -168,8 +172,8 @@ void Navigator::run() fds[2].fd = _mission_sub; fds[2].events = POLLIN; - int16_t geofence_update_counter{0}; - int16_t safe_points_update_counter{0}; + uint16_t geofence_update_counter{0}; + uint16_t safe_points_update_counter{0}; /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); @@ -575,6 +579,8 @@ void Navigator::run() // CMD_NAV_TAKEOFF is acknowledged by commander +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF) { _vtol_takeoff.setTransitionAltitudeAbsolute(cmd.param7); @@ -584,13 +590,15 @@ void Navigator::run() // loiter height is the height above takeoff altitude at which the vehicle will establish on a loiter circle _vtol_takeoff.setLoiterHeight(cmd.param1); +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { // find NAV_CMD_DO_LAND_START in the mission and // use MAV_CMD_MISSION_START to start the mission from the next item containing a position setpoint + uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; - if (_mission.land_start()) { + if (_mission.get_land_start_available()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = _mission.get_land_start_index(); @@ -598,9 +606,10 @@ void Navigator::run() } else { PX4_WARN("planned mission landing not available"); + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED; } - publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + publish_vehicle_command_ack(cmd, result); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { @@ -694,7 +703,6 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); navigation_mode_new = &_mission; break; @@ -704,131 +712,31 @@ void Navigator::run() navigation_mode_new = &_loiter; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + + // If we are already in mission landing, do not switch. + if (_navigation_mode == &_mission && _mission.isLanding()) { + navigation_mode_new = &_mission; + + } else { _pos_sp_triplet_published_invalid_once = false; - const bool rtl_activated_now = !_rtl_activated; - - switch (_rtl.get_rtl_type()) { - case RTL::RTL_TYPE_MISSION_LANDING: - case RTL::RTL_TYPE_CLOSEST: { - // If a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode. - // In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission. - if (rtl_activated_now) { - _shouldEngageMissionForLanding = _rtl.getRTLDestinationTypeMission() - && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - } - - if (_shouldEngageMissionForLanding && (on_mission_landing() || _rtl.getRTLState() > RTL::RTL_STATE_CLIMB)) { - - // already in a mission landing, we just need to inform the user and stay in mission - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t"); - events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info, - "RTL to Mission landing, continue landing"); - } - - if (_navigation_mode != &_mission) { - // the first time we're here start the mission landig - start_mission_landing(); - } - - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - navigation_mode_new = &_mission; - - } else { - navigation_mode_new = &_rtl; - } - - break; - } - - case RTL::RTL_TYPE_MISSION_LANDING_REVERSED: - if (_mission.get_land_start_available() && !get_land_detected()->landed) { - // the mission contains a landing spot - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - - if (_navigation_mode != &_mission) { - if (_navigation_mode == nullptr) { - // switching from an manual mode, go to landing if not already landing - if (!on_mission_landing()) { - start_mission_landing(); - } - - } else { - // switching from an auto mode, continue the mission from the closest item - _mission.set_closest_item_as_current(); - } - } - - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t"); - events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info, - "RTL Mission activated, continue mission"); - } - - navigation_mode_new = &_mission; - - } else { - // fly the mission in reverse if switching from a non-manual mode - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE); - - if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) && - (! _mission.get_mission_finished()) && - (!get_land_detected()->landed)) { - // determine the closest mission item if switching from a non-mission mode, and we are either not already - // mission mode or the mission waypoints changed. - // The seconds condition is required so that when no mission was uploaded and one is available the closest - // mission item is determined and also that if the user changes the active mission index while rtl is active - // always that waypoint is tracked first. - if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) { - _mission.set_closest_item_as_current(); - } - - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t"); - events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info, - "RTL Mission activated, fly mission in reverse"); - } - - navigation_mode_new = &_mission; - - } else { - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t"); - events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info, - "RTL Mission activated, fly to home"); - } - - navigation_mode_new = &_rtl; - } - } - - break; - - default: - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t"); - events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated"); - } - - navigation_mode_new = &_rtl; - break; - - } - - _rtl_activated = true; - break; + navigation_mode_new = &_rtl; } + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_takeoff; break; +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_vtol_takeoff; break; +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; @@ -851,15 +759,9 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_STAB: default: navigation_mode_new = nullptr; - _can_loiter_at_sp = false; break; } - if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _rtl_activated = false; - _rtl.resetRtlState(); - } - // Do not execute any state machine while we are disarmed if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { navigation_mode_new = nullptr; @@ -929,9 +831,7 @@ void Navigator::run() publish_mission_result(); } - _mission.run(); _geofence.run(); - _rtl.run(); perf_end(_loop_perf); } diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 5adb0b1bdb..b0ced9006c 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -55,7 +55,6 @@ NavigatorMode::run(bool active) { if (active) { if (!_active) { - _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 3aa6f255f6..60abdcd165 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -54,6 +54,8 @@ public: void run(bool active); + bool isActive() {return _active;}; + /** * This function is called while the mode is inactive */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 517b0f9c39..d28952c9f5 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,32 +42,23 @@ #include "rtl.h" #include "navigator.h" -#include + +#include #include -#include - - -static constexpr float DELAY_SIGMA = 0.01f; - using namespace time_literals; using namespace math; +static constexpr float MIN_DIST_THRESHOLD = 2.f; + RTL::RTL(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + NavigatorMode(navigator), + ModuleParams(navigator), + _rtl_direct(navigator) { - _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); - _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); - _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); - _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); - _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); - _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); - _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); - _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); } -void RTL::run() +void RTL::updateDatamanCache() { bool success; @@ -85,8 +76,8 @@ void RTL::run() case DatamanState::Read: _dataman_state = DatamanState::ReadWait; - success = _dataman_client.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), - sizeof(mission_stats_entry_s)); + success = _dataman_client_geofence.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); if (!success) { _error_state = DatamanState::Read; @@ -97,9 +88,9 @@ void RTL::run() case DatamanState::ReadWait: - _dataman_client.update(); + _dataman_client_geofence.update(); - if (_dataman_client.lastOperationCompleted(success)) { + if (_dataman_client_geofence.lastOperationCompleted(success)) { if (!success) { _error_state = DatamanState::ReadWait; @@ -110,14 +101,14 @@ void RTL::run() _update_counter = _stats.update_counter; _safe_points_updated = false; - _dataman_cache.invalidate(); + _dataman_cache_geofence.invalidate(); - if (_dataman_cache.size() != _stats.num_items) { - _dataman_cache.resize(_stats.num_items); + if (_dataman_cache_geofence.size() != _stats.num_items) { + _dataman_cache_geofence.resize(_stats.num_items); } - for (int index = 1; index <= _dataman_cache.size(); ++index) { - _dataman_cache.load(DM_KEY_SAFE_POINTS, index); + for (int index = 1; index <= _dataman_cache_geofence.size(); ++index) { + _dataman_cache_geofence.load(DM_KEY_SAFE_POINTS, index); } _dataman_state = DatamanState::Load; @@ -131,9 +122,9 @@ void RTL::run() case DatamanState::Load: - _dataman_cache.update(); + _dataman_cache_geofence.update(); - if (!_dataman_cache.isLoading()) { + if (!_dataman_cache_geofence.isLoading()) { _dataman_state = DatamanState::UpdateRequestWait; _safe_points_updated = true; } @@ -149,647 +140,306 @@ void RTL::run() break; } + + if (_mission_counter != _mission_sub.get().mission_update_counter) { + _mission_counter = _mission_sub.get().mission_update_counter; + const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + _dataman_cache_landItem.invalidate(); + + if (_mission_sub.get().land_start_index > 0) { + _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_start_index); + } + + if (_mission_sub.get().land_index > 0) { + _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_index); + } + } + + _dataman_cache_landItem.update(); } void RTL::on_inactivation() { - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_inactivation(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactivation(); + break; + + default: + break; } } void RTL::on_inactive() { + _global_pos_sub.update(); + _vehicle_status_sub.update(); + _mission_sub.update(); + _home_pos_sub.update(); + + updateDatamanCache(); + + parameters_update(); + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_inactive(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactive(); + break; + + default: + break; + } + // Limit inactive calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); + hrt_abstime now{hrt_absolute_time()}; - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + if ((now - _destination_check_time) > 1_s) { + _destination_check_time = now; + setRtlTypeAndDestination(); - const bool global_position_recently_updated = global_position.timestamp > 0 - && hrt_elapsed_time(&global_position.timestamp) < 10_s; + const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0 + && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; - rtl_time_estimate_s rtl_time_estimate{}; - rtl_time_estimate.valid = false; + rtl_time_estimate_s estimated_time{}; + estimated_time.valid = false; - // Calculate RTL destination and time estimate only when there is a valid home and global position if (_navigator->home_global_position_valid() && global_position_recently_updated) { - find_RTL_destination(); - calcRtlTimeEstimate(RTLState::RTL_STATE_NONE, rtl_time_estimate); - rtl_time_estimate.valid = true; + switch (_rtl_type) { + case RtlType::RTL_DIRECT: + estimated_time = _rtl_direct.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_DIRECT_MISSION_LAND: + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + break; + + default: break; + } } - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); + _rtl_time_estimate_pub.publish(estimated_time); } } -void RTL::find_RTL_destination() +void RTL::on_activation() { - // get home position: - home_position_s &home_landing_position = *_navigator->get_home_position(); + setRtlTypeAndDestination(); - // get global position - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + switch (_rtl_type) { + case RtlType::RTL_DIRECT_MISSION_LAND: // Fall through + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt); + _rtl_mission_type_handle->on_activation(); + break; + case RtlType::RTL_DIRECT: + _rtl_direct.setReturnAltMin(_enforce_rtl_alt); + _rtl_direct.on_activation(); + break; + + + default: + break; + } +} + +void RTL::on_active() +{ + _global_pos_sub.update(); + _vehicle_status_sub.update(); + _mission_sub.update(); + _home_pos_sub.update(); + + updateDatamanCache(); + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_active(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_active(); + break; + + default: + break; + } +} + +void RTL::setRtlTypeAndDestination() +{ + + init_rtl_mission_type(); + + if (_param_rtl_type.get() != 2) { + // check the closest allowed destination. + bool isMissionLanding{false}; + RtlDirect::RtlPosition rtl_position; + float rtl_alt; + findRtlDestination(isMissionLanding, rtl_position, rtl_alt); + + if (isMissionLanding) { + _rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; + _rtl_mission_type_handle->setRtlAlt(rtl_alt); + + } else { + _rtl_type = RtlType::RTL_DIRECT; + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position); + } + } +} + +void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +{ // set destination to home per default, then check if other valid landing spot is closer - _destination.set(home_landing_position); + rtl_position.alt = _home_pos_sub.get().alt; + rtl_position.lat = _home_pos_sub.get().lat; + rtl_position.lon = _home_pos_sub.get().lon; + rtl_position.yaw = _home_pos_sub.get().yaw; + isMissionLanding = false; + + const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); // get distance to home position - double dlat = home_landing_position.lat - global_position.lat; - double dlon = home_landing_position.lon - global_position.lon; + float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; + float min_dist; - double lon_scale = cos(radians(global_position.lat)); + if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) { + // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode. + min_dist = FLT_MAX; - auto coord_dist_sq = [lon_scale](double lat_diff, double lon_diff) -> double { - double lon_diff_scaled = lon_scale * matrix::wrap(lon_diff, -180., 180.); - return lat_diff * lat_diff + lon_diff_scaled * lon_diff_scaled; - }; - - double min_dist_squared = coord_dist_sq(dlat, dlon); - - _destination.type = RTL_DESTINATION_HOME; - - const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - - // consider the mission landing if not RTL_TYPE_HOME_OR_RALLY type set - if (_param_rtl_type.get() != RTL_TYPE_HOME_OR_RALLY && _navigator->get_mission_start_land_available()) { - double mission_landing_lat; - double mission_landing_lon; - float mission_landing_alt; - - if (vtol_in_rw_mode) { - mission_landing_lat = _navigator->get_mission_landing_lat(); - mission_landing_lon = _navigator->get_mission_landing_lon(); - mission_landing_alt = _navigator->get_mission_landing_alt(); - - } else { - mission_landing_lat = _navigator->get_mission_landing_start_lat(); - mission_landing_lon = _navigator->get_mission_landing_start_lon(); - mission_landing_alt = _navigator->get_mission_landing_start_alt(); - } - - dlat = mission_landing_lat - global_position.lat; - dlon = mission_landing_lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); - - // always find closest destination if in hover and VTOL - if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) { - - // compare home position to landing position to decide which is closer - if (dist_squared < min_dist_squared) { - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; - } - - } else { - // it has to be the mission landing - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; - } + } else { + min_dist = home_dist; } - // do not consider rally point if RTL type is set to RTL_TYPE_MISSION_LANDING_REVERSED, so exit function and use either home or mission landing - if (_param_rtl_type.get() == RTL_TYPE_MISSION_LANDING_REVERSED) { - return; - } + // consider the mission landing if available and allowed + if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) { + mission_item_s land_mission_item; + const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + bool success = _dataman_cache_landItem.loadWait(dm_item, _mission_sub.get().land_index, + reinterpret_cast(&land_mission_item), sizeof(mission_item_s), 500_ms); - mission_safe_point_s closest_safe_point {}; - // check if a safe point is closer than home or landing - int closest_index = 0; + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t"); + events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error, + "Mission land item could not be read"); + } + + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; + + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { + min_dist = dist; + setLandPosAsDestination(rtl_position, land_mission_item); + isMissionLanding = true; + } + } if (_safe_points_updated) { - for (int current_seq = 1; current_seq <= _dataman_cache.size(); ++current_seq) { + for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) { mission_safe_point_s mission_safe_point; - bool success = _dataman_cache.loadWait(DM_KEY_SAFE_POINTS, current_seq, - reinterpret_cast(&mission_safe_point), - sizeof(mission_safe_point_s)); + bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq, + reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s), 500_ms); if (!success) { PX4_ERR("dm_read failed"); continue; } - // TODO: take altitude into account for distance measurement - dlat = mission_safe_point.lat - global_position.lat; - dlon = mission_safe_point.lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { + min_dist = dist; + setSafepointAsDestination(rtl_position, mission_safe_point); + isMissionLanding = false; } } } - if (closest_index > 0) { - _destination.type = RTL_DESTINATION_SAFE_POINT; - - // There is a safe point closer than home/mission landing - // TODO: handle all possible mission_safe_point.frame cases - switch (closest_safe_point.frame) { - case 0: // MAV_FRAME_GLOBAL - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt; - _destination.yaw = home_landing_position.yaw; - break; - - case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home - _destination.yaw = home_landing_position.yaw; - break; - - default: - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); - events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", - closest_safe_point.frame); - break; - } - } - if (_param_rtl_cone_half_angle_deg.get() > 0 - && _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()); + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get()); } else { - _rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); + rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get()); } } -void RTL::on_activation() +void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item) { - _rtl_state = RTL_STATE_NONE; - - // output the correct message, depending on where the RTL destination is - switch (_destination.type) { - case RTL_DESTINATION_HOME: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.\t"); - events::send(events::ID("rtl_land_at_home"), events::Log::Info, "RTL: landing at home position"); - break; - - case RTL_DESTINATION_MISSION_LANDING: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.\t"); - events::send(events::ID("rtl_land_at_mission"), events::Log::Info, "RTL: landing at mission landing"); - break; - - case RTL_DESTINATION_SAFE_POINT: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.\t"); - events::send(events::ID("rtl_land_at_safe_point"), events::Log::Info, "RTL: landing at safe landing point"); - break; - } - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - if (_navigator->get_land_detected()->landed) { - // For safety reasons don't go into RTL if landed. - _rtl_state = RTL_STATE_LANDED; - - } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { - - // If lower than return altitude, climb up first. - // If rtl_alt_min is true then forcing altitude change even if above. - _rtl_state = RTL_STATE_CLIMB; - - } else { - // Otherwise go straight to return - _rtl_state = RTL_STATE_RETURN; - } - - // reset cruising speed and throttle to default for RTL - _navigator->reset_cruising_speed(); - _navigator->set_cruising_throttle(); - - set_rtl_item(); - + rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + + _home_pos_sub.get().alt : land_mission_item.altitude; + rtl_position.lat = land_mission_item.lat; + rtl_position.lon = land_mission_item.lon; + rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::on_active() +void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, + const mission_safe_point_s &mission_safe_point) { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { - advance_rtl(); - set_rtl_item(); - } - - if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - - // Limit rtl time calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - const bool global_position_recently_updated = global_position.timestamp > 0 - && hrt_elapsed_time(&global_position.timestamp) < 10_s; - - rtl_time_estimate_s rtl_time_estimate{}; - rtl_time_estimate.valid = false; - - // Calculate time estimate only when there is a valid home and global position - if (_navigator->home_global_position_valid() && global_position_recently_updated) { - calcRtlTimeEstimate(_rtl_state, rtl_time_estimate); - rtl_time_estimate.valid = true; - } - - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); - } -} - -void RTL::set_rtl_item() -{ - _navigator->set_can_loiter_at_sp(false); - - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - - // if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission - const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ? - _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get(); - - const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - - _mission_item.lat = gpos.lat; - _mission_item.lon = gpos.lon; - _mission_item.altitude = _rtl_alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", - (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); - events::send(events::ID("rtl_climb_to"), events::Log::Info, - "RTL: climb to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); - break; - } - - case RTL_STATE_RETURN: { - - // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status - // can be displayed on groundstation and the WP is accepted once within loiter radius - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - - - } else { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _rtl_alt; // Don't change altitude - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && - destination_dist > _param_rtl_min_dist.get()) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || - destination_dist < _param_rtl_min_dist.get()) { - // Use destination yaw if close to _destination. - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_return_at"), events::Log::Info, - "RTL: return at {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - // Except for vtol which might be still off here and should point towards this location. - const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_descend_to"), events::Log::Info, - "RTL: descend to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - break; - } - - case RTL_STATE_LOITER: { - const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); - - if (autocontinue) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", - (double)_param_rtl_land_delay.get()); - events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); - - } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); - events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; // Don't change altitude. - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); - _mission_item.autocontinue = autocontinue; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - _navigator->set_can_loiter_at_sp(true); - - 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; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.vtol_back_transition = true; - // acceptance_radius will be overwritten since vtol_back_transition is set, - // set as a default value only - _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; - } - - case RTL_MOVE_TO_LAND_HOVER_VTOL: { - _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; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.origin = ORIGIN_ONBOARD; - break; - } - - case RTL_STATE_LAND: { - // Land at destination. - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _destination.alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.land_precision = _param_rtl_pld_md.get(); - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - _navigator->get_precland()->on_activation(); - - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); - } - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); - events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); - break; - } - - case RTL_STATE_LANDED: { - set_idle_item(&_mission_item); - set_return_alt_min(false); - break; - } - - default: - break; - } - - reset_mission_item_reached(); - - // Execute command if set. This is required for commands like VTOL transition. - if (!item_contains_position(_mission_item)) { - issue_command(_mission_item); - } - - // Convert mission item to current position setpoint and make it valid. - mission_apply_limitation(_mission_item); - - if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { - _navigator->set_position_setpoint_triplet_updated(); - } -} - -void RTL::advance_rtl() -{ - // determines if the vehicle should loiter above land - const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; - - // vehicle is a vtol and currently in fixed wing mode - const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + // There is a safe point closer than home/mission landing + // TODO: handle all possible mission_safe_point.frame cases + switch (mission_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt; + rtl_position.yaw = _home_pos_sub.get().yaw;; break; - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; - break; - - case RTL_STATE_DESCEND: - - if (descend_and_loiter) { - _rtl_state = RTL_STATE_LOITER; - - } else if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_LOITER: - - if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _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; - - break; - - case RTL_MOVE_TO_LAND_HOVER_VTOL: - - _rtl_state = RTL_STATE_LAND; - - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt + _home_pos_sub.get().alt; // alt of safe point is rel to home + rtl_position.yaw = _home_pos_sub.get().yaw;; break; default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); + events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", + mission_safe_point.frame); break; } } -float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) +float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, + float cone_half_angle_deg) { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - // horizontal distance to destination - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); + const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + rtl_position.lat, rtl_position.lon); // minium rtl altitude to use when outside of horizontal acceptance radius of target position. // We choose the minimum height to be two times the distance from the land position in order to // avoid the vehicle touching the ground while still moving horizontally. - const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f * - _navigator->get_acceptance_radius(); + const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get(); + float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); - if (destination_dist <= _navigator->get_acceptance_radius()) { - return_altitude_amsl = _destination.alt + 2.0f * destination_dist; + if (destination_dist <= _param_nav_acc_rad.get()) { + return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { @@ -799,7 +449,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); // minimum altitude we need in order to be within the user defined cone - const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + _destination.alt; + const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt; return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); } @@ -807,211 +457,73 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, gpos.alt); + return max(return_altitude_amsl, _global_pos_sub.get().alt); } -void RTL::calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate) +void RTL::init_rtl_mission_type() { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + RtlType new_rtl_mission_type{RtlType::RTL_DIRECT_MISSION_LAND}; - // Sum up time estimate for various segments of the landing procedure - switch (rtl_state) { - case RTL_STATE_NONE: - case RTL_STATE_CLIMB: { - // Climb segment is only relevant if the drone is below return altitude - const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0; + if (_param_rtl_type.get() == 2) { + if (hasMissionLandStart()) { + new_rtl_mission_type = RtlType::RTL_MISSION_FAST; - if (climb_dist > FLT_EPSILON) { - rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); - } + } else { + new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; } + } - // FALLTHROUGH - case RTL_STATE_RETURN: + if (_set_rtl_mission_type == new_rtl_mission_type) { + return; + } - // Add cruise segment to home - rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed(); - - // FALLTHROUGH - case RTL_STATE_HEAD_TO_CENTER: - case RTL_STATE_TRANSITION_TO_MC: - case RTL_STATE_DESCEND: { - // when descending, the target altitude is stored in the current mission item - float initial_altitude = 0.f; - float loiter_altitude = 0.f; - - if (rtl_state == RTL_STATE_DESCEND) { - // Take current vehicle altitude as the starting point for calculation - initial_altitude = gpos.alt; // TODO: Check if this is in the right frame - loiter_altitude = _mission_item.altitude; // Next waypoint = loiter - - - } else { - // Take the return altitude as the starting point for the calculation - initial_altitude = _rtl_alt; // CLIMB and RETURN - loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - } - - // Add descend segment (first landing phase: return alt to loiter alt) - rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); - } - - // FALLTHROUGH - case RTL_STATE_LOITER: - // Add land delay (the short pause for deploying landing gear) - // TODO: Check if landing gear is deployed or not - rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); - - // FALLTHROUGH - case RTL_MOVE_TO_LAND_HOVER_VTOL: - case RTL_STATE_LAND: { - float initial_altitude; - - // Add land segment (second landing phase) which comes after LOITER - if (rtl_state == RTL_STATE_LAND) { - // If we are in this phase, use the current vehicle altitude instead - // of the altitude paramteter to get a continous time estimate - initial_altitude = gpos.alt; - - - } else { - // If this phase is not active yet, simply use the loiter altitude, - // which is where the LAND phase will start - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - initial_altitude = loiter_altitude; - } - - // Prevent negative times when close to the ground - if (initial_altitude > _destination.alt) { - rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); - } - } + if (_rtl_mission_type_handle) { + delete _rtl_mission_type_handle; + _rtl_mission_type_handle = nullptr; + _set_rtl_mission_type = RtlType::NONE; + } + switch (new_rtl_mission_type) { + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle = new RtlDirectMissionLand(_navigator); + _set_rtl_mission_type = RtlType::RTL_DIRECT_MISSION_LAND; + // RTL type is either direct or mission land have to set it later. break; - case RTL_STATE_LANDED: - // Remaining time is 0 + case RtlType::RTL_MISSION_FAST: + _rtl_mission_type_handle = new RtlMissionFast(_navigator); + _set_rtl_mission_type = RtlType::RTL_MISSION_FAST; + _rtl_type = RtlType::RTL_MISSION_FAST; + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator); + _set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; + _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;; + break; + + default: break; } - - // Prevent negative durations as phyiscally they make no sense. These can - // occur during the last phase of landing when close to the ground. - rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); - - // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin - rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate - + _param_rtl_time_margin.get(); } -float RTL::getCruiseSpeed() +void RTL::parameters_update() { - float ret = 1e6f; + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { - ret = 1e6f; - } + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { - if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { - ret = 1e6f; + if (!isActive()) { + setRtlTypeAndDestination(); } } - - return ret; } -float RTL::getHoverLandSpeed() +bool RTL::hasMissionLandStart() { - float ret = 1e6f; - - if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - - return ret; -} - -matrix::Vector2f RTL::get_wind() -{ - _wind_sub.update(); - matrix::Vector2f wind; - - 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; -} - -float RTL::getClimbRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - - if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getDescendRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getCruiseGroundSpeed() -{ - float cruise_speed = getCruiseSpeed(); - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - matrix::Vector2f wind = get_wind(); - - matrix::Vector2f to_destination_vec; - get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, - &to_destination_vec(0), &to_destination_vec(1)); - - const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); - - const float wind_towards_home = wind.dot(to_home_dir); - const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); - - - // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient - const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( - 0.f, wind_towards_home); - - cruise_speed = ground_speed; - } - - return cruise_speed; + return _mission_sub.get().land_start_index > 0; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9c7fe55d5b..fab59e65d6 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,106 +44,98 @@ #include #include "navigator_mode.h" -#include "mission_block.h" - -#include -#include -#include -#include -#include -#include -#include #include +#include "rtl_base.h" +#include "rtl_direct.h" +#include "rtl_direct_mission_land.h" +#include "rtl_mission_fast.h" +#include "rtl_mission_fast_reverse.h" + +#include +#include +#include +#include +#include +#include +#include class Navigator; -class RTL : public MissionBlock, public ModuleParams +class RTL : public NavigatorMode, public ModuleParams { public: RTL(Navigator *navigator); ~RTL() = default; - enum RTLType { - RTL_TYPE_HOME_OR_RALLY = 0, - RTL_TYPE_MISSION_LANDING, - RTL_TYPE_MISSION_LANDING_REVERSED, - RTL_TYPE_CLOSEST, + enum class RtlType { + NONE, + RTL_DIRECT, + RTL_DIRECT_MISSION_LAND, + RTL_MISSION_FAST, + RTL_MISSION_FAST_REVERSE, }; - enum RTLDestinationType { - RTL_DESTINATION_HOME = 0, - RTL_DESTINATION_MISSION_LANDING, - RTL_DESTINATION_SAFE_POINT, - }; - - enum RTLHeadingMode { - RTL_NAVIGATION_HEADING = 0, - RTL_DESTINATION_HEADING, - RTL_CURRENT_HEADING, - }; - - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_TRANSITION_TO_MC, - RTL_MOVE_TO_LAND_HOVER_VTOL, - RTL_STATE_LAND, - RTL_STATE_LANDED, - RTL_STATE_HEAD_TO_CENTER, - }; - - /** - * @brief function to call regularly to do background work - */ - void run(); - void on_inactivation() override; void on_inactive() override; void on_activation() override; void on_active() override; - void find_RTL_destination(); + void initialize() override {}; - void set_return_alt_min(bool min) { _rtl_alt_min = min; } - - int get_rtl_type() const { return _param_rtl_type.get(); } - - void get_rtl_xy_z_speed(float &xy, float &z); - - matrix::Vector2f get_wind(); - - RTLState getRTLState() { return _rtl_state; } - - bool getRTLDestinationTypeMission() { return _destination.type == RTLDestinationType::RTL_DESTINATION_MISSION_LANDING; } - - void resetRtlState() { _rtl_state = RTL_STATE_NONE; } + void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } void updateSafePoints() { _initiate_safe_points_updated = true; } private: + bool hasMissionLandStart(); - void set_rtl_item(); + /** + * @brief function to call regularly to do background work + */ + void updateDatamanCache(); - void advance_rtl(); + void setRtlTypeAndDestination(); - float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); - void calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate); + /** + * @brief Find RTL destination. + * + */ + void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); - float getCruiseGroundSpeed(); + /** + * @brief Set the position of the land start marker in the planned mission as destination. + * + */ + void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item); - float getClimbRate(); + /** + * @brief Set the safepoint as destination. + * + * @param mission_safe_point is the mission safe point/rally point to set as destination. + */ + void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point); - float getDescendRate(); + /** + * @brief calculate return altitude from cone half angle + * + * @param[in] rtl_position landing position of the rtl + * @param[in] cone_half_angle_deg half angle of the cone [deg] + * @return return altitude + */ + float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); - float getCruiseSpeed(); + /** + * @brief initialize RTL mission type + * + */ + void init_rtl_mission_type(); - float getHoverLandSpeed(); - - RTLState _rtl_state{RTL_STATE_NONE}; + /** + * @brief Update parameters + * + */ + void parameters_update(); enum class DatamanState { UpdateRequestWait, @@ -153,71 +145,43 @@ private: Error }; - struct RTLPosition { - double lat; - double lon; - float alt; - float yaw; - uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points) - RTLDestinationType type{RTL_DESTINATION_HOME}; + hrt_abstime _destination_check_time{0}; - void set(const home_position_s &home_position) - { - lat = home_position.lat; - lon = home_position.lon; - alt = home_position.alt; - yaw = home_position.yaw; - safe_point_index = 0; - type = RTL_DESTINATION_HOME; - } - }; + RtlBase *_rtl_mission_type_handle{nullptr}; + RtlType _set_rtl_mission_type{RtlType::NONE}; + + RtlType _rtl_type{RtlType::RTL_DIRECT}; DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _error_state{DatamanState::UpdateRequestWait}; uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache - DatamanCache _dataman_cache{"rtl_dm_cache_miss", 4}; - DatamanClient &_dataman_client = _dataman_cache.client(); + DatamanCache _dataman_cache_geofence{"rtl_dm_cache_miss_geo", 4}; + DatamanClient &_dataman_client_geofence = _dataman_cache_geofence.client(); bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed + DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; + int16_t _mission_counter = -1; mission_stats_entry_s _stats; - RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) + RtlDirect _rtl_direct; - hrt_abstime _destination_check_time{0}; - - float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position - - bool _rtl_alt_min{false}; + bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rtl_return_alt, - (ParamFloat) _param_rtl_descend_alt, - (ParamFloat) _param_rtl_land_delay, - (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamInt) _param_rtl_pld_md, - (ParamFloat) _param_rtl_loiter_rad, - (ParamInt) _param_rtl_hdg_md, - (ParamFloat) _param_rtl_time_factor, - (ParamInt) _param_rtl_time_margin + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_min_dist, + (ParamFloat) _param_nav_acc_rad ) - param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; - param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; - param_t _param_mpc_land_speed{PARAM_INVALID}; - param_t _param_fw_climb_rate{PARAM_INVALID}; - param_t _param_fw_sink_rate{PARAM_INVALID}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - param_t _param_fw_airspeed_trim{PARAM_INVALID}; - param_t _param_mpc_xy_cruise{PARAM_INVALID}; - param_t _param_rover_cruise_speed{PARAM_INVALID}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; - uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; }; - -float time_to_home(const matrix::Vector3f &to_home_vec, - const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, - float vehicle_descent_speed_m_s); diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h new file mode 100644 index 0000000000..dc698687fc --- /dev/null +++ b/src/modules/navigator/rtl_base.h @@ -0,0 +1,57 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_base.h + * + * Helper class for RTL modes using the mission + * + */ + +#pragma once + +#include "mission_base.h" +#include + +class RtlBase : public MissionBase +{ +public: + RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): + MissionBase(navigator, dataman_cache_size_signed) {}; + virtual ~RtlBase() = default; + + virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; + + virtual void setReturnAltMin(bool min) { (void)min;}; + + virtual void setRtlAlt(float alt) { (void)alt;}; +}; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp new file mode 100644 index 0000000000..22c79f7161 --- /dev/null +++ b/src/modules/navigator/rtl_direct.cpp @@ -0,0 +1,723 @@ +/**************************************************************************** + * + * 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 + * 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 rtl.cpp + * + * Helper class to access RTL + * + * @author Julian Oes + * @author Anton Babushkin + * @author Julian Kent + */ + +#include "rtl_direct.h" +#include "navigator.h" +#include +#include + +#include + + +static constexpr float DELAY_SIGMA = 0.01f; + +using namespace time_literals; +using namespace math; + +RtlDirect::RtlDirect(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ + _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); + _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); + _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); + _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); + _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); + _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); +} + +void RtlDirect::on_inactivation() +{ + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RtlDirect::on_activation() +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + + parameters_update(); + + if (_land_detected_sub.get().landed) { + // For safety reasons don't go into RTL if landed. + _rtl_state = RTL_STATE_LANDED; + + } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || _enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If rtl_alt_min is true then forcing altitude change even if above. + _rtl_state = RTL_STATE_CLIMB; + + } else { + // Otherwise go start with climb + _rtl_state = RTL_STATE_RETURN; + } + + // reset cruising speed and throttle to default for RTL + _navigator->reset_cruising_speed(); + _navigator->set_cruising_throttle(); + + set_rtl_item(); +} + +void RtlDirect::on_active() +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + + parameters_update(); + + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { + advance_rtl(); + set_rtl_item(); + } + + if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + // Need to update the position and type on the current setpoint triplet. + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RtlDirect::set_rtl_item() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + + const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + events::send(events::ID("rtl_climb_to"), events::Log::Info, + "RTL: climb to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); + break; + } + + case RTL_STATE_RETURN: { + + // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status + // can be displayed on groundstation and the WP is accepted once within loiter radius + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + + + } else { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _rtl_alt; // Don't change altitude + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && + destination_dist > _param_rtl_min_dist.get()) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || + destination_dist < _param_rtl_min_dist.get()) { + // Use destination yaw if close to _destination. + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_return_at"), events::Log::Info, + "RTL: return at {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // Except for vtol which might be still off here and should point towards this location. + const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_descend_to"), events::Log::Info, + "RTL: descend to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + break; + } + + case RTL_STATE_LOITER: { + const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); + + if (autocontinue) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", + (double)_param_rtl_land_delay.get()); + events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); + events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; // Don't change altitude. + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); + _mission_item.autocontinue = autocontinue; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + 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; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.vtol_back_transition = true; + // acceptance_radius will be overwritten since vtol_back_transition is set, + // set as a default value only + _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; + } + + case RTL_MOVE_TO_LAND_HOVER_VTOL: { + _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; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.origin = ORIGIN_ONBOARD; + break; + } + + case RTL_STATE_LAND: { + // Land at destination. + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _destination.alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _navigator->get_precland()->on_activation(); + + } else if (_mission_item.land_precision == 2) { + _navigator->get_precland()->set_mode(PrecLandMode::Required); + _navigator->get_precland()->on_activation(); + } + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); + events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); + break; + } + + case RTL_STATE_LANDED: { + set_idle_item(&_mission_item); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + // Execute command if set. This is required for commands like VTOL transition. + if (!MissionBlock::item_contains_position(_mission_item)) { + issue_command(_mission_item); + } + + // Convert mission item to current position setpoint and make it valid. + mission_apply_limitation(_mission_item); + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void RtlDirect::advance_rtl() +{ + // determines if the vehicle should loiter above land + const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; + + // vehicle is a vtol and currently in fixed wing mode + const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + + if (descend_and_loiter) { + _rtl_state = RTL_STATE_LOITER; + + } else if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_LOITER: + + if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _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; + + break; + + case RTL_MOVE_TO_LAND_HOVER_VTOL: + + _rtl_state = RTL_STATE_LAND; + + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + break; + + default: + break; + } +} + +rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() +{ + _global_pos_sub.update(); + + rtl_time_estimate_s rtl_time_estimate{}; + + RTLState start_state_for_estimate{RTL_STATE_NONE}; + + if (isActive()) { + start_state_for_estimate = _rtl_state; + } + + // Calculate RTL time estimate only when there is a valid home position + // TODO: Also check if vehicle position is valid + if (!_navigator->home_global_position_valid()) { + rtl_time_estimate.valid = false; + + } else { + rtl_time_estimate.valid = true; + + // Sum up time estimate for various segments of the landing procedure + switch (start_state_for_estimate) { + case RTL_STATE_NONE: + case RTL_STATE_CLIMB: { + // Climb segment is only relevant if the drone is below return altitude + const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0; + + if (climb_dist > FLT_EPSILON) { + rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); + } + } + + // FALLTHROUGH + case RTL_STATE_RETURN: + + // Add cruise segment to home + rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( + _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + + // FALLTHROUGH + case RTL_STATE_HEAD_TO_CENTER: + case RTL_STATE_TRANSITION_TO_MC: + case RTL_STATE_DESCEND: { + // when descending, the target altitude is stored in the current mission item + float initial_altitude = 0.f; + float loiter_altitude = 0.f; + + if (start_state_for_estimate == RTL_STATE_DESCEND) { + // Take current vehicle altitude as the starting point for calculation + initial_altitude = _global_pos_sub.get().alt; // TODO: Check if this is in the right frame + loiter_altitude = _mission_item.altitude; // Next waypoint = loiter + + + } else { + // Take the return altitude as the starting point for the calculation + initial_altitude = _rtl_alt; // CLIMB and RETURN + loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + } + + // Add descend segment (first landing phase: return alt to loiter alt) + rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); + } + + // FALLTHROUGH + case RTL_STATE_LOITER: + // Add land delay (the short pause for deploying landing gear) + // TODO: Check if landing gear is deployed or not + rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); + + // FALLTHROUGH + case RTL_MOVE_TO_LAND_HOVER_VTOL: + case RTL_STATE_LAND: { + float initial_altitude; + + // Add land segment (second landing phase) which comes after LOITER + if (start_state_for_estimate == RTL_STATE_LAND) { + // If we are in this phase, use the current vehicle altitude instead + // of the altitude paramteter to get a continous time estimate + initial_altitude = _global_pos_sub.get().alt; + + + } else { + // If this phase is not active yet, simply use the loiter altitude, + // which is where the LAND phase will start + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + initial_altitude = loiter_altitude; + } + + // Prevent negative times when close to the ground + if (initial_altitude > _destination.alt) { + rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); + } + } + + break; + + case RTL_STATE_LANDED: + // Remaining time is 0 + break; + } + + // Prevent negative durations as phyiscally they make no sense. These can + // occur during the last phase of landing when close to the ground. + rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); + + // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin + rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate + + _param_rtl_time_margin.get(); + } + + // return message + rtl_time_estimate.timestamp = hrt_absolute_time(); + + return rtl_time_estimate; +} + +float RtlDirect::getCruiseSpeed() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getHoverLandSpeed() +{ + float ret = 1e6f; + + if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + + return ret; +} + +matrix::Vector2f RtlDirect::get_wind() +{ + _wind_sub.update(); + matrix::Vector2f wind; + + 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; +} + +float RtlDirect::getClimbRate() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getDescendRate() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getCruiseGroundSpeed() +{ + float cruise_speed = getCruiseSpeed(); + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + matrix::Vector2f wind = get_wind(); + + matrix::Vector2f to_destination_vec; + get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, + &to_destination_vec(0), &to_destination_vec(1)); + + const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); + + const float wind_towards_home = wind.dot(to_home_dir); + const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); + + + // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient + const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( + 0.f, wind_towards_home); + + cruise_speed = ground_speed; + } + + return cruise_speed; +} + +void RtlDirect::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h new file mode 100644 index 0000000000..94ada8f514 --- /dev/null +++ b/src/modules/navigator/rtl_direct.h @@ -0,0 +1,241 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_direct.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include + +#include "mission_block.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class Navigator; + +class RtlDirect : public MissionBlock, public ModuleParams +{ +public: + /** + * @brief Return to launch position. + * Defines the position and landing yaw for the return to launch destination. + * + */ + struct RtlPosition { + double lat; /**< latitude in WGS84 [rad].*/ + double lon; /**< longitude in WGS84 [rad].*/ + float alt; /**< altitude in MSL [m].*/ + float yaw; /**< final yaw when landed [rad].*/ + }; + + RtlDirect(Navigator *navigator); + + ~RtlDirect() = default; + + /** + * @brief on inactivation + * + */ + void on_inactivation() override; + + /** + * @brief on activation. + * Initialize the return to launch calculations. + * + */ + void on_activation() override; + + /** + * @brief on active + * Update the return to launch calculation and set new setpoints for controller if necessary. + * + */ + void on_active() override; + + /** + * @brief Calculate the estimated time needed to return to launch. + * + * @return estimated time to return to launch. + */ + rtl_time_estimate_s calc_rtl_time_estimate(); + + void setReturnAltMin(bool min) { _enforce_rtl_alt = min; } + void setRtlAlt(float alt) {_rtl_alt = alt;}; + + void setRtlPosition(RtlPosition position) {_destination = position;}; + +private: + /** + * @brief Return to launch heading mode. + * + */ + enum RTLHeadingMode { + RTL_NAVIGATION_HEADING = 0, + RTL_DESTINATION_HEADING, + RTL_CURRENT_HEADING, + }; + + /** + * @brief Return to launch state machine. + * + */ + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_TRANSITION_TO_MC, + RTL_MOVE_TO_LAND_HOVER_VTOL, + RTL_STATE_LAND, + RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, + }; + +private: + /** + * @brief Get the horizontal wind velocity + * + * @return horizontal wind velocity. + */ + matrix::Vector2f get_wind(); + + /** + * @brief Set the return to launch control setpoint. + * + */ + void set_rtl_item(); + + /** + * @brief Advance the return to launch state machine. + * + */ + void advance_rtl(); + + /** + * @brief Get the Cruise Ground Speed + * + * @return Ground speed in cruise mode [m/s]. + */ + float getCruiseGroundSpeed(); + + /** + * @brief Get the climb rate + * + * @return Climb rate [m/s] + */ + float getClimbRate(); + + /** + * @brief Get the descend rate + * + * @return descend rate [m/s] + */ + float getDescendRate(); + + /** + * @brief Get the cruise speed + * + * @return cruise speed [m/s] + */ + float getCruiseSpeed(); + + /** + * @brief Get the Hover Land Speed + * + * @return Hover land speed [m/s] + */ + float getHoverLandSpeed(); + + /** + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + /** Current state in the state machine.*/ + RTLState _rtl_state{RTL_STATE_NONE}; + + bool _enforce_rtl_alt{false}; + + RtlPosition _destination{}; ///< the RTL position to fly to + + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position + + DEFINE_PARAMETERS( + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_descend_alt, + (ParamFloat) _param_rtl_land_delay, + (ParamFloat) _param_rtl_min_dist, + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad, + (ParamInt) _param_rtl_hdg_md, + (ParamFloat) _param_rtl_time_factor, + (ParamInt) _param_rtl_time_margin + ) + + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; + param_t _param_mpc_land_speed{PARAM_INVALID}; + param_t _param_fw_climb_rate{PARAM_INVALID}; + param_t _param_fw_sink_rate{PARAM_INVALID}; + + param_t _param_fw_airspeed_trim{PARAM_INVALID}; + param_t _param_mpc_xy_cruise{PARAM_INVALID}; + param_t _param_rover_cruise_speed{PARAM_INVALID}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */ + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; +}; diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp new file mode 100644 index 0000000000..236b7b61c5 --- /dev/null +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -0,0 +1,281 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_direct_mission_land.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_direct_mission_land.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5; + +RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) : + RtlBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE) +{ + +} + +void RtlDirectMissionLand::on_activation() +{ + _land_detected_sub.update(); + _global_pos_sub.update(); + + _needs_climbing = false; + + if (hasMissionLandStart()) { + _is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK); + + if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If enforce_rtl_alt is true then forcing altitude change even if above. + _needs_climbing = true; + + } + + } else { + _is_current_planned_mission_item_valid = false; + } + + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +bool RtlDirectMissionLand::setNextMissionItem() +{ + return (goToNextPositionItem(true) == PX4_OK); +} + +void RtlDirectMissionLand::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Climb to altitude + if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, + // even if current climb altitude is below (e.g. RTL immediately after take off) + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL Mission land: climb to %d m\t", + (int)ceilf(_rtl_alt)); + events::send(events::ID("rtl_mission_land_climb"), events::Log::Info, + "RTL Mission Land: climb to {1m_v}", + (int32_t)ceilf(_rtl_alt)); + + _needs_climbing = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Transition to fixed wing if necessary. + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + int32_t next_mission_item_index; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlDirectMissionLand::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + } +} + +bool RtlDirectMissionLand::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h new file mode 100644 index 0000000000..002d26da67 --- /dev/null +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -0,0 +1,74 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_direct_mission_land.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "rtl_base.h" + +#include +#include +#include + +class Navigator; + +class RtlDirectMissionLand : public RtlBase +{ +public: + RtlDirectMissionLand(Navigator *navigator); + ~RtlDirectMissionLand() = default; + + void on_activation() override; + + rtl_time_estimate_s calc_rtl_time_estimate() override; + + void setReturnAltMin(bool min) override { _enforce_rtl_alt = min; }; + void setRtlAlt(float alt) override {_rtl_alt = alt;}; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + bool _needs_climbing{false}; //< Flag if climbing is required at the start + bool _enforce_rtl_alt{false}; + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position +}; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp new file mode 100644 index 0000000000..924ef8b8ba --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -0,0 +1,241 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5; + +RtlMissionFast::RtlMissionFast(Navigator *navigator) : + RtlBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) +{ + +} + +void RtlMissionFast::on_activation() +{ + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFast::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFast::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFast::setNextMissionItem() +{ + return (goToNextPositionItem(true) == PX4_OK); +} + +void RtlMissionFast::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Transition to fixed wing if necessary. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + int32_t next_mission_item_index; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + } +} + +bool RtlMissionFast::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h new file mode 100644 index 0000000000..576152efe2 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "rtl_base.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFast : public RtlBase +{ +public: + RtlMissionFast(Navigator *navigator); + ~RtlMissionFast() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate() override; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ +}; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp new file mode 100644 index 0000000000..1b5feb850d --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -0,0 +1,268 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast_reverse.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast_reverse.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5; + +RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : + RtlBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) +{ + +} + +void RtlMissionFastReverse::on_activation() +{ + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidate the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFastReverse::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFastReverse::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFastReverse::setNextMissionItem() +{ + return (goToPreviousPositionItem(true) == PX4_OK); +} + +void RtlMissionFastReverse::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Transition to fixed wing if necessary. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + int32_t next_mission_item_index; + size_t num_found_items = 0; + getPreviousPositionItems(_mission.current_seq, &next_mission_item_index, num_found_items, 1u); + + // If the current item is a takeoff item or there is no further position item start landing. + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || + num_found_items == 0) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed; + bool vtol_in_fw = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + if (needs_to_land) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Go to Take off location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + if (!PX4_ISFINITE(_mission_item.altitude)) { + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + } + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (vtol_in_fw) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) { + // Go to home location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.yaw = NAN; + + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } else { + _mission_item.altitude = _home_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + } + } +} + +bool RtlMissionFastReverse::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h new file mode 100644 index 0000000000..acc2893b3a --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast_reverse.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "rtl_base.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFastReverse : public RtlBase +{ +public: + RtlMissionFastReverse(Navigator *navigator); + ~RtlMissionFastReverse() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate() override; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ +}; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index f998b9c1d9..6c5af0de3d 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -98,60 +98,31 @@ Takeoff::set_takeoff_position() { struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); - float abs_altitude = 0.0f; + float takeoff_altitude_amsl = 0.f; - float min_abs_altitude; - - // TODO: review this, comments are talking about home pos, the validity is checked but the - // current altitude is used instead. Also, the "else" case does not consider the current altitude at all. - if (_navigator->home_alt_valid()) { //only use home position if it is valid - min_abs_altitude = _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt(); - - } else { //e.g. flow - min_abs_altitude = _navigator->get_takeoff_min_alt(); - } - - // Use altitude if it has been set. If home position is invalid use min_abs_altitude - events::LogLevel log_level = events::LogLevel::Disabled; - - if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_alt_valid()) { - abs_altitude = rep->current.alt; - - // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. - if (abs_altitude < min_abs_altitude) { - if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m\t", (double)_navigator->get_takeoff_min_alt()); - log_level = events::LogLevel::Warning; - } - - abs_altitude = min_abs_altitude; - } + if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) { + takeoff_altitude_amsl = rep->current.alt; } else { - // Use home + minimum clearance but only notify. - abs_altitude = min_abs_altitude; + takeoff_altitude_amsl = _navigator->get_global_position()->alt + _navigator->get_param_mis_takeoff_alt(); mavlink_log_info(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m\t", (double)_navigator->get_takeoff_min_alt()); - log_level = events::LogLevel::Info; + "Using default takeoff altitude: %.1f m\t", (double)_navigator->get_param_mis_takeoff_alt()); + + events::send(events::ID("navigator_takeoff_default_alt"), {events::Log::Info, events::LogInternal::Info}, + "Using default takeoff altitude: {1:.2m}", + _navigator->get_param_mis_takeoff_alt()); } - if (log_level != events::LogLevel::Disabled) { - events::send(events::ID("navigator_takeoff_min_alt"), {log_level, events::LogInternal::Info}, - "Using minimum takeoff altitude: {1:.2m}", - _navigator->get_takeoff_min_alt()); - } - - if (abs_altitude < _navigator->get_global_position()->alt) { + if (takeoff_altitude_amsl < _navigator->get_global_position()->alt) { // If the suggestion is lower than our current alt, let's not go down. - abs_altitude = _navigator->get_global_position()->alt; + takeoff_altitude_amsl = _navigator->get_global_position()->alt; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude\t"); events::send(events::ID("navigator_takeoff_already_higher"), {events::Log::Error, events::LogInternal::Info}, "Already higher than takeoff altitude (not descending)"); } // set current mission item to takeoff - set_takeoff_item(&_mission_item, abs_altitude); + set_takeoff_item(&_mission_item, takeoff_altitude_amsl); _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); @@ -181,12 +152,5 @@ Takeoff::set_takeoff_position() memset(rep, 0, sizeof(*rep)); } - if (PX4_ISFINITE(pos_sp_triplet->current.lat) && PX4_ISFINITE(pos_sp_triplet->current.lon)) { - _navigator->set_can_loiter_at_sp(true); - - } else { - _navigator->set_can_loiter_at_sp(false); - } - _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index af106a249d..45aeb30143 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -51,7 +51,7 @@ VtolTakeoff::VtolTakeoff(Navigator *navigator) : void VtolTakeoff::on_activation() { - if (_navigator->home_global_position_valid()) { + if (hrt_elapsed_time(&_navigator->get_global_position()->timestamp) < 1_s) { set_takeoff_position(); _takeoff_state = vtol_takeoff_state::TAKEOFF_HOVER; _navigator->reset_cruising_speed(); @@ -71,8 +71,8 @@ VtolTakeoff::on_active() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_navigator->get_home_position()->lat, - _navigator->get_home_position()->lon, _loiter_location(0), _loiter_location(1))); + _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, + _mission_item.lon, _loiter_location(0), _loiter_location(1))); _mission_item.force_heading = true; mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); @@ -172,8 +172,8 @@ VtolTakeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, _transition_alt_amsl); - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 6218b09e00..3f05082f9a 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -65,6 +65,7 @@ #include "ReplayEkf2.hpp" #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" +#define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt" using namespace std; using namespace time_literals; @@ -124,6 +125,38 @@ Replay::setupReplayFile(const char *file_name) _replay_file = strdup(file_name); } +void +Replay::setParameter(const string ¶meter_name, const double parameter_value) +{ + param_t handle = param_find(parameter_name.c_str()); + param_type_t param_format = param_type(handle); + + if (param_format == PARAM_TYPE_INT32) { + int32_t orig_value = 0; + param_get(handle, &orig_value); + + int32_t value = (int32_t)parameter_value; + + if (orig_value != value) { + PX4_WARN("Setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); + } + + param_set(handle, (const void *)&value); + + } else if (param_format == PARAM_TYPE_FLOAT) { + float orig_value = 0; + param_get(handle, &orig_value); + + float value = (float)parameter_value; + + if (fabsf(orig_value - value) > FLT_EPSILON) { + PX4_WARN("Setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); + } + + param_set(handle, (const void *)&value); + } +} + void Replay::setUserParams(const char *filename) { @@ -149,39 +182,59 @@ Replay::setUserParams(const char *filename) mystrstream >> pname; mystrstream >> value_string; - double param_value_double = stod(value_string); - - param_t handle = param_find(pname.c_str()); - param_type_t param_format = param_type(handle); _overridden_params.insert(pname); - if (param_format == PARAM_TYPE_INT32) { - int32_t orig_value = 0; - param_get(handle, &orig_value); + double param_value_double = stod(value_string); - int32_t value = (int32_t)param_value_double; - - if (orig_value != value) { - PX4_WARN("setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); - } - - param_set(handle, (const void *)&value); - - } else if (param_format == PARAM_TYPE_FLOAT) { - float orig_value = 0; - param_get(handle, &orig_value); - - float value = (float)param_value_double; - - if (fabsf(orig_value - value) > FLT_EPSILON) { - PX4_WARN("setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); - } - - param_set(handle, (const void *)&value); - } + setParameter(pname, param_value_double); } } +void +Replay::readDynamicParams(const char *filename) +{ + _dynamic_parameter_schedule.clear(); + + string line; + string param_name; + string value_string; + string time_string; + ifstream myfile(filename); + + if (!myfile.is_open()) { + return; + } + + PX4_INFO("Reading dynamic params from %s...", filename); + + while (!myfile.eof()) { + getline(myfile, line); + + if (line.empty() || line[0] == '#') { + continue; + } + + istringstream mystrstream(line); + mystrstream >> param_name; + mystrstream >> value_string; + mystrstream >> time_string; + + _dynamic_parameters.insert(param_name); + + double param_value = stod(value_string); + uint64_t change_timestamp = (uint64_t)(stod(time_string) * 1e6); + + // Construct and store parameter change event + ParameterChangeEvent change_event = {change_timestamp, param_name, param_value}; + _dynamic_parameter_schedule.push_back(change_event); + } + + // Sort by event time + sort(_dynamic_parameter_schedule.begin(), _dynamic_parameter_schedule.end()); + + _next_param_change = 0; +} + bool Replay::readFileHeader(std::ifstream &file) { @@ -611,7 +664,8 @@ Replay::readAndApplyParameter(std::ifstream &file, uint16_t msg_size) string type = key.substr(0, pos); string param_name = key.substr(pos + 1); - if (_overridden_params.find(param_name) != _overridden_params.end()) { + if (_overridden_params.find(param_name) != _overridden_params.end() || + _dynamic_parameters.find(param_name) != _dynamic_parameters.end()) { //this parameter is overridden, so don't apply it return true; } @@ -826,6 +880,7 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file) } setUserParams(PARAMS_OVERRIDE_FILE); + readDynamicParams(DYNAMIC_PARAMS_OVERRIDE_FILE); return true; } @@ -896,7 +951,7 @@ Replay::run() Subscription &sub = *_subscriptions[next_msg_id]; - if (next_file_time == 0) { + if (next_file_time == 0 || next_file_time < _file_start_time) { //someone didn't set the timestamp properly. Consider the message invalid nextDataMessage(replay_file, sub, next_msg_id); continue; @@ -908,6 +963,17 @@ Replay::run() readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); last_additional_message_pos = next_additional_message_pos; + // Perform scheduled parameter changes + while (_next_param_change < _dynamic_parameter_schedule.size() && + _dynamic_parameter_schedule[_next_param_change].timestamp <= next_file_time) { + const auto param_change = _dynamic_parameter_schedule[_next_param_change]; + PX4_WARN("Performing param change scheduled for t=%.3lf at t=%.3lf.", + (double)param_change.timestamp / 1.e6, + (double)next_file_time / 1.e6); + setParameter(param_change.parameter_name, param_change.parameter_value); + _next_param_change++; + } + const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset); // It's time to publish diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 94ecc9d416..ef6f0ee97b 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -220,6 +221,23 @@ protected: private: std::set _overridden_params; + + struct ParameterChangeEvent { + uint64_t timestamp; + std::string parameter_name; + double parameter_value; + + // Comparison operator such that sorting is done by timestamp + bool operator<(const ParameterChangeEvent &other) const + { + return timestamp < other.timestamp; + } + }; + + std::set _dynamic_parameters; + std::vector _dynamic_parameter_schedule; + size_t _next_param_change; + std::map _file_formats; ///< all formats we read from the file uint64_t _file_start_time; @@ -275,7 +293,9 @@ private: /** get the size of a type that can be an array */ static size_t sizeOfFullType(const std::string &type_name_full); + void setParameter(const std::string ¶meter_name, const double parameter_value); void setUserParams(const char *filename); + void readDynamicParams(const char *filename); std::string parseOrbFields(const std::string &fields); diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml index 70670f882c..5fa440d5ff 100644 --- a/src/modules/sensors/module.yaml +++ b/src/modules/sensors/module.yaml @@ -360,6 +360,7 @@ parameters: short: Magnetometer ${i} rotation relative to airframe long: | An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW. category: System type: enum values: @@ -405,12 +406,52 @@ parameters: 38: Roll 90°, Pitch 68°, Yaw 293° 39: Pitch 315° 40: Roll 90°, Pitch 315° + 100: Custom Euler Angle min: -1 - max: 40 + max: 100 default: -1 num_instances: *max_num_sensor_instances instance_start: 0 + CAL_MAG${i}_ROLL: + description: + short: Magnetometer ${i} Custom Euler Roll Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + + CAL_MAG${i}_PITCH: + description: + short: Magnetometer ${i} Custom Euler Pitch Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + + CAL_MAG${i}_YAW: + description: + short: Magnetometer ${i} Custom Euler Yaw Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + CAL_MAG${i}_XOFF: description: short: Magnetometer ${i} X-axis offset diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt index c8457cc2df..909f249a7c 100644 --- a/src/modules/sensors/vehicle_air_data/CMakeLists.txt +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -40,4 +40,6 @@ target_link_libraries(vehicle_air_data data_validator px4_work_queue sensor_calibration + PUBLIC + atmosphere ) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 472d228c58..4c15c74a2a 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -36,11 +36,14 @@ #include #include #include +#include + namespace sensors { using namespace matrix; +using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; @@ -189,8 +192,9 @@ void VehicleAirData::Run() // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; - float data_array[3] {pressure_corrected, report.temperature, PressureToAltitude(pressure_corrected)}; + float data_array[3] {pressure_corrected, report.temperature, getAltitudeFromPressure(pressure_corrected, pressure_sealevel_pa)}; _voter.put(uorb_index, report.timestamp, data_array, report.error_count, _priority[uorb_index]); _timestamp_sample_sum[uorb_index] += report.timestamp_sample; @@ -251,11 +255,11 @@ void VehicleAirData::Run() const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; - float altitude = PressureToAltitude(pressure_pa, temperature); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; + const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - float air_density = pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius - - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; @@ -295,32 +299,6 @@ void VehicleAirData::Run() perf_end(_cycle_perf); } -float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const -{ - // calculate altitude using the hypsometric equation - static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin - static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre - - // current pressure at MSL in kPa (QNH in hPa) - const float p1 = _param_sens_baro_qnh.get() * 0.1f; - - // measured pressure in kPa - const float p = pressure_pa * 0.001f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; - - return altitude; -} - void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us) { // check failover and report (save failover report for a cycle where parameters didn't update) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 390f25767f..fd28de34f8 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -79,8 +79,6 @@ private: bool ParametersUpdate(bool force = false); void UpdateStatus(); - float PressureToAltitude(float pressure_pa, float temperature = 15.f) const; - static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _sensors_status_baro_pub{ORB_ID(sensors_status_baro)}; diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 87f356781e..2e36e7cc46 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -112,22 +112,34 @@ void VehicleOpticalFlow::Run() if (_sensor_flow_sub.update(&sensor_optical_flow)) { // clear data accumulation if there's a gap in data - if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) - > sensor_optical_flow.integration_timespan_us * 1.5f) - || (_accumulated_count > 0 && _quality_sum == 0)) { + const uint64_t integration_gap_threshold_us = sensor_optical_flow.integration_timespan_us * 2; + + if ((sensor_optical_flow.timestamp_sample >= _flow_timestamp_sample_last + integration_gap_threshold_us) + || (_accumulated_count > 0 && (sensor_optical_flow.quality > 0) && _quality_sum == 0)) { + ClearAccumulatedData(); } - const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf( - sensor_optical_flow.integration_timespan_us); + const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - sensor_optical_flow.integration_timespan_us; const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp; // delta angle // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available - if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) { + if (sensor_optical_flow.delta_angle_available && Vector2f(sensor_optical_flow.delta_angle).isAllFinite()) { // passthrough integrated gyro if available - _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; + Vector3f delta_angle(sensor_optical_flow.delta_angle); + + if (!PX4_ISFINITE(delta_angle(2))) { + // Some sensors only provide X and Y angular rates, rotate them but place back the NAN on the Z axis + delta_angle(2) = 0.f; + _delta_angle += _flow_rotation * delta_angle; + _delta_angle(2) = NAN; + + } else { + _delta_angle += _flow_rotation * delta_angle; + } + _delta_angle_available = true; } else { @@ -203,12 +215,7 @@ void VehicleOpticalFlow::Run() const float interval_us = 1e6f / _param_sens_flow_rate.get(); // don't allow publishing faster than SENS_FLOW_RATE - if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) { - publish = false; - } - - // integrate for full interval unless we haven't published recently - if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) { + if (_integration_timespan_us < interval_us) { publish = false; } } @@ -271,8 +278,6 @@ void VehicleOpticalFlow::Run() vehicle_optical_flow.timestamp = hrt_absolute_time(); _vehicle_optical_flow_pub.publish(vehicle_optical_flow); - _last_publication_timestamp = vehicle_optical_flow.timestamp_sample; - // vehicle_optical_flow_vel if distance is available (for logging) if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { @@ -326,10 +331,12 @@ void VehicleOpticalFlow::Run() // gyro_rate flow_vel.gyro_rate[0] = measured_body_rate(0); flow_vel.gyro_rate[1] = measured_body_rate(1); + flow_vel.gyro_rate[2] = measured_body_rate(2); // gyro_rate_integral flow_vel.gyro_rate_integral[0] = gyro_xyz(0); flow_vel.gyro_rate_integral[1] = gyro_xyz(1); + flow_vel.gyro_rate_integral[2] = gyro_xyz(2); flow_vel.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index 186af594a8..fb424ea5af 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -117,8 +117,6 @@ private: uint16_t _quality_sum{0}; uint8_t _accumulated_count{0}; - hrt_abstime _last_publication_timestamp{0}; - int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations hrt_abstime _last_range_sensor_update{0}; diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 8462dec6d3..6c7a11b7a1 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -32,13 +32,13 @@ ############################################################################ # Find the gz_Transport library -find_package(gz-transport - #REQUIRED COMPONENTS core - NAMES - #ignition-transport11 # IGN (Fortress and earlier) no longer supported - gz-transport12 - #QUIET -) +# First look for GZ Harmonic libraries +find_package(gz-transport NAMES gz-transport13) + +# If Harmonic not found, look for GZ Garden libraries +if(NOT gz-transport_FOUND) + find_package(gz-transport NAMES gz-transport12) +endif() if(gz-transport_FOUND) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index db7517fc47..7b94bd63bd 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -282,7 +282,7 @@ int GZBridge::task_spawn(int argc, char *argv[]) #if defined(ENABLE_LOCKSTEP_SCHEDULER) // lockstep scheduler wait for initial clock set before returning - int sleep_count_limit = 1000; + int sleep_count_limit = 10000; while ((instance->world_time_us() == 0) && sleep_count_limit > 0) { // wait for first clock message diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index e8e3c5a244..316ff6195a 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -35,6 +35,7 @@ #include +#include #include #include diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 7b73e03f75..37e4014d20 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -81,6 +81,8 @@ if(gazebo_FOUND) iris_foggy_lidar iris_irlock iris_obs_avoid + iris_depth_camera + iris_downward_depth_camera iris_opt_flow iris_opt_flow_mockup iris_rplidar diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index fd8da11891..1fa076568b 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -546,6 +546,7 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.heading = Eulerf(_q).psi(); local_position.heading_good_for_control = true; + local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); _local_position_ground_truth_pub.publish(local_position); diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index bcbc908d3a..9e4be9832e 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -21,68 +21,104 @@ import os #include #include -#include +#include #include +#include @[for include in type_includes]@ #include +#include @[end for]@ +#define UXRCE_DEFAULT_POLL_RATE 10 + +typedef bool (*UcdrSerializeMethod)(const void* data, ucdrBuffer& buf, int64_t time_offset); + +static constexpr int max_topic_size = 512; +@[ for pub in publications]@ +static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too large, increase max_topic_size"); +@[ end for]@ + +struct SendSubscription { + const struct orb_metadata *orb_meta; + uxrObjectId data_writer; + const char* dds_type_name; + uint32_t topic_size; + UcdrSerializeMethod ucdr_serialize_method; +}; + // Subscribers for messages to send struct SendTopicsSubs { + SendSubscription send_subscriptions[@(len(publications))] = { @[ for pub in publications]@ - uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))}; - uxrObjectId @(pub['topic_simple'])_data_writer{}; + { ORB_ID(@(pub['topic_simple'])), + uxr_object_id(0, UXR_INVALID_ID), + "@(pub['dds_type'])", + ucdr_topic_size_@(pub['simple_base_type'])(), + &ucdr_serialize_@(pub['simple_base_type']), + }, @[ end for]@ + }; + + px4_pollfd_struct_t fds[@(len(publications))] {}; uint32_t num_payload_sent{}; + void init(); void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace); void reset(); }; +void SendTopicsSubs::init() { + for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { + fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta); + fds[idx].events = POLLIN; + orb_set_interval(fds[idx].fd, UXRCE_DEFAULT_POLL_RATE); + } +} + void SendTopicsSubs::reset() { num_payload_sent = 0; -@[ for idx, pub in enumerate(publications)]@ - @(pub['topic_simple'])_data_writer = uxr_object_id(0, UXR_INVALID_ID); -@[ end for]@ + for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { + send_subscriptions[idx].data_writer = uxr_object_id(0, UXR_INVALID_ID); + } }; void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace) { int64_t time_offset_us = session->time_offset / 1000; // ns -> us -@[ for idx, pub in enumerate(publications)]@ - { - @(pub['simple_base_type'])_s data; + alignas(sizeof(uint64_t)) char topic_data[max_topic_size]; - if (@(pub['topic_simple'])_sub.update(&data)) { - - if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { + for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { + if (fds[idx].revents & POLLIN) { + // Topic updated, copy data and send + orb_copy(send_subscriptions[idx].orb_meta, fds[idx].fd, &topic_data); + if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) { // data writer not created yet - create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); + create_data_writer(session, reliable_out_stream_id, participant_id, static_cast(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].orb_meta->o_name, + send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); } - if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { + if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) { ucdrBuffer ub; - uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); - if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { - ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); + uint32_t topic_size = send_subscriptions[idx].topic_size; + if (uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { + send_subscriptions[idx].ucdr_serialize_method(&topic_data, ub, time_offset_us); // TODO: fill up the MTU and then flush, which reduces the packet overhead uxr_flash_output_streams(session); num_payload_sent += topic_size; } else { - //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); } } else { - //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); + //PX4_ERR("Error UXR_INVALID_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); } } } -@[ end for]@ } // Publishers for received messages diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 75fd6dbd9c..ff31bbbedc 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -85,43 +85,28 @@ with open(args.yaml_file, 'r') as file: merged_em_globals = {} all_type_includes = [] -for p in msg_map['publications']: +def process_message_type(msg_type): # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint - simple_base_type = p['type'].split('::')[-1] + simple_base_type = msg_type['type'].split('::')[-1] # eg TrajectoryWaypoint -> trajectory_waypoint base_type_name_snake_case = re.sub(r'(? trajectory_waypoint - base_type_name_snake_case = re.sub(r'(?%s/px4_micro_xrce_dds%s", - _localhost_only ? - "" - "" - "" - "" - "udp_localhost" - "UDPv4" - "
127.0.0.1
" - "
" - "
" - "
" - "" - "" - : - "" - "" - "", - _client_namespace != nullptr ? - _client_namespace - : - "", - _localhost_only ? - "false" - "udp_localhost" - "" - "" - "" - : - "" - "" - "
" - ); - - if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) { - PX4_ERR("create entities failed: namespace too long"); - return; - } - - uint16_t participant_req{}; if (_custom_participant) { + // Create participant by reference (XML not required) participant_req = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id, domain_id, "px4_participant", UXR_REPLACE); } else { + // Construct participant XML and create participant by XML + char participant_xml[PARTICIPANT_XML_SIZE]; + int ret = snprintf(participant_xml, PARTICIPANT_XML_SIZE, "%s%s/px4_micro_xrce_dds%s", + _localhost_only ? + "" + "" + "" + "" + "udp_localhost" + "UDPv4" + "
127.0.0.1
" + "
" + "
" + "
" + "" + "" + : + "" + "" + "", + _client_namespace != nullptr ? + _client_namespace + : + "", + _localhost_only ? + "false" + "udp_localhost" + "" + "" + "" + : + "" + "" + "
" + ); + + if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) { + PX4_ERR("create entities failed: namespace too long"); + return; + } + participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id, participant_xml, UXR_REPLACE); } @@ -335,9 +332,31 @@ void UxrceddsClient::run() bool had_ping_reply = false; uint32_t last_num_payload_sent{}; uint32_t last_num_payload_received{}; + int poll_error_counter = 0; + + _subs->init(); while (!should_exit() && _connected) { + /* Wait for topic updates for max 1000 ms (1sec) */ + int poll = px4_poll(&_subs->fds[0], (sizeof(_subs->fds) / sizeof(_subs->fds[0])), 1000); + + /* Handle the poll results */ + if (poll == 0) { + /* Timeout, no updates in selected uorbs */ + continue; + + } else if (poll < 0) { + /* Error */ + if (poll_error_counter < 10 || poll_error_counter % 50 == 0) { + /* Prevent flooding */ + PX4_ERR("ERROR while polling uorbs: %d", poll); + } + + poll_error_counter++; + continue; + } + _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); uxr_run_session_timeout(&session, 0); @@ -388,7 +407,6 @@ void UxrceddsClient::run() _connected = false; } - px4_usleep(1000); } uxr_delete_session_retries(&session, _connected ? 1 : 0); @@ -563,21 +581,22 @@ int UxrceddsClient::print_status() #if defined(UXRCE_DDS_CLIENT_UDP) if (_transport_udp != nullptr) { - PX4_INFO("Using transport: udp"); - PX4_INFO("Agent IP: %s", _agent_ip); - PX4_INFO("Agent port: %s", _port); - + PX4_INFO("Using transport: udp"); + PX4_INFO("Agent IP: %s", _agent_ip); + PX4_INFO("Agent port: %s", _port); + PX4_INFO("Custom participant: %s", _custom_participant ? "yes" : "no"); + PX4_INFO("Localhost only: %s", _localhost_only ? "yes" : "no"); } #endif if (_transport_serial != nullptr) { - PX4_INFO("Using transport: serial"); + PX4_INFO("Using transport: serial"); } if (_connected) { - PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate); - PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate); + PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate); + PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate); } return 0; @@ -606,7 +625,7 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) const char *client_namespace = nullptr;//"px4"; - while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:lcn:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 't': if (!strcmp(myoptarg, "serial")) { @@ -643,14 +662,6 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) case 'p': snprintf(port, PORT_MAX_LENGTH, "%s", myoptarg); break; - - case 'l': - localhost_only = true; - break; - - case 'c': - custom_participant = true; - break; #endif // UXRCE_DDS_CLIENT_UDP case 'n': @@ -693,6 +704,19 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) static_cast(ip_i & 0xff)); } + int32_t participant_config = 0; + param_get(param_find("UXRCE_DDS_PTCFG"), &participant_config); + + switch (participant_config) { + case 1: + localhost_only = true; + break; + + case 2: + custom_participant = true; + break; + } + #endif // UXRCE_DDS_CLIENT_UDP if (error_flag) { @@ -742,8 +766,6 @@ $ uxrce_dds_client start -t udp -h 127.0.0.1 -p 15555 PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:)", true); PRINT_MODULE_USAGE_PARAM_STRING('h', nullptr, "", "Agent IP. If not provided, defaults to UXRCE_DDS_AG_IP", true); PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 65535, "Agent listening port. If not provided, defaults to UXRCE_DDS_PRT", true); - PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true); - PRINT_MODULE_USAGE_PARAM_FLAG('c', "Use custom participant config (profile_name=\"px4_participant\")", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9be4ce5b3e..f8eee38ba7 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -204,16 +204,16 @@ void Standard::update_transition_state() _param_vt_psher_slew.get() * _dt, _param_vt_f_trans_thr.get()); } - _airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get(); + _airspeed_trans_blend_margin = getTransitionAirspeed() - getBlendAirspeed(); // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed if (_airspeed_trans_blend_margin > 0.0f && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && _airspeed_validated->calibrated_airspeed_m_s > 0.0f && - _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() && + _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed() && _time_since_trans_start > getMinimumFrontTransitionTime()) { - mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) / + mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2215b96de2..0db029c161 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -43,7 +43,6 @@ #include "vtol_att_control_main.h" using namespace matrix; -using namespace time_literals; #define FRONTTRANS_THR_MIN 0.25f #define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 0.5f @@ -182,44 +181,8 @@ void Tiltrotor::update_mc_state() { VtolType::update_mc_state(); - /*Motor spin up: define the first second after arming as motor spin up time, during which - * the tilt is set to the value of VT_TILT_SPINUP. This allows the user to set a spin up - * tilt angle in case the propellers don't spin up smoothly in full upright (MC mode) position. - */ - - const int spin_up_duration_p1 = 1000_ms; // duration of 1st phase of spinup (at fixed tilt) - const int spin_up_duration_p2 = 700_ms; // duration of 2nd phase of spinup (transition from spinup tilt to mc tilt) - - // reset this timestamp while disarmed - if (!_v_control_mode->flag_armed) { - _last_timestamp_disarmed = hrt_absolute_time(); - _tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0 - - } else if (_tilt_motors_for_startup) { - // leave motors tilted forward after arming to allow them to spin up easier - if (hrt_absolute_time() - _last_timestamp_disarmed > (spin_up_duration_p1 + spin_up_duration_p2)) { - _tilt_motors_for_startup = false; - } - } - - if (_tilt_motors_for_startup) { - if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) { - _tilt_control = _param_vt_tilt_spinup.get(); - - } else { - // duration phase 2: begin to adapt tilt to multicopter tilt - float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get()); - _tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() - - (_last_timestamp_disarmed + spin_up_duration_p1)); - } - - _mc_yaw_weight = 0.0f; //disable yaw control during spinup - - } else { - // normal operation - _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); - _mc_yaw_weight = 1.0f; - } + _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); + _mc_yaw_weight = 1.0f; } void Tiltrotor::update_fw_state() @@ -281,9 +244,9 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = 1.0f; if (!_param_fw_arsp_mode.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && - _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get()) { - const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) / - (_param_vt_arsp_trans.get() - _param_vt_arsp_blend.get()); + _airspeed_validated->calibrated_airspeed_m_s >= getBlendAirspeed()) { + const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - getBlendAirspeed()) / + (getTransitionAirspeed() - getBlendAirspeed()); _mc_roll_weight = weight; _mc_yaw_weight = weight; } diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index b5baa864da..cac172c2ae 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -91,14 +91,11 @@ private: void blendThrottleDuringBacktransition(const float scale, const float target_throttle); bool isFrontTransitionCompletedBase() override; - hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ - bool _tilt_motors_for_startup{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, (ParamFloat) _param_vt_tilt_mc, (ParamFloat) _param_vt_tilt_trans, (ParamFloat) _param_vt_tilt_fw, - (ParamFloat) _param_vt_tilt_spinup, (ParamFloat) _param_vt_trans_p2_dur, (ParamFloat) _param_vt_bt_tilt_dur ) diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 9d8d76ea89..92fa558454 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -71,20 +71,6 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.4f); */ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); -/** - * Tilt when disarmed and in the first second after arming - * - * This specific tilt during spin-up is necessary for some systems whose motors otherwise don't - * spin-up freely. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 2 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f); - /** * Duration of front transition phase 2 * diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 85908e2571..3bcb56e498 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2023 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 @@ -210,9 +210,9 @@ PARAM_DEFINE_FLOAT(VT_QC_ALT_LOSS, 0.0f); /** * Quad-chute transition altitude loss threshold * - * Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight. + * Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight + * in altitude-controlled flight modes. * Active until 5s after completing transition to fixed-wing. - * Only active if altitude estimate is valid and in altitude-controlled mode. * If the current altitude is more than this value below the altitude at the beginning of the * transition, it will instantly switch back to MC mode and execute behavior defined in COM_QC_ACT. * @@ -369,8 +369,7 @@ PARAM_DEFINE_FLOAT(VT_PITCH_MIN, -5.0f); * Minimum pitch angle during hover landing. * * Overrides VT_PITCH_MIN when the vehicle is in LAND mode (hovering). - * During landing it can be beneficial to allow lower minimum pitch angles as it can avoid the wings - * generating too much lift and preventing the vehicle from sinking at the desired rate. + * During landing it can be beneficial to reduce the pitch angle to reduce the generated lift in head wind. * * @unit deg * @min -10.0 diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 0a558b0ec6..ea66ea5348 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -50,6 +50,11 @@ using namespace matrix; #define THROTTLE_BLENDING_DUR_S 1.0f +// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float kMinWeightRatio = 0.5f; + +// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float kMaxWeightRatio = 2.0f; VtolType::VtolType(VtolAttitudeControl *att_controller) : ModuleParams(nullptr), @@ -210,7 +215,7 @@ bool VtolType::isFrontTransitionCompletedBase() if (airspeed_triggers_transition) { transition_to_fw = minimum_trans_time_elapsed - && _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get(); + && _airspeed_validated->calibrated_airspeed_m_s >= getTransitionAirspeed(); } else { transition_to_fw = openloop_trans_time_elapsed; @@ -581,3 +586,21 @@ float VtolType::getOpenLoopFrontTransitionTime() const { return getFrontTransitionTimeFactor() * _param_vt_f_tr_ol_tm.get(); } +float VtolType::getTransitionAirspeed() const +{ + return math::max(_param_vt_arsp_trans.get(), getMinimumTransitionAirspeed()); +} +float VtolType::getMinimumTransitionAirspeed() const +{ + float weight_ratio = 1.0f; + + if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { + weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), kMinWeightRatio, kMaxWeightRatio); + } + + return sqrtf(weight_ratio) * _param_airspeed_min.get(); +} +float VtolType::getBlendAirspeed() const +{ + return _param_vt_arsp_blend.get(); +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 84dfbe6d00..93a2ff4494 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -237,6 +237,24 @@ public: */ float getOpenLoopFrontTransitionTime() const; + /** + * + * @return The minimum calibrated airspeed compensated for weight [m/s] + */ + float getMinimumTransitionAirspeed() const; + + /** + * + * @return The calibrated blending airspeed [m/s] + */ + float getBlendAirspeed() const; + + /** + * + * @return The calibrated transition airspeed [m/s] + */ + float getTransitionAirspeed() const; + virtual void parameters_update() = 0; /** @@ -343,7 +361,11 @@ protected: (ParamInt) _param_vt_fwd_thrust_en, (ParamFloat) _param_mpc_land_alt1, (ParamFloat) _param_mpc_land_alt2, - (ParamFloat) _param_vt_lnd_pitch_min + (ParamFloat) _param_vt_lnd_pitch_min, + (ParamFloat) _param_weight_base, + (ParamFloat) _param_weight_gross, + (ParamFloat) _param_airspeed_min + ) private: diff --git a/src/modules/zenoh/CMakeLists.txt b/src/modules/zenoh/CMakeLists.txt new file mode 100644 index 0000000000..204bad04a7 --- /dev/null +++ b/src/modules/zenoh/CMakeLists.txt @@ -0,0 +1,100 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +function(message) + if (NOT MESSAGE_QUIET) + _message(${ARGN}) + endif() +endfunction() + +set(POSIX_COMPATIBLE YES) +set(BUILD_SHARED_LIBS OFF) +set(BUILD_TESTING OFF) +set(CHECK_THREADS NO) +set(MESSAGE_QUIET ON) +set(ZENOH_DEBUG ${CONFIG_ZENOH_DEBUG}) + +px4_add_git_submodule(TARGET git_zenoh-pico PATH "zenoh-pico") +add_subdirectory(zenoh-pico) +unset(MESSAGE_QUIET) +add_dependencies(zenohpico git_zenoh-pico px4_platform) +target_compile_options(zenohpico PUBLIC -Wno-cast-align + -Wno-narrowing + -Wno-stringop-overflow + -Wno-unused-result + -DZ_BATCH_SIZE_RX=512 + -DZ_BATCH_SIZE_TX=512 + -DZ_FRAG_MAX_SIZE=1024) + +if(CONFIG_PLATFORM_NUTTX) + target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF) +endif() + +if(CONFIG_ZENOH_SERIAL) + target_compile_options(zenohpico PRIVATE -DZ_LINK_SERIAL) +endif() + + +px4_add_module( + MODULE modules__zenoh + MAIN zenoh + SRCS + zenoh.cpp + zenoh_config.cpp + zenoh.h + publishers/zenoh_publisher.cpp + subscribers/zenoh_subscriber.cpp + MODULE_CONFIG + module.yaml + DEPENDS + cdr + uorb_msgs + px4_work_queue + zenohpico + zenoh_topics + git_zenoh-pico + INCLUDES + ${PX4_BINARY_DIR}/msg + zenoh-pico/include + ${CMAKE_CURRENT_LIST_DIR} + ${PX4_BINARY_DIR}/src/modules/zenoh/ + COMPILE_FLAGS + -Wno-pointer-compare + -Wno-cast-align + -Wno-address-of-packed-member + -Wno-double-promotion + -Wno-unused + -DZENOH_LINUX + -DZENOH_NO_STDATOMIC + -D_Bool=int8_t +) diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig new file mode 100644 index 0000000000..d868dae392 --- /dev/null +++ b/src/modules/zenoh/Kconfig @@ -0,0 +1,73 @@ +menuconfig MODULES_ZENOH + bool "Zenoh" + default n + select LIB_CDRSTREAM + ---help--- + Enable support for Zenoh + +if MODULES_ZENOH + config ZENOH_CONFIG_PATH + string "Config path" + default "/fs/microsd/zenoh" + help + Path to store network, publishers and subscribers configuration + + config ZENOH_SERIAL + bool "Zenoh serial transport" + default n + help + Enables transport over serial (Not yet supported on NuttX/Linux) + + config ZENOH_DEBUG + int "Zenoh debug level" + default 0 + help + Set Zenoh debug level + 0: NONE + 1: ERROR + 2: INFO + ERROR + 3: DEBUG + INFO + ERROR + + # Choose exactly one item + choice ZENOH_PUBSUB_SELECTION + prompt "Publishers/Subscribers selection" + default ZENOH_PUBSUB_ALL + + config ZENOH_PUBSUB_MINIMAL + bool "Minimal" + select ZENOH_PUBSUB_COLLISION_CONSTRAINTS + select ZENOH_PUBSUB_FAILSAFE_FLAGS + select ZENOH_PUBSUB_POSITION_SETPOINT_TRIPLET + select ZENOH_PUBSUB_SENSOR_COMBINED + select ZENOH_PUBSUB_TIMESYNC_STATUS + select ZENOH_PUBSUB_VEHICLE_ATTITUDE + select ZENOH_PUBSUB_VEHICLE_CONTROL_MODE + select ZENOH_PUBSUB_VEHICLE_GLOBAL_POSITION + select ZENOH_PUBSUB_SENSOR_GPS + select ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION + select ZENOH_PUBSUB_VEHICLE_ODOMETRY + select ZENOH_PUBSUB_VEHICLE_STATUS + select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT + select ZENOH_PUBSUB_OFFBOARD_CONTROL_MODE + select ZENOH_PUBSUB_ONBOARD_COMPUTER_STATUS + select ZENOH_PUBSUB_OBSTACLE_DISTANCE + select ZENOH_PUBSUB_SENSOR_OPTICAL_FLOW + select ZENOH_PUBSUB_TELEMETRY_STATUS + select ZENOH_PUBSUB_TRAJECTORY_SETPOINT + select ZENOH_PUBSUB_VEHICLE_ATTITUDE_SETPOINT + select ZENOH_PUBSUB_VEHICLE_ODOMETRY + select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT + select ZENOH_PUBSUB_VEHICLE_COMMAND + select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER + select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT + + config ZENOH_PUBSUB_ALL + bool "All" + + config ZENOH_PUBSUB_CUSTOM + bool "Custom" + endchoice + +endif + +rsource "Kconfig.topics" diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics new file mode 100644 index 0000000000..643109d93f --- /dev/null +++ b/src/modules/zenoh/Kconfig.topics @@ -0,0 +1,960 @@ + +menu "Zenoh publishers/subscribers" + depends on MODULES_ZENOH + config ZENOH_PUBSUB_ACTION_REQUEST + bool "action_request" + default n + + config ZENOH_PUBSUB_ACTUATOR_ARMED + bool "actuator_armed" + default n + + config ZENOH_PUBSUB_ACTUATOR_CONTROLS_STATUS + bool "actuator_controls_status" + default n + + config ZENOH_PUBSUB_ACTUATOR_MOTORS + bool "actuator_motors" + default n + + config ZENOH_PUBSUB_ACTUATOR_OUTPUTS + bool "actuator_outputs" + default n + + config ZENOH_PUBSUB_ACTUATOR_SERVOS + bool "actuator_servos" + default n + + config ZENOH_PUBSUB_ACTUATOR_SERVOS_TRIM + bool "actuator_servos_trim" + default n + + config ZENOH_PUBSUB_ACTUATOR_TEST + bool "actuator_test" + default n + + config ZENOH_PUBSUB_ADC_REPORT + bool "adc_report" + default n + + config ZENOH_PUBSUB_AIRSPEED + bool "airspeed" + default n + + config ZENOH_PUBSUB_AIRSPEED_VALIDATED + bool "airspeed_validated" + default n + + config ZENOH_PUBSUB_AIRSPEED_WIND + bool "airspeed_wind" + default n + + config ZENOH_PUBSUB_AUTOTUNE_ATTITUDE_CONTROL_STATUS + bool "autotune_attitude_control_status" + default n + + config ZENOH_PUBSUB_BATTERY_STATUS + bool "battery_status" + default n + + config ZENOH_PUBSUB_BUTTON_EVENT + bool "button_event" + default n + + config ZENOH_PUBSUB_CAMERA_CAPTURE + bool "camera_capture" + default n + + config ZENOH_PUBSUB_CAMERA_STATUS + bool "camera_status" + default n + + config ZENOH_PUBSUB_CAMERA_TRIGGER + bool "camera_trigger" + default n + + config ZENOH_PUBSUB_CELLULAR_STATUS + bool "cellular_status" + default n + + config ZENOH_PUBSUB_COLLISION_CONSTRAINTS + bool "collision_constraints" + default n + + config ZENOH_PUBSUB_COLLISION_REPORT + bool "collision_report" + default n + + config ZENOH_PUBSUB_CONTROL_ALLOCATOR_STATUS + bool "control_allocator_status" + default n + + config ZENOH_PUBSUB_CPULOAD + bool "cpuload" + default n + + config ZENOH_PUBSUB_DATAMAN_REQUEST + bool "dataman_request" + default n + + config ZENOH_PUBSUB_DATAMAN_RESPONSE + bool "dataman_response" + default n + + config ZENOH_PUBSUB_DEBUG_ARRAY + bool "debug_array" + default n + + config ZENOH_PUBSUB_DEBUG_KEY_VALUE + bool "debug_key_value" + default n + + config ZENOH_PUBSUB_DEBUG_VALUE + bool "debug_value" + default n + + config ZENOH_PUBSUB_DEBUG_VECT + bool "debug_vect" + default n + + config ZENOH_PUBSUB_DIFFERENTIAL_PRESSURE + bool "differential_pressure" + default n + + config ZENOH_PUBSUB_DISTANCE_SENSOR + bool "distance_sensor" + default n + + config ZENOH_PUBSUB_EKF2_TIMESTAMPS + bool "ekf2_timestamps" + default n + + config ZENOH_PUBSUB_ESC_REPORT + bool "esc_report" + default n + + config ZENOH_PUBSUB_ESC_STATUS + bool "esc_status" + default n + + config ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE1D + bool "estimator_aid_source1d" + default n + + config ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE2D + bool "estimator_aid_source2d" + default n + + config ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE3D + bool "estimator_aid_source3d" + default n + + config ZENOH_PUBSUB_ESTIMATOR_BIAS + bool "estimator_bias" + default n + + config ZENOH_PUBSUB_ESTIMATOR_BIAS3D + bool "estimator_bias3d" + default n + + config ZENOH_PUBSUB_ESTIMATOR_EVENT_FLAGS + bool "estimator_event_flags" + default n + + config ZENOH_PUBSUB_ESTIMATOR_GPS_STATUS + bool "estimator_gps_status" + default n + + config ZENOH_PUBSUB_ESTIMATOR_INNOVATIONS + bool "estimator_innovations" + default n + + config ZENOH_PUBSUB_ESTIMATOR_SELECTOR_STATUS + bool "estimator_selector_status" + default n + + config ZENOH_PUBSUB_ESTIMATOR_SENSOR_BIAS + bool "estimator_sensor_bias" + default n + + config ZENOH_PUBSUB_ESTIMATOR_STATES + bool "estimator_states" + default n + + config ZENOH_PUBSUB_ESTIMATOR_STATUS + bool "estimator_status" + default n + + config ZENOH_PUBSUB_ESTIMATOR_STATUS_FLAGS + bool "estimator_status_flags" + default n + + config ZENOH_PUBSUB_EVENT + bool "event" + default n + + config ZENOH_PUBSUB_FAILSAFE_FLAGS + bool "failsafe_flags" + default n + + config ZENOH_PUBSUB_FAILURE_DETECTOR_STATUS + bool "failure_detector_status" + default n + + config ZENOH_PUBSUB_FOLLOW_TARGET + bool "follow_target" + default n + + config ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR + bool "follow_target_estimator" + default n + + config ZENOH_PUBSUB_FOLLOW_TARGET_STATUS + bool "follow_target_status" + default n + + config ZENOH_PUBSUB_GENERATOR_STATUS + bool "generator_status" + default n + + config ZENOH_PUBSUB_GEOFENCE_RESULT + bool "geofence_result" + default n + + config ZENOH_PUBSUB_GIMBAL_CONTROLS + bool "gimbal_controls" + default n + + config ZENOH_PUBSUB_GIMBAL_DEVICE_ATTITUDE_STATUS + bool "gimbal_device_attitude_status" + default n + + config ZENOH_PUBSUB_GIMBAL_DEVICE_INFORMATION + bool "gimbal_device_information" + default n + + config ZENOH_PUBSUB_GIMBAL_DEVICE_SET_ATTITUDE + bool "gimbal_device_set_attitude" + default n + + config ZENOH_PUBSUB_GIMBAL_MANAGER_INFORMATION + bool "gimbal_manager_information" + default n + + config ZENOH_PUBSUB_GIMBAL_MANAGER_SET_ATTITUDE + bool "gimbal_manager_set_attitude" + default n + + config ZENOH_PUBSUB_GIMBAL_MANAGER_SET_MANUAL_CONTROL + bool "gimbal_manager_set_manual_control" + default n + + config ZENOH_PUBSUB_GIMBAL_MANAGER_STATUS + bool "gimbal_manager_status" + default n + + config ZENOH_PUBSUB_GPIO_CONFIG + bool "gpio_config" + default n + + config ZENOH_PUBSUB_GPIO_IN + bool "gpio_in" + default n + + config ZENOH_PUBSUB_GPIO_OUT + bool "gpio_out" + default n + + config ZENOH_PUBSUB_GPIO_REQUEST + bool "gpio_request" + default n + + config ZENOH_PUBSUB_GPS_DUMP + bool "gps_dump" + default n + + config ZENOH_PUBSUB_GPS_INJECT_DATA + bool "gps_inject_data" + default n + + config ZENOH_PUBSUB_GRIPPER + bool "gripper" + default n + + config ZENOH_PUBSUB_HEALTH_REPORT + bool "health_report" + default n + + config ZENOH_PUBSUB_HEATER_STATUS + bool "heater_status" + default n + + config ZENOH_PUBSUB_HOME_POSITION + bool "home_position" + default n + + config ZENOH_PUBSUB_HOVER_THRUST_ESTIMATE + bool "hover_thrust_estimate" + default n + + config ZENOH_PUBSUB_INPUT_RC + bool "input_rc" + default n + + config ZENOH_PUBSUB_INTERNAL_COMBUSTION_ENGINE_STATUS + bool "internal_combustion_engine_status" + default n + + config ZENOH_PUBSUB_IRIDIUMSBD_STATUS + bool "iridiumsbd_status" + default n + + config ZENOH_PUBSUB_IRLOCK_REPORT + bool "irlock_report" + default n + + config ZENOH_PUBSUB_LANDING_GEAR + bool "landing_gear" + default n + + config ZENOH_PUBSUB_LANDING_GEAR_WHEEL + bool "landing_gear_wheel" + default n + + config ZENOH_PUBSUB_LANDING_TARGET_INNOVATIONS + bool "landing_target_innovations" + default n + + config ZENOH_PUBSUB_LANDING_TARGET_POSE + bool "landing_target_pose" + default n + + config ZENOH_PUBSUB_LAUNCH_DETECTION_STATUS + bool "launch_detection_status" + default n + + config ZENOH_PUBSUB_LED_CONTROL + bool "led_control" + default n + + config ZENOH_PUBSUB_LOG_MESSAGE + bool "log_message" + default n + + config ZENOH_PUBSUB_LOGGER_STATUS + bool "logger_status" + default n + + config ZENOH_PUBSUB_MAG_WORKER_DATA + bool "mag_worker_data" + default n + + config ZENOH_PUBSUB_MAGNETOMETER_BIAS_ESTIMATE + bool "magnetometer_bias_estimate" + default n + + config ZENOH_PUBSUB_MANUAL_CONTROL_SETPOINT + bool "manual_control_setpoint" + default n + + config ZENOH_PUBSUB_MANUAL_CONTROL_SWITCHES + bool "manual_control_switches" + default n + + config ZENOH_PUBSUB_MAVLINK_LOG + bool "mavlink_log" + default n + + config ZENOH_PUBSUB_MAVLINK_TUNNEL + bool "mavlink_tunnel" + default n + + config ZENOH_PUBSUB_MISSION + bool "mission" + default n + + config ZENOH_PUBSUB_MISSION_RESULT + bool "mission_result" + default n + + config ZENOH_PUBSUB_MODE_COMPLETED + bool "mode_completed" + default n + + config ZENOH_PUBSUB_MOUNT_ORIENTATION + bool "mount_orientation" + default n + + config ZENOH_PUBSUB_NAVIGATOR_MISSION_ITEM + bool "navigator_mission_item" + default n + + config ZENOH_PUBSUB_NORMALIZED_UNSIGNED_SETPOINT + bool "normalized_unsigned_setpoint" + default n + + config ZENOH_PUBSUB_NPFG_STATUS + bool "npfg_status" + default n + + config ZENOH_PUBSUB_OBSTACLE_DISTANCE + bool "obstacle_distance" + default n + + config ZENOH_PUBSUB_OFFBOARD_CONTROL_MODE + bool "offboard_control_mode" + default n + + config ZENOH_PUBSUB_ONBOARD_COMPUTER_STATUS + bool "onboard_computer_status" + default n + + config ZENOH_PUBSUB_ORB_TEST + bool "orb_test" + default n + + config ZENOH_PUBSUB_ORB_TEST_LARGE + bool "orb_test_large" + default n + + config ZENOH_PUBSUB_ORB_TEST_MEDIUM + bool "orb_test_medium" + default n + + config ZENOH_PUBSUB_ORBIT_STATUS + bool "orbit_status" + default n + + config ZENOH_PUBSUB_PARAMETER_UPDATE + bool "parameter_update" + default n + + config ZENOH_PUBSUB_PING + bool "ping" + default n + + config ZENOH_PUBSUB_POSITION_CONTROLLER_LANDING_STATUS + bool "position_controller_landing_status" + default n + + config ZENOH_PUBSUB_POSITION_CONTROLLER_STATUS + bool "position_controller_status" + default n + + config ZENOH_PUBSUB_POSITION_SETPOINT + bool "position_setpoint" + default n + + config ZENOH_PUBSUB_POSITION_SETPOINT_TRIPLET + bool "position_setpoint_triplet" + default n + + config ZENOH_PUBSUB_POWER_BUTTON_STATE + bool "power_button_state" + default n + + config ZENOH_PUBSUB_POWER_MONITOR + bool "power_monitor" + default n + + config ZENOH_PUBSUB_PPS_CAPTURE + bool "pps_capture" + default n + + config ZENOH_PUBSUB_PWM_INPUT + bool "pwm_input" + default n + + config ZENOH_PUBSUB_PX4IO_STATUS + bool "px4io_status" + default n + + config ZENOH_PUBSUB_QSHELL_REQ + bool "qshell_req" + default n + + config ZENOH_PUBSUB_QSHELL_RETVAL + bool "qshell_retval" + default n + + config ZENOH_PUBSUB_RADIO_STATUS + bool "radio_status" + default n + + config ZENOH_PUBSUB_RATE_CTRL_STATUS + bool "rate_ctrl_status" + default n + + config ZENOH_PUBSUB_RC_CHANNELS + bool "rc_channels" + default n + + config ZENOH_PUBSUB_RC_PARAMETER_MAP + bool "rc_parameter_map" + default n + + config ZENOH_PUBSUB_RPM + bool "rpm" + default n + + config ZENOH_PUBSUB_RTL_TIME_ESTIMATE + bool "rtl_time_estimate" + default n + + config ZENOH_PUBSUB_SATELLITE_INFO + bool "satellite_info" + default n + + config ZENOH_PUBSUB_SENSOR_ACCEL + bool "sensor_accel" + default n + + config ZENOH_PUBSUB_SENSOR_ACCEL_FIFO + bool "sensor_accel_fifo" + default n + + config ZENOH_PUBSUB_SENSOR_BARO + bool "sensor_baro" + default n + + config ZENOH_PUBSUB_SENSOR_COMBINED + bool "sensor_combined" + default n + + config ZENOH_PUBSUB_SENSOR_CORRECTION + bool "sensor_correction" + default n + + config ZENOH_PUBSUB_SENSOR_GNSS_RELATIVE + bool "sensor_gnss_relative" + default n + + config ZENOH_PUBSUB_SENSOR_GPS + bool "sensor_gps" + default n + + config ZENOH_PUBSUB_SENSOR_GYRO + bool "sensor_gyro" + default n + + config ZENOH_PUBSUB_SENSOR_GYRO_FFT + bool "sensor_gyro_fft" + default n + + config ZENOH_PUBSUB_SENSOR_GYRO_FIFO + bool "sensor_gyro_fifo" + default n + + config ZENOH_PUBSUB_SENSOR_HYGROMETER + bool "sensor_hygrometer" + default n + + config ZENOH_PUBSUB_SENSOR_MAG + bool "sensor_mag" + default n + + config ZENOH_PUBSUB_SENSOR_OPTICAL_FLOW + bool "sensor_optical_flow" + default n + + config ZENOH_PUBSUB_SENSOR_PREFLIGHT_MAG + bool "sensor_preflight_mag" + default n + + config ZENOH_PUBSUB_SENSOR_SELECTION + bool "sensor_selection" + default n + + config ZENOH_PUBSUB_SENSOR_UWB + bool "sensor_uwb" + default n + + config ZENOH_PUBSUB_SENSORS_STATUS + bool "sensors_status" + default n + + config ZENOH_PUBSUB_SENSORS_STATUS_IMU + bool "sensors_status_imu" + default n + + config ZENOH_PUBSUB_SYSTEM_POWER + bool "system_power" + default n + + config ZENOH_PUBSUB_TAKEOFF_STATUS + bool "takeoff_status" + default n + + config ZENOH_PUBSUB_TASK_STACK_INFO + bool "task_stack_info" + default n + + config ZENOH_PUBSUB_TECS_STATUS + bool "tecs_status" + default n + + config ZENOH_PUBSUB_TELEMETRY_STATUS + bool "telemetry_status" + default n + + config ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS + bool "tiltrotor_extra_controls" + default n + + config ZENOH_PUBSUB_TIMESYNC_STATUS + bool "timesync_status" + default n + + config ZENOH_PUBSUB_TRAJECTORY_BEZIER + bool "trajectory_bezier" + default n + + config ZENOH_PUBSUB_TRAJECTORY_SETPOINT + bool "trajectory_setpoint" + default n + + config ZENOH_PUBSUB_TRAJECTORY_WAYPOINT + bool "trajectory_waypoint" + default n + + config ZENOH_PUBSUB_TRANSPONDER_REPORT + bool "transponder_report" + default n + + config ZENOH_PUBSUB_TUNE_CONTROL + bool "tune_control" + default n + + config ZENOH_PUBSUB_UAVCAN_PARAMETER_REQUEST + bool "uavcan_parameter_request" + default n + + config ZENOH_PUBSUB_UAVCAN_PARAMETER_VALUE + bool "uavcan_parameter_value" + default n + + config ZENOH_PUBSUB_ULOG_STREAM + bool "ulog_stream" + default n + + config ZENOH_PUBSUB_ULOG_STREAM_ACK + bool "ulog_stream_ack" + default n + + config ZENOH_PUBSUB_VEHICLE_ACCELERATION + bool "vehicle_acceleration" + default n + + config ZENOH_PUBSUB_VEHICLE_AIR_DATA + bool "vehicle_air_data" + default n + + config ZENOH_PUBSUB_VEHICLE_ANGULAR_ACCELERATION_SETPOINT + bool "vehicle_angular_acceleration_setpoint" + default n + + config ZENOH_PUBSUB_VEHICLE_ANGULAR_VELOCITY + bool "vehicle_angular_velocity" + default n + + config ZENOH_PUBSUB_VEHICLE_ATTITUDE + bool "vehicle_attitude" + default n + + config ZENOH_PUBSUB_VEHICLE_ATTITUDE_SETPOINT + bool "vehicle_attitude_setpoint" + default n + + config ZENOH_PUBSUB_VEHICLE_COMMAND + bool "vehicle_command" + default n + + config ZENOH_PUBSUB_VEHICLE_COMMAND_ACK + bool "vehicle_command_ack" + default n + + config ZENOH_PUBSUB_VEHICLE_CONSTRAINTS + bool "vehicle_constraints" + default n + + config ZENOH_PUBSUB_VEHICLE_CONTROL_MODE + bool "vehicle_control_mode" + default n + + config ZENOH_PUBSUB_VEHICLE_GLOBAL_POSITION + bool "vehicle_global_position" + default n + + config ZENOH_PUBSUB_VEHICLE_IMU + bool "vehicle_imu" + default n + + config ZENOH_PUBSUB_VEHICLE_IMU_STATUS + bool "vehicle_imu_status" + default n + + config ZENOH_PUBSUB_VEHICLE_LAND_DETECTED + bool "vehicle_land_detected" + default n + + config ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION + bool "vehicle_local_position" + default n + + config ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION_SETPOINT + bool "vehicle_local_position_setpoint" + default n + + config ZENOH_PUBSUB_VEHICLE_MAGNETOMETER + bool "vehicle_magnetometer" + default n + + config ZENOH_PUBSUB_VEHICLE_ODOMETRY + bool "vehicle_odometry" + default n + + config ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW + bool "vehicle_optical_flow" + default n + + config ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW_VEL + bool "vehicle_optical_flow_vel" + default n + + config ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT + bool "vehicle_rates_setpoint" + default n + + config ZENOH_PUBSUB_VEHICLE_ROI + bool "vehicle_roi" + default n + + config ZENOH_PUBSUB_VEHICLE_STATUS + bool "vehicle_status" + default n + + config ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT + bool "vehicle_thrust_setpoint" + default n + + config ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT + bool "vehicle_torque_setpoint" + default n + + config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER + bool "vehicle_trajectory_bezier" + default n + + config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT + bool "vehicle_trajectory_waypoint" + default n + + config ZENOH_PUBSUB_VTOL_VEHICLE_STATUS + bool "vtol_vehicle_status" + default n + + config ZENOH_PUBSUB_WIND + bool "wind" + default n + + config ZENOH_PUBSUB_YAW_ESTIMATOR_STATUS + bool "yaw_estimator_status" + default n + + + +config ZENOH_PUBSUB_ALL_SELECTION + bool + default y if ZENOH_PUBSUB_ALL + select ZENOH_PUBSUB_ACTION_REQUEST + select ZENOH_PUBSUB_ACTUATOR_ARMED + select ZENOH_PUBSUB_ACTUATOR_CONTROLS_STATUS + select ZENOH_PUBSUB_ACTUATOR_MOTORS + select ZENOH_PUBSUB_ACTUATOR_OUTPUTS + select ZENOH_PUBSUB_ACTUATOR_SERVOS + select ZENOH_PUBSUB_ACTUATOR_SERVOS_TRIM + select ZENOH_PUBSUB_ACTUATOR_TEST + select ZENOH_PUBSUB_ADC_REPORT + select ZENOH_PUBSUB_AIRSPEED + select ZENOH_PUBSUB_AIRSPEED_VALIDATED + select ZENOH_PUBSUB_AIRSPEED_WIND + select ZENOH_PUBSUB_AUTOTUNE_ATTITUDE_CONTROL_STATUS + select ZENOH_PUBSUB_BATTERY_STATUS + select ZENOH_PUBSUB_BUTTON_EVENT + select ZENOH_PUBSUB_CAMERA_CAPTURE + select ZENOH_PUBSUB_CAMERA_STATUS + select ZENOH_PUBSUB_CAMERA_TRIGGER + select ZENOH_PUBSUB_CELLULAR_STATUS + select ZENOH_PUBSUB_COLLISION_CONSTRAINTS + select ZENOH_PUBSUB_COLLISION_REPORT + select ZENOH_PUBSUB_CONTROL_ALLOCATOR_STATUS + select ZENOH_PUBSUB_CPULOAD + select ZENOH_PUBSUB_DATAMAN_REQUEST + select ZENOH_PUBSUB_DATAMAN_RESPONSE + select ZENOH_PUBSUB_DEBUG_ARRAY + select ZENOH_PUBSUB_DEBUG_KEY_VALUE + select ZENOH_PUBSUB_DEBUG_VALUE + select ZENOH_PUBSUB_DEBUG_VECT + select ZENOH_PUBSUB_DIFFERENTIAL_PRESSURE + select ZENOH_PUBSUB_DISTANCE_SENSOR + select ZENOH_PUBSUB_EKF2_TIMESTAMPS + select ZENOH_PUBSUB_ESC_REPORT + select ZENOH_PUBSUB_ESC_STATUS + select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE1D + select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE2D + select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE3D + select ZENOH_PUBSUB_ESTIMATOR_BIAS + select ZENOH_PUBSUB_ESTIMATOR_BIAS3D + select ZENOH_PUBSUB_ESTIMATOR_EVENT_FLAGS + select ZENOH_PUBSUB_ESTIMATOR_GPS_STATUS + select ZENOH_PUBSUB_ESTIMATOR_INNOVATIONS + select ZENOH_PUBSUB_ESTIMATOR_SELECTOR_STATUS + select ZENOH_PUBSUB_ESTIMATOR_SENSOR_BIAS + select ZENOH_PUBSUB_ESTIMATOR_STATES + select ZENOH_PUBSUB_ESTIMATOR_STATUS + select ZENOH_PUBSUB_ESTIMATOR_STATUS_FLAGS + select ZENOH_PUBSUB_EVENT + select ZENOH_PUBSUB_FAILSAFE_FLAGS + select ZENOH_PUBSUB_FAILURE_DETECTOR_STATUS + select ZENOH_PUBSUB_FOLLOW_TARGET + select ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR + select ZENOH_PUBSUB_FOLLOW_TARGET_STATUS + select ZENOH_PUBSUB_GENERATOR_STATUS + select ZENOH_PUBSUB_GEOFENCE_RESULT + select ZENOH_PUBSUB_GIMBAL_CONTROLS + select ZENOH_PUBSUB_GIMBAL_DEVICE_ATTITUDE_STATUS + select ZENOH_PUBSUB_GIMBAL_DEVICE_INFORMATION + select ZENOH_PUBSUB_GIMBAL_DEVICE_SET_ATTITUDE + select ZENOH_PUBSUB_GIMBAL_MANAGER_INFORMATION + select ZENOH_PUBSUB_GIMBAL_MANAGER_SET_ATTITUDE + select ZENOH_PUBSUB_GIMBAL_MANAGER_SET_MANUAL_CONTROL + select ZENOH_PUBSUB_GIMBAL_MANAGER_STATUS + select ZENOH_PUBSUB_GPIO_CONFIG + select ZENOH_PUBSUB_GPIO_IN + select ZENOH_PUBSUB_GPIO_OUT + select ZENOH_PUBSUB_GPIO_REQUEST + select ZENOH_PUBSUB_GPS_DUMP + select ZENOH_PUBSUB_GPS_INJECT_DATA + select ZENOH_PUBSUB_GRIPPER + select ZENOH_PUBSUB_HEALTH_REPORT + select ZENOH_PUBSUB_HEATER_STATUS + select ZENOH_PUBSUB_HOME_POSITION + select ZENOH_PUBSUB_HOVER_THRUST_ESTIMATE + select ZENOH_PUBSUB_INPUT_RC + select ZENOH_PUBSUB_INTERNAL_COMBUSTION_ENGINE_STATUS + select ZENOH_PUBSUB_IRIDIUMSBD_STATUS + select ZENOH_PUBSUB_IRLOCK_REPORT + select ZENOH_PUBSUB_LANDING_GEAR + select ZENOH_PUBSUB_LANDING_GEAR_WHEEL + select ZENOH_PUBSUB_LANDING_TARGET_INNOVATIONS + select ZENOH_PUBSUB_LANDING_TARGET_POSE + select ZENOH_PUBSUB_LAUNCH_DETECTION_STATUS + select ZENOH_PUBSUB_LED_CONTROL + select ZENOH_PUBSUB_LOG_MESSAGE + select ZENOH_PUBSUB_LOGGER_STATUS + select ZENOH_PUBSUB_MAG_WORKER_DATA + select ZENOH_PUBSUB_MAGNETOMETER_BIAS_ESTIMATE + select ZENOH_PUBSUB_MANUAL_CONTROL_SETPOINT + select ZENOH_PUBSUB_MANUAL_CONTROL_SWITCHES + select ZENOH_PUBSUB_MAVLINK_LOG + select ZENOH_PUBSUB_MAVLINK_TUNNEL + select ZENOH_PUBSUB_MISSION + select ZENOH_PUBSUB_MISSION_RESULT + select ZENOH_PUBSUB_MODE_COMPLETED + select ZENOH_PUBSUB_MOUNT_ORIENTATION + select ZENOH_PUBSUB_NAVIGATOR_MISSION_ITEM + select ZENOH_PUBSUB_NORMALIZED_UNSIGNED_SETPOINT + select ZENOH_PUBSUB_NPFG_STATUS + select ZENOH_PUBSUB_OBSTACLE_DISTANCE + select ZENOH_PUBSUB_OFFBOARD_CONTROL_MODE + select ZENOH_PUBSUB_ONBOARD_COMPUTER_STATUS + select ZENOH_PUBSUB_ORB_TEST + select ZENOH_PUBSUB_ORB_TEST_LARGE + select ZENOH_PUBSUB_ORB_TEST_MEDIUM + select ZENOH_PUBSUB_ORBIT_STATUS + select ZENOH_PUBSUB_PARAMETER_UPDATE + select ZENOH_PUBSUB_PING + select ZENOH_PUBSUB_POSITION_CONTROLLER_LANDING_STATUS + select ZENOH_PUBSUB_POSITION_CONTROLLER_STATUS + select ZENOH_PUBSUB_POSITION_SETPOINT + select ZENOH_PUBSUB_POSITION_SETPOINT_TRIPLET + select ZENOH_PUBSUB_POWER_BUTTON_STATE + select ZENOH_PUBSUB_POWER_MONITOR + select ZENOH_PUBSUB_PPS_CAPTURE + select ZENOH_PUBSUB_PWM_INPUT + select ZENOH_PUBSUB_PX4IO_STATUS + select ZENOH_PUBSUB_QSHELL_REQ + select ZENOH_PUBSUB_QSHELL_RETVAL + select ZENOH_PUBSUB_RADIO_STATUS + select ZENOH_PUBSUB_RATE_CTRL_STATUS + select ZENOH_PUBSUB_RC_CHANNELS + select ZENOH_PUBSUB_RC_PARAMETER_MAP + select ZENOH_PUBSUB_RPM + select ZENOH_PUBSUB_RTL_TIME_ESTIMATE + select ZENOH_PUBSUB_SATELLITE_INFO + select ZENOH_PUBSUB_SENSOR_ACCEL + select ZENOH_PUBSUB_SENSOR_ACCEL_FIFO + select ZENOH_PUBSUB_SENSOR_BARO + select ZENOH_PUBSUB_SENSOR_COMBINED + select ZENOH_PUBSUB_SENSOR_CORRECTION + select ZENOH_PUBSUB_SENSOR_GNSS_RELATIVE + select ZENOH_PUBSUB_SENSOR_GPS + select ZENOH_PUBSUB_SENSOR_GYRO + select ZENOH_PUBSUB_SENSOR_GYRO_FFT + select ZENOH_PUBSUB_SENSOR_GYRO_FIFO + select ZENOH_PUBSUB_SENSOR_HYGROMETER + select ZENOH_PUBSUB_SENSOR_MAG + select ZENOH_PUBSUB_SENSOR_OPTICAL_FLOW + select ZENOH_PUBSUB_SENSOR_PREFLIGHT_MAG + select ZENOH_PUBSUB_SENSOR_SELECTION + select ZENOH_PUBSUB_SENSOR_UWB + select ZENOH_PUBSUB_SENSORS_STATUS + select ZENOH_PUBSUB_SENSORS_STATUS_IMU + select ZENOH_PUBSUB_SYSTEM_POWER + select ZENOH_PUBSUB_TAKEOFF_STATUS + select ZENOH_PUBSUB_TASK_STACK_INFO + select ZENOH_PUBSUB_TECS_STATUS + select ZENOH_PUBSUB_TELEMETRY_STATUS + select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS + select ZENOH_PUBSUB_TIMESYNC_STATUS + select ZENOH_PUBSUB_TRAJECTORY_BEZIER + select ZENOH_PUBSUB_TRAJECTORY_SETPOINT + select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT + select ZENOH_PUBSUB_TRANSPONDER_REPORT + select ZENOH_PUBSUB_TUNE_CONTROL + select ZENOH_PUBSUB_UAVCAN_PARAMETER_REQUEST + select ZENOH_PUBSUB_UAVCAN_PARAMETER_VALUE + select ZENOH_PUBSUB_ULOG_STREAM + select ZENOH_PUBSUB_ULOG_STREAM_ACK + select ZENOH_PUBSUB_VEHICLE_ACCELERATION + select ZENOH_PUBSUB_VEHICLE_AIR_DATA + select ZENOH_PUBSUB_VEHICLE_ANGULAR_ACCELERATION_SETPOINT + select ZENOH_PUBSUB_VEHICLE_ANGULAR_VELOCITY + select ZENOH_PUBSUB_VEHICLE_ATTITUDE + select ZENOH_PUBSUB_VEHICLE_ATTITUDE_SETPOINT + select ZENOH_PUBSUB_VEHICLE_COMMAND + select ZENOH_PUBSUB_VEHICLE_COMMAND_ACK + select ZENOH_PUBSUB_VEHICLE_CONSTRAINTS + select ZENOH_PUBSUB_VEHICLE_CONTROL_MODE + select ZENOH_PUBSUB_VEHICLE_GLOBAL_POSITION + select ZENOH_PUBSUB_VEHICLE_IMU + select ZENOH_PUBSUB_VEHICLE_IMU_STATUS + select ZENOH_PUBSUB_VEHICLE_LAND_DETECTED + select ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION + select ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION_SETPOINT + select ZENOH_PUBSUB_VEHICLE_MAGNETOMETER + select ZENOH_PUBSUB_VEHICLE_ODOMETRY + select ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW + select ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW_VEL + select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT + select ZENOH_PUBSUB_VEHICLE_ROI + select ZENOH_PUBSUB_VEHICLE_STATUS + select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT + select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT + select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER + select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT + select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS + select ZENOH_PUBSUB_WIND + select ZENOH_PUBSUB_YAW_ESTIMATOR_STATUS + +endmenu diff --git a/src/modules/zenoh/module.yaml b/src/modules/zenoh/module.yaml new file mode 100644 index 0000000000..25e658f9f3 --- /dev/null +++ b/src/modules/zenoh/module.yaml @@ -0,0 +1,14 @@ +module_name: Zenoh bridge + +parameters: + - group: Zenoh + definitions: + + ZENOH_ENABLE: + description: + short: Zenoh Enable + long: Zenoh + category: System + type: int32 + reboot_required: true + default: 0 diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp new file mode 100644 index 0000000000..b541bfd1dd --- /dev/null +++ b/src/modules/zenoh/publishers/uorb_publisher.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 uorb_publisher.hpp + * + * Defines generic, templatized uORB over Zenoh / ROS2 + * + * @author Peter van der Perk + */ + +#pragma once + +#include "zenoh_publisher.hpp" +#include +#include + +#define CDR_SAFETY_MARGIN 12 + +class uORB_Zenoh_Publisher : public Zenoh_Publisher +{ +public: + uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) : + Zenoh_Publisher(true), + _uorb_meta{meta}, + _cdr_ops(ops) + { + _uorb_sub = orb_subscribe(meta); + }; + + ~uORB_Zenoh_Publisher() override = default; + + // Update the uORB Subscription and broadcast a Zenoh ROS2 message + virtual int8_t update() override + { + uint8_t data[_uorb_meta->o_size]; + orb_copy(_uorb_meta, _uorb_sub, data); + + uint8_t buf[_uorb_meta->o_size + 4 + CDR_SAFETY_MARGIN]; + memcpy(buf, ros2_header, sizeof(ros2_header)); + + dds_ostream_t os; + os.m_buffer = buf; + os.m_index = (uint32_t)sizeof(ros2_header); + os.m_size = (uint32_t)sizeof(ros2_header) + _uorb_meta->o_size + CDR_SAFETY_MARGIN; + os.m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2; + + if (dds_stream_write(&os, + &dds_allocator, + (const char *)&data, + _cdr_ops)) { + return publish((const uint8_t *)buf, os.m_size); + + } else { + return _Z_ERR_MESSAGE_SERIALIZATION_FAILED; + } + }; + + void setPollFD(px4_pollfd_struct_t *pfd) + { + pfd->fd = _uorb_sub; + pfd->events = POLLIN; + } + + void print() + { + printf("uORB %s -> ", _uorb_meta->o_name); + Zenoh_Publisher::print(); + } + +private: + const orb_metadata *_uorb_meta; + int _uorb_sub; + const uint32_t *_cdr_ops; +}; diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp new file mode 100644 index 0000000000..1e312ffea5 --- /dev/null +++ b/src/modules/zenoh/publishers/zenoh_publisher.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 zenoh_publisher.cpp + * + * Zenoh publisher + * + * @author Peter van der Perk + */ + +#include "zenoh_publisher.hpp" + + +Zenoh_Publisher::Zenoh_Publisher(bool rostopic) +{ + this->_rostopic = rostopic; + this->_topic[0] = 0x0; +} + +Zenoh_Publisher::~Zenoh_Publisher() +{ + undeclare_publisher(); +} + +int Zenoh_Publisher::undeclare_publisher() +{ + z_undeclare_publisher(z_publisher_move(&_pub)); + return 0; +} + +int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) +{ + if (_rostopic) { + strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + + if (keyexpr[0] == '/') { + strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + + } else { + strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); + } + + } else { + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); + } + + _pub = z_declare_publisher(s, z_keyexpr(this->_topic), NULL); + + if (!z_publisher_check(&_pub)) { + printf("Unable to declare publisher for key expression!\n"); + return -1; + } + + return 0; +} + +int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size) +{ + z_publisher_put_options_t options = z_publisher_put_options_default(); + options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_CUSTOM, NULL); + return z_publisher_put(z_publisher_loan(&_pub), buf, size, &options); +} + +void Zenoh_Publisher::print() +{ + printf("Topic: %s\n", this->_topic); +} diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp new file mode 100644 index 0000000000..1d88a453e2 --- /dev/null +++ b/src/modules/zenoh/publishers/zenoh_publisher.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 zenoh_publisher.hpp + * + * Defines basic functionality of Zenoh publisher class + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include + +#include +#include +#include + +class Zenoh_Publisher : public ListNode +{ +public: + Zenoh_Publisher(bool rostopic = true); + virtual ~Zenoh_Publisher(); + + virtual int declare_publisher(z_session_t s, const char *keyexpr); + + virtual int undeclare_publisher(); + + virtual int8_t update() = 0; + + virtual void print(); + +protected: + int8_t publish(const uint8_t *, int size); + + z_owned_publisher_t _pub; + + char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. + + // Indicates ROS2 Topic namespace + bool _rostopic; + const char *_rt_prefix = "rt/"; + const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars +}; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp new file mode 100644 index 0000000000..bd16b64398 --- /dev/null +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 uorb_subscriber.hpp + * + * Defines generic, templatized uORB over Zenoh / ROS2 + * + * @author Peter van der Perk + */ + +#pragma once + +#include "zenoh_subscriber.hpp" +#include +#include +#include + +class uORB_Zenoh_Subscriber : public Zenoh_Subscriber +{ +public: + uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) : + Zenoh_Subscriber(true), + _uorb_meta{meta}, + _cdr_ops(ops) + { + int instance = 0; + _uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize + }; + + ~uORB_Zenoh_Subscriber() override = default; + + // Update the uORB Subscription and broadcast a Zenoh ROS2 message + void data_handler(const z_sample_t *sample) + { + char data[_uorb_meta->o_size]; + + dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast(sample->payload.len), + .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2 + }; + dds_stream_read(&is, data, &dds_allocator, _cdr_ops); + + // As long as we don't have timesynchronization between Zenoh nodes + // we've to manually set the timestamp + fix_timestamp(data); + + // ORB_ID::input_rc needs additional timestamp fixup + if (static_cast(_uorb_meta->o_id) == ORB_ID::input_rc) { + memcpy(&data[8], data, sizeof(hrt_abstime)); + } + + orb_publish(_uorb_meta, _uorb_pub_handle, &data); + }; + + void fix_timestamp(char *data) + { + hrt_abstime now = hrt_absolute_time(); + memcpy(data, &now, sizeof(hrt_abstime)); + } + + void print() + { + Zenoh_Subscriber::print("uORB", _uorb_meta->o_name); + } + +protected: + // Default payload-size function -- can specialize in derived class + size_t get_payload_size() + { + return _uorb_meta->o_size; + } + +private: + const orb_metadata *_uorb_meta; + orb_advert_t _uorb_pub_handle; + const uint32_t *_cdr_ops; +}; diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp new file mode 100644 index 0000000000..aab66ee5bd --- /dev/null +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 zenoh_subscriber.cpp + * + * Zenoh subscriber + * + * @author Peter van der Perk + */ + +#include "zenoh_subscriber.hpp" + +static void data_handler_cb(const z_sample_t *sample, void *arg) +{ + static_cast(arg)->data_handler(sample); +} + +void Zenoh_Subscriber::data_handler(const z_sample_t *sample) +{ + z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); + printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len); + z_str_drop(z_str_move(&keystr)); +} + + +Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic) +{ + this->_rostopic = rostopic; + this->_topic[0] = 0x0; +} + +Zenoh_Subscriber::~Zenoh_Subscriber() +{ + undeclare_subscriber(); +} + +int Zenoh_Subscriber::undeclare_subscriber() +{ + z_undeclare_subscriber(z_subscriber_move(&_sub)); + return 0; +} + +int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr) +{ + z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this); + + if (_rostopic) { + strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + + if (keyexpr[0] == '/') { + strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + + } else { + strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); + } + + } else { + strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic)); + } + + _sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL); + + + if (!z_subscriber_check(&_sub)) { + printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr); + return -1; + } + + return 0; +} + +void Zenoh_Subscriber::print() +{ + printf("Topic: %s\n", this->_topic); +} + +void Zenoh_Subscriber::print(const char *type_string, const char *topic_string) +{ + printf("Topic: %s -> %s %s \n", this->_topic, type_string, topic_string); +} diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp new file mode 100644 index 0000000000..e3f1200e92 --- /dev/null +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 zenoh_subscriber.hpp + * + * Defines basic functionality of Zenoh subscriber class + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include + +#include +#include +#include + +// CycloneDDS CDR Deserializer +#include +#include + +class Zenoh_Subscriber : public ListNode +{ +public: + Zenoh_Subscriber(bool rostopic = true); + virtual ~Zenoh_Subscriber(); + + virtual int declare_subscriber(z_session_t s, const char *keyexpr); + + virtual int undeclare_subscriber(); + + virtual void data_handler(const z_sample_t *sample); + + virtual void print(); + +protected: + virtual void print(const char *type_string, const char *topic_string); + + z_owned_subscriber_t _sub; + char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. + + + // Indicates ROS2 Topic namespace + bool _rostopic; + const char *_rt_prefix = "rt/"; + const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars +}; diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico new file mode 160000 index 0000000000..22bbfc2150 --- /dev/null +++ b/src/modules/zenoh/zenoh-pico @@ -0,0 +1 @@ +Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp new file mode 100644 index 0000000000..0caf2c5eaa --- /dev/null +++ b/src/modules/zenoh/zenoh.cpp @@ -0,0 +1,290 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "zenoh.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// CycloneDDS CDR Deserializer +#include + +// Auto-generated header to all uORB <-> CDR conversions +#include + + +#define Z_PUBLISH +#define Z_SUBSCRIBE + +extern "C" __EXPORT int zenoh_main(int argc, char *argv[]); + +ZENOH::ZENOH(): + ModuleParams(nullptr) +{ + +} + +ZENOH::~ZENOH() +{ + +} + +void ZENOH::run() +{ + char mode[NET_MODE_SIZE]; + char locator[NET_LOCATOR_SIZE]; + int8_t ret; + int i; + + Zenoh_Config z_config; + + z_config.getNetworkConfig(mode, locator); + + z_owned_config_t config = z_config_default(); + zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + + if (locator[0] != 0) { + zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + + } else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) { + zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(Z_CONFIG_MULTICAST_LOCATOR_DEFAULT)); + } + + PX4_INFO("Opening session..."); + z_owned_session_t s = z_open(z_config_move(&config)); + + if (!z_session_check(&s)) { + PX4_ERR("Unable to open session!"); + return; + } + + // Start read and lease tasks for zenoh-pico + if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + PX4_ERR("Unable to start read and lease tasks"); + return; + } + +#ifdef Z_SUBSCRIBE + _sub_count = z_config.getSubCount(); + _zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count); + { + char topic[TOPIC_INFO_SIZE]; + char type[TOPIC_INFO_SIZE]; + + for (i = 0; i < _sub_count; i++) { + z_config.getSubscriberMapping(topic, type); + _zenoh_subscribers[i] = genSubscriber(type); + + if (_zenoh_subscribers[i] != 0) { + _zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic); + } + + + } + + if (z_config.getSubscriberMapping(topic, type) < 0) { + PX4_WARN("Subscriber mapping parsing error"); + } + } +#endif + +#ifdef Z_PUBLISH + + _pub_count = z_config.getPubCount(); + _zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *)); + px4_pollfd_struct_t pfds[_pub_count]; + + { + char topic[TOPIC_INFO_SIZE]; + char type[TOPIC_INFO_SIZE]; + + for (i = 0; i < _pub_count; i++) { + z_config.getPublisherMapping(topic, type); + _zenoh_publishers[i] = genPublisher(type); + + if (_zenoh_publishers[i] != 0) { + _zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic); + _zenoh_publishers[i]->setPollFD(&pfds[i]); + } + } + + if (z_config.getSubscriberMapping(topic, type) < 0) { + PX4_WARN("Publisher mapping parsing error"); + } + } + + if (_pub_count == 0) { + // Nothing to publish but we don't want to stop this thread + while (!should_exit()) { + sleep(2); + } + } + + while (!should_exit()) { + int pret = px4_poll(pfds, _pub_count, 100); + + if (pret == 0) { + //PX4_INFO("Zenoh poll timeout\n"); + + } else { + for (i = 0; i < _pub_count; i++) { + if (pfds[i].revents & POLLIN) { + ret = _zenoh_publishers[i]->update(); + + if (ret < 0) { + PX4_WARN("Publisher error %i", ret); + + } + } + } + } + } + +#endif + + // Exiting cleaning up publisher and subscribers + for (i = 0; i < _sub_count; i++) { + delete _zenoh_subscribers[i]; + } + + free(_zenoh_subscribers); + + for (i = 0; i < _pub_count; i++) { + delete _zenoh_publishers[i]; + } + + free(_zenoh_publishers); + + // Stop read and lease tasks for zenoh-pico + zp_stop_read_task(z_session_loan(&s)); + zp_stop_lease_task(z_session_loan(&s)); + + z_close(z_session_move(&s)); + exit_and_cleanup(); +} + +int ZENOH::custom_command(int argc, char *argv[]) +{ + if (argc > 0 && strcmp("config", argv[0]) == 0) { + Zenoh_Config z_config; + + if (z_config.cli(argc, argv) == 0) { + return 0; + } + } + + return print_usage("Unrecognized command."); +} + +int ZENOH::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_USAGE_NAME("zenoh", "driver"); + PRINT_MODULE_DESCRIPTION(R"DESC_STR( +### Description + +Zenoh demo bridge + )DESC_STR"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("stop"); + PRINT_MODULE_USAGE_COMMAND("status"); + PRINT_MODULE_USAGE_COMMAND("config"); + PX4_INFO_RAW(" addpublisher Publish uORB topic to Zenoh\n"); + PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); + PX4_INFO_RAW(" net Zenoh network mode\n"); + PX4_INFO_RAW(" values: client|peer \n"); + PX4_INFO_RAW(" client: locator address for router\n"); + PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.225:7447#iface=eth0\n"); + return 0; +} + +int ZENOH::print_status() +{ + PX4_INFO("running"); + + PX4_INFO("Publishers"); + + for (int i = 0; i < _pub_count; i++) { + _zenoh_publishers[i]->print(); + } + + PX4_INFO("Subscribers"); + + for (int i = 0; i < _sub_count; i++) { + _zenoh_subscribers[i]->print(); + } + + return 0; +} + +int ZENOH::task_spawn(int argc, char *argv[]) +{ + + int task_id = px4_task_spawn_cmd( + "zenoh", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + &run_trampoline, + argv + ); + + if (task_id < 0) { + return -errno; + + } else { + _task_id = task_id; + return 0; + } +} + +ZENOH *ZENOH::instantiate(int argc, char *argv[]) +{ + return new ZENOH(); +} + +int zenoh_main(int argc, char *argv[]) +{ + return ZENOH::main(argc, argv); +} diff --git a/src/modules/zenoh/zenoh.h b/src/modules/zenoh/zenoh.h new file mode 100644 index 0000000000..a7cb0465a9 --- /dev/null +++ b/src/modules/zenoh/zenoh.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#ifndef ZENOH_MODULE_H +#define ZENOH_MODULE_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "zenoh_config.hpp" +#include "publishers/uorb_publisher.hpp" +#include "subscribers/uorb_subscriber.hpp" + +class ZENOH : public ModuleBase, public ModuleParams +{ +public: + ZENOH(); + + ~ZENOH(); + + + /** + * @see ModuleBase::custom_command + */ + static int custom_command(int argc, char *argv[]); + + /** + * @see ModuleBase::print_usage + */ + static int print_usage(const char *reason = nullptr); + + /** + * @see ModuleBase::print_usage + */ + int print_status(); + + /** + * @see ModuleBase::task_spawn + */ + static int task_spawn(int argc, char *argv[]); + + static ZENOH *instantiate(int argc, char *argv[]); + + void run() override; + +private: + + Zenoh_Config _config; + + int _pub_count; + uORB_Zenoh_Publisher **_zenoh_publishers; + int _sub_count; + Zenoh_Subscriber **_zenoh_subscribers; + +}; + +#endif //ZENOH_MODULE_H diff --git a/src/modules/zenoh/zenoh_config.cpp b/src/modules/zenoh/zenoh_config.cpp new file mode 100644 index 0000000000..8069956585 --- /dev/null +++ b/src/modules/zenoh/zenoh_config.cpp @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 zenoh_config.cpp + * + * Zenoh configuration backend + * + * @author Peter van der Perk + */ + +#include "zenoh_config.hpp" + +#include +#include +#include +#include +#include + +#include + + +const char *default_net_config = Z_CONFIG_MODE_DEFAULT; +const char *default_pub_config = ""; +const char *default_sub_config = ""; //TODO maybe use YAML + + +Zenoh_Config::Zenoh_Config() +{ + bool correct_config = true; + DIR *dir = opendir(ZENOH_SD_ROOT_PATH); + fp_mapping = NULL; + + if (dir) { + /* Directory exists. */ + closedir(dir); + + if (access(ZENOH_NET_CONFIG_PATH, F_OK) != 0) { + correct_config = false; + + } else if (access(ZENOH_PUB_CONFIG_PATH, F_OK) != 0) { + correct_config = false; + + } else if (access(ZENOH_SUB_CONFIG_PATH, F_OK) != 0) { + correct_config = false; + } + + } else { + /* opendir() failed */ + correct_config = false; + } + + if (!correct_config) { + generate_clean_config(); + } +} + +Zenoh_Config::~Zenoh_Config() +{ + if (fp_mapping != NULL) { + fclose(fp_mapping); + } +} + +int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename) +{ + { + char f_topic[TOPIC_INFO_SIZE]; + char f_type[TOPIC_INFO_SIZE]; + + while (getPubSubMapping(f_topic, f_type, filename) > 0) { + if (strcmp(topic, f_topic) == 0 + || strcmp(datatype, f_type) == 0) { + printf("Already mapped to uORB %s -> %s\n", f_type, f_topic); + return 0; + } + + } + } + + for (size_t i = 0; i < orb_topics_count(); i++) { + const struct orb_metadata *meta = get_orb_meta((ORB_ID)i); + + if (meta != NULL && + strcmp(meta->o_name, datatype) == 0) { + FILE *fp = fopen(filename, "a"); + + if (fp) { + fprintf(fp, "%s;%s\n", topic, datatype); + + } else { + return -1; + } + + fclose(fp); + return 1; + } + } + + printf("%s not found\n", datatype); + return 0; +} + + +int Zenoh_Config::SetNetworkConfig(char *mode, char *locator) +{ + + FILE *fp = fopen(ZENOH_NET_CONFIG_PATH, "w"); + + if (fp) { + if (locator == 0) { + fprintf(fp, "%s\n", mode); + + } else { + fprintf(fp, "%s;%s\n", mode, locator); + } + + } else { + return -1; + } + + fclose(fp); + return 0; +} + +int Zenoh_Config::cli(int argc, char *argv[]) +{ + if (argc == 1) { + dump_config(); + + } else if (argc == 3) { + if (strcmp(argv[1], "net") == 0) { + SetNetworkConfig(argv[2], 0); + } + + } else if (argc == 4) { + if (strcmp(argv[1], "addpublisher") == 0) { + if (AddPubSub(argv[2], argv[3], ZENOH_PUB_CONFIG_PATH) > 0) { + printf("Added %s %s to publishers\n", argv[2], argv[3]); + + } else { + printf("Could not add uORB %s -> %s to publishers\n", argv[3], argv[2]); + } + + } else if (strcmp(argv[1], "addsubscriber") == 0) { + if (AddPubSub(argv[2], argv[3], ZENOH_SUB_CONFIG_PATH) > 0) { + printf("Added %s -> uORB %s to subscribers\n", argv[2], argv[3]); + + } else { + printf("Could not add %s -> uORB %s to subscribers\n", argv[2], argv[3]); + } + + } else if (strcmp(argv[1], "net") == 0) { + SetNetworkConfig(argv[2], argv[3]); + } + } + + //TODO make CLI to modify configuration now you would have to manually modify the files + return 0; +} + +const char *Zenoh_Config::get_csv_field(char *line, int num) +{ + const char *tok; + + for ( + tok = strtok(line, ";"); + tok && *tok; + tok = strtok(NULL, ";\n")) { + if (!--num) { + return tok; + } + } + + return NULL; +} + +void Zenoh_Config::getNetworkConfig(char *mode, char *locator) +{ + FILE *fp; + char buffer[NET_CONFIG_LINE_SIZE]; + + fp = fopen(ZENOH_NET_CONFIG_PATH, "r"); + + // If file opened successfully, then read the file + if (fp) { + fgets(buffer, NET_CONFIG_LINE_SIZE, fp); + const char *config_locator = get_csv_field(buffer, 2); + char *config_mode = (char *)get_csv_field(buffer, 1); + + if (config_mode) { + config_mode[strcspn(config_mode, "\n")] = 0; + strncpy(mode, config_mode, NET_MODE_SIZE); + + } else { + mode[0] = 0; + } + + if (config_locator) { + strncpy(locator, config_locator, NET_LOCATOR_SIZE); + + } else { + locator[0] = 0; + } + + } else { + printf("Failed to open the file\n"); + } + + //Close the file + fclose(fp); +} + +int Zenoh_Config::getLineCount(const char *filename) +{ + int lines = 0; + int ch; + + // Open file in write mode + FILE *fp = fopen(filename, "r"); + + while ((ch = fgetc(fp)) != EOF) { + if (ch == '\n') { + lines++; + } + } + + //Close the file + fclose(fp); + + return lines; +} + +// Very rudamentary here but we've to wait for a more advanced param system +int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename) +{ + char buffer[MAX_LINE_SIZE]; + + if (fp_mapping == NULL) { + fp_mapping = fopen(filename, "r"); + } + + if (fp_mapping) { + while (fgets(buffer, MAX_LINE_SIZE, fp_mapping) != NULL) { + if (buffer[0] != '\n') { + const char *config_type = get_csv_field(buffer, 2); + const char *config_topic = get_csv_field(buffer, 1); + + strncpy(type, config_type, TOPIC_INFO_SIZE); + strncpy(topic, config_topic, TOPIC_INFO_SIZE); + return 1; + } + + } + + } else { + printf("Failed to open the file\n"); + return -1; + } + + //Close the file + fclose(fp_mapping); + fp_mapping = NULL; + return 0; + +} + + +void Zenoh_Config::dump_config() +{ + printf("Network config:\n"); + { + char mode[NET_MODE_SIZE]; + char locator[NET_LOCATOR_SIZE]; + getNetworkConfig(mode, locator); + + printf("Mode: %s\n", mode); + + if (locator[0] == 0) { + printf("Locator: scout\n"); + + } else { + printf("Locator: %s\n", locator); + } + + printf("\n"); + } + + { + char topic[TOPIC_INFO_SIZE]; + char type[TOPIC_INFO_SIZE]; + + printf("Publisher config:\n"); + + while (getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH) > 0) { + printf("Topic: %s\n", topic); + printf("Type: %s\n", type); + } + + printf("\nSubscriber config:\n"); + + while (getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH) > 0) { + printf("Topic: %s\n", topic); + printf("Type: %s\n", type); + } + } +} + + +void Zenoh_Config::generate_clean_config() +{ + printf("Generate clean\n"); + FILE *fp; + + DIR *dir = opendir(ZENOH_SD_ROOT_PATH); + + if (dir) { + printf("Zenoh directory exists\n"); + + } else { + /* Create zenoh dir. */ + if (mkdir(ZENOH_SD_ROOT_PATH, 0700) < 0) { + printf("Failed to create Zenoh directory\n"); + return; + } + + } + + if (access(ZENOH_NET_CONFIG_PATH, F_OK) != 0) { + // Open file in write mode + fp = fopen(ZENOH_NET_CONFIG_PATH, "w"); + + // If file opened successfully, then write the string to file + if (fp) { + fputs(default_net_config, fp); + + } else { + printf("Failed to open the file\n"); + return; + } + + //Close the file + fclose(fp); + } + + if (access(ZENOH_PUB_CONFIG_PATH, F_OK) != 0) { + // Open file in write mode + fp = fopen(ZENOH_PUB_CONFIG_PATH, "w"); + + // If file opened successfully, then write the string to file + if (fp) { + fputs(default_pub_config, fp); + + } else { + printf("Failed to open the file\n"); + return; + } + + //Close the file + fclose(fp); + } + + if (access(ZENOH_SUB_CONFIG_PATH, F_OK) != 0) { + // Open file in write mode + fp = fopen(ZENOH_SUB_CONFIG_PATH, "w"); + + // If file opened successfully, then write the string to file + if (fp) { + fputs(default_sub_config, fp); + + } else { + printf("Failed to open the file\n"); + return; + } + + //Close the file + fclose(fp); + } +} diff --git a/src/modules/zenoh/zenoh_config.hpp b/src/modules/zenoh/zenoh_config.hpp new file mode 100644 index 0000000000..72ae22d391 --- /dev/null +++ b/src/modules/zenoh/zenoh_config.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 zenoh_config.hpp + * + * Defines Zenoh configuration backend + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#define ZENOH_MAX_PATH_LENGTH (128 + 40) +#define ZENOH_SD_ROOT_PATH CONFIG_ZENOH_CONFIG_PATH +#define ZENOH_PUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/pub.csv" +#define ZENOH_SUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/sub.csv" +#define ZENOH_NET_CONFIG_PATH ZENOH_SD_ROOT_PATH"/net.txt" + +#define NET_MODE_SIZE sizeof("client") +#define NET_LOCATOR_SIZE 64 +#define NET_CONFIG_LINE_SIZE NET_MODE_SIZE + NET_LOCATOR_SIZE +#define TOPIC_INFO_SIZE 64 +#define MAX_LINE_SIZE 2*TOPIC_INFO_SIZE + +class Zenoh_Config +{ +public: + Zenoh_Config(); + ~Zenoh_Config(); + + int cli(int argc, char *argv[]); + + void getNetworkConfig(char *mode, char *locator); + int getPubCount() + { + return getLineCount(ZENOH_PUB_CONFIG_PATH); + } + int getSubCount() + { + return getLineCount(ZENOH_SUB_CONFIG_PATH); + } + int getPublisherMapping(char *topic, char *type) + { + return getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH); + } + int getSubscriberMapping(char *topic, char *type) + { + return getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH); + } + + +private: + int getPubSubMapping(char *topic, char *type, const char *filename); + int AddPubSub(char *topic, char *datatype, const char *filename); + int SetNetworkConfig(char *mode, char *locator); + int getLineCount(const char *filename); + + const char *get_csv_field(char *line, int num); + void generate_clean_config(); + void dump_config(); + + FILE *fp_mapping; + + +}; diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 90b27f943a..3b4be475e3 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -17,6 +17,7 @@ if(MAVSDK_FOUND) test_main.cpp autopilot_tester.cpp autopilot_tester_failure.cpp + autopilot_tester_rtl.cpp # follow-me needs a MAVSDK update: # https://github.com/mavlink/MAVSDK/pull/1770 # autopilot_tester_follow_me.cpp diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index fb7cfdd9f7..5811d796d4 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -268,7 +268,7 @@ void AutopilotTester::execute_mission_and_lose_gps() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -280,7 +280,7 @@ void AutopilotTester::execute_mission_and_lose_mag() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -296,7 +296,7 @@ void AutopilotTester::execute_mission_and_lose_baro() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -312,7 +312,7 @@ void AutopilotTester::execute_mission_and_get_baro_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success); @@ -328,7 +328,7 @@ void AutopilotTester::execute_mission_and_get_mag_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success); @@ -560,6 +560,49 @@ void AutopilotTester::stop_checking_altitude() _telemetry->subscribe_position(nullptr); } +void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse) +{ + auto mission_raw = _mission_raw->download_mission(); + CHECK(mission_raw.first == MissionRaw::Result::Success); + + auto mission_items = mission_raw.second; + auto ct = get_coordinate_transformation(); + + _telemetry->set_rate_position_velocity_ned(5); + _telemetry->subscribe_position_velocity_ned([ct, mission_items, corridor_radius_m, reverse, + this](Telemetry::PositionVelocityNed position_velocity_ned) { + auto progress = _mission_raw->mission_progress(); + + + std::function(std::vector, unsigned, mavsdk::geometry::CoordinateTransformation)> + get_waypoint_for_sequence = [](std::vector mission_items, int sequence, auto ct) { + for (auto waypoint : mission_items) { + + if (waypoint.seq == (uint32_t)sequence) { + return get_local_mission_item_from_raw_item(waypoint, ct); + } + } + + return std::array({0.0f, 0.0f, 0.0f}); + }; + + if (progress.current > 0 && progress.current < progress.total) { + // Get shortest distance of current position to 3D line between previous and next waypoint + + std::array current { position_velocity_ned.position.north_m, + position_velocity_ned.position.east_m, + position_velocity_ned.position.down_m }; + std::array wp_prev = get_waypoint_for_sequence(mission_items, + reverse ? progress.current + 1 : progress.current - 1, ct); + std::array wp_next = get_waypoint_for_sequence(mission_items, progress.current, ct); + + float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next); + + CHECK(distance_to_trajectory < corridor_radius_m); + } + }); +} + void AutopilotTester::check_tracks_mission(float corridor_radius_m) { auto mission = _mission->download_mission(); @@ -594,6 +637,12 @@ void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m); } +void AutopilotTester::execute_rtl_when_reaching_mission_sequence(int sequence_number) +{ + start_and_wait_for_mission_sequence_raw(sequence_number); + execute_rtl(); +} + std::array AutopilotTester::get_current_position_ned() { mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned(); @@ -701,15 +750,15 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry: return pass; } -void AutopilotTester::start_and_wait_for_first_mission_item() +void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number) { auto prom = std::promise {}; auto fut = prom.get_future(); - _mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) { + _mission->subscribe_mission_progress([&prom, this, sequence_number](Mission::MissionProgress progress) { std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; - if (progress.current >= 1) { + if (progress.current >= sequence_number) { _mission->subscribe_mission_progress(nullptr); prom.set_value(); } @@ -720,6 +769,25 @@ void AutopilotTester::start_and_wait_for_first_mission_item() REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); } +void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_number) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _mission_raw->subscribe_mission_progress([&prom, this, sequence_number](MissionRaw::MissionProgress progress) { + std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + + if (progress.current >= sequence_number) { + _mission_raw->subscribe_mission_progress(nullptr); + prom.set_value(); + } + }); + + REQUIRE(_mission_raw->start_mission() == MissionRaw::Result::Success); + + REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); +} + void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout) { auto prom = std::promise {}; diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 90d1505899..22a9225628 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -138,9 +138,11 @@ public: void request_ground_truth(); void check_mission_item_speed_above(int item_index, float min_speed_m_s); void check_tracks_mission(float corridor_radius_m = 1.5f); + void check_tracks_mission_raw(float corridor_radius_m = 1.f, bool reverse = false); void start_checking_altitude(const float max_deviation_m); void stop_checking_altitude(); void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f); + void execute_rtl_when_reaching_mission_sequence(int sequence_number); // Blocking call to get the drone's current position in NED frame std::array get_current_position_ned(); @@ -200,7 +202,8 @@ private: bool ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, float min_distance_m); bool estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m); bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m); - void start_and_wait_for_first_mission_item(); + void start_and_wait_for_mission_sequence(int sequence_number); + void start_and_wait_for_mission_sequence_raw(int sequence_number); void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout); void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout); void wait_for_mission_finished(std::chrono::seconds timeout); diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp new file mode 100644 index 0000000000..b3996a76c4 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_rtl.cpp @@ -0,0 +1,57 @@ + +/**************************************************************************** + * + * Copyright (c) 2023 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 "autopilot_tester_rtl.h" + +#include "math_helpers.h" +#include +#include +#include +#include + + +void AutopilotTesterRtl::connect(const std::string uri) +{ + AutopilotTester::connect(uri); +} + +void AutopilotTesterRtl::set_rtl_type(int rtl_type) +{ + CHECK(getParams()->set_param_int("RTL_TYPE", rtl_type) == Param::Result::Success); +} + +void AutopilotTesterRtl::set_takeoff_land_requirements(int req) +{ + CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success); +} diff --git a/test/mavsdk_tests/autopilot_tester_rtl.h b/test/mavsdk_tests/autopilot_tester_rtl.h new file mode 100644 index 0000000000..6c59d64462 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_rtl.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "autopilot_tester.h" + +#include +#include + + +class AutopilotTesterRtl : public AutopilotTester +{ +public: + AutopilotTesterRtl() = default; + ~AutopilotTesterRtl() = default; + + void set_rtl_type(int rtl_type); + void set_takeoff_land_requirements(int req); + void connect(const std::string uri); + + +private: + std::unique_ptr _failure{}; +}; diff --git a/test/mavsdk_tests/math_helpers.h b/test/mavsdk_tests/math_helpers.h index 34817381ef..3f26d3aa18 100644 --- a/test/mavsdk_tests/math_helpers.h +++ b/test/mavsdk_tests/math_helpers.h @@ -44,6 +44,20 @@ std::array get_local_mission_item(const Mission::MissionItem &item, const return {static_cast(local.north_m), static_cast(local.east_m), -item.relative_altitude_m}; } +template +std::array get_local_mission_item_from_raw_item(const mavsdk::MissionRaw::MissionItem &item, + const CoordinateTransformation &ct) +{ + using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate; + GlobalCoordinate global; + global.latitude_deg = item.x / 1e7; + global.longitude_deg = item.y / 1e7; + + + auto local = ct.local_from_global(global); + return {static_cast(local.north_m), static_cast(local.east_m), -item.z}; +} + template T sq(T x) { diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp index 5417254fce..e12f583c2c 100644 --- a/test/mavsdk_tests/test_vtol_mission.cpp +++ b/test/mavsdk_tests/test_vtol_mission.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include "autopilot_tester.h" +#include "autopilot_tester_rtl.h" TEST_CASE("Fly VTOL mission", "[vtol]") @@ -44,3 +44,47 @@ TEST_CASE("Fly VTOL mission", "[vtol]") tester.execute_mission_raw(); tester.wait_until_disarmed(); } + +TEST_CASE("RTL direct Home", "[vtol]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan"); + // fly directly to home position + tester.set_rtl_type(0); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(2); + tester.wait_until_disarmed(std::chrono::seconds(90)); + tester.check_home_within(5.0f); +} + +TEST_CASE("RTL with Mission Landing", "[vtol]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan"); + // Vehicle should follow the mission and use the mission landing + tester.set_rtl_type(2); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(2); + tester.check_tracks_mission_raw(35.0f); + tester.wait_until_disarmed(std::chrono::seconds(95)); +} + +TEST_CASE("RTL with Reverse Mission", "[vtol]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.set_takeoff_land_requirements(0); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_without_landing.plan"); + // vehicle should follow the mission in reverse and land at the home position // TODO enable checks again if MAVSDK can handle mission in reverse order + tester.set_rtl_type(2); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(6); + //tester.check_tracks_mission_raw(35.0f); + tester.wait_until_disarmed(std::chrono::seconds(90)); +} diff --git a/test/mavsdk_tests/vtol_mission_without_landing.plan b/test/mavsdk_tests/vtol_mission_without_landing.plan new file mode 100644 index 0000000000..e52e3f6fa4 --- /dev/null +++ b/test/mavsdk_tests/vtol_mission_without_landing.plan @@ -0,0 +1,184 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 12, + "globalPlanAltitudeMode": 1, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 84, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39833113265167, + 8.545508725338607, + 20 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.399332700068925, + 8.54481499384454, + 20 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39908888031702, + 8.54344001880591, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39760160279815, + 8.542394178137585, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.396941861414504, + 8.54282818797708, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.396686401111786, + 8.544419333554089, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.397202447861446, + 8.545440338322464, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39766309343905, + 8.545713820298545, + 20 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.39775218584113, + 8.545620889782981, + 489.0021493051957 + ], + "vehicleType": 20, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +}