From 635e162a0cebe03d24dd0c04d29ee982c00eebf6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Nov 2024 12:16:22 -0500 Subject: [PATCH 001/211] test/mavsdk_tests: continue sending manual control between steps in fly_forward_in_posctl()/fly_forward_in_altctl() --- test/mavsdk_tests/autopilot_tester.cpp | 48 ++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 730aa1d46052..a39ca1f3c9af 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -452,8 +452,29 @@ void AutopilotTester::fly_forward_in_posctl() } CHECK(_manual_control->start_position_control() == ManualControl::Result::Success); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + store_home(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + wait_until_ready(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + arm(); // Climb up for 5 seconds @@ -490,10 +511,37 @@ void AutopilotTester::fly_forward_in_altctl() } CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + store_home(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + wait_until_ready(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + arm(); + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + // Climb up for 5 seconds for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); From 1d2ed28966b9e8a7cc9ca7c43999e153dfa3be86 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Nov 2024 12:18:05 -0500 Subject: [PATCH 002/211] init.d-posix/rcS: relax default COM_RC_LOSS_T/COM_OF_LOSS_T with speed factor (0.5->1.0) --- ROMFS/px4fmu_common/init.d-posix/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index f96f97c56e49..e47b685a21f7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -190,11 +190,11 @@ if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER" param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER - COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc) + COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc) echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER" param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER - COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc) + COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc) echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER" param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER From ddc604d0de28eb22a41c643879e92190eeb524db Mon Sep 17 00:00:00 2001 From: Federico Ciresola Date: Wed, 20 Nov 2024 11:47:12 +0100 Subject: [PATCH 003/211] Broken link to the ARKV6X on the README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 74f8eeeada6f..e2e62c11c60e 100644 --- a/README.md +++ b/README.md @@ -104,7 +104,7 @@ These boards fully comply with Pixhawk Standard, and are maintained by the PX4-A These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers. -* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html) +* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/ark_v6x.html) * [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html) * [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html) * [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html) From 3f83a8fa7a0b5d7cc0b3b945d21b7a20f03763b6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Nov 2024 18:32:54 +0100 Subject: [PATCH 004/211] mavlink: remove reference to COLLISION message I had missed Should have been part of b5d18c613181f0935890aef91cbc1da4d06e00c3 --- src/modules/mavlink/mavlink_main.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d3258023ff1b..820a2d4b2229 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1420,7 +1420,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("DISTANCE_SENSOR", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); @@ -1496,7 +1495,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); @@ -1575,7 +1573,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); @@ -1661,7 +1658,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 10.0f); configure_stream_local("ESC_INFO", 10.0f); @@ -1764,7 +1760,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); From 7c507f136c6521fabb059eafb0df98ed964ecf93 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2024 13:20:54 +1300 Subject: [PATCH 005/211] bmp280: disable device filter It looks like the BMP280 was heavily filtered, leading to a step response of around 3 seconds. Comparing it to baro logs of BMP388 and MS5611, it looks very slow. I therefore suggest to disable the on device IIR filter which makes the log data look more like the other baros. --- src/drivers/barometer/bmp280/BMP280.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp index ae5387ad7c2a..864d4d3f3b88 100644 --- a/src/drivers/barometer/bmp280/BMP280.cpp +++ b/src/drivers/barometer/bmp280/BMP280.cpp @@ -67,7 +67,7 @@ BMP280::init() // set config, recommended settings _interface->set_reg(_curr_ctrl, BMP280_ADDR_CTRL); - _interface->set_reg(BMP280_CONFIG_F16, BMP280_ADDR_CONFIG); + _interface->set_reg(BMP280_CONFIG_F0, BMP280_ADDR_CONFIG); // get calibration and pre process them _cal = _interface->get_calibration(BMP280_ADDR_CAL); From 8e288629d028ae3f21136b6b75be5a4bc74c0931 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 20 Nov 2024 10:56:05 -0800 Subject: [PATCH 006/211] ci: move runners to capacity optimized --- .github/workflows/build_all_targets.yml | 8 ++++---- .github/workflows/sitl_tests.yml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml index b8073d56abe0..b8b66854e6dd 100644 --- a/.github/workflows/build_all_targets.yml +++ b/.github/workflows/build_all_targets.yml @@ -27,7 +27,7 @@ jobs: group_targets: name: Scan for Board Targets # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] outputs: matrix: ${{ steps.set-matrix.outputs.matrix }} timestamp: ${{ steps.set-timestamp.outputs.timestamp }} @@ -75,7 +75,7 @@ jobs: setup: name: Build Group [${{ matrix.group }}] # runs-on: ubuntu-latest - runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] needs: group_targets strategy: matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} @@ -128,7 +128,7 @@ jobs: artifacts: name: Upload Artifacts to S3 # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] needs: [setup, group_targets] if: contains(fromJSON('["main", "stable", "beta"]'), needs.group_targets.outputs.branchname) steps: @@ -157,7 +157,7 @@ jobs: release: name: Create Release and Upload Artifacts # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] needs: [setup, group_targets] if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch' steps: diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index 0492137a4990..da6ad69290de 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -16,7 +16,7 @@ on: jobs: build: name: Testing PX4 ${{ matrix.config.model }} - runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] container: image: px4io/px4-dev-simulation-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined From ada2bb4c42bb58b4ddb2cb12ab54c0c6c69ccebc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Nov 2024 13:19:15 -0500 Subject: [PATCH 007/211] uORB_tests: relax latency threshold on non-realtime SITL --- platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index b40b60ad4760..035d2f9ac52a 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -146,7 +146,14 @@ int uORBTest::UnitTest::pubsublatency_main() pubsubtest_passed = true; - if (mean > 150.0f) { +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + // relaxed on SITL (non-realtime) + const float kMaxMeanUs = 1000.f; // 1000 microseconds +#else + const float kMaxMeanUs = 150.f; // 150 microseconds +#endif + + if (mean > kMaxMeanUs) { pubsubtest_res = PX4_ERROR; } else { From 7288d16855ea7d432697c4561c9a239fab99e26f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Nov 2024 13:53:29 -0500 Subject: [PATCH 008/211] mavsdk_tests set nice levels for px4/gzclient/mavsdk_tests --- test/mavsdk_tests/process_helper.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index 5c8701e39dac..ff65e7743455 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -155,10 +155,12 @@ def __init__(self, workspace_dir: str, log_dir: str, debugger: str, verbose: bool, build_dir: str): super().__init__(log_dir, model, case, verbose) self.name = "px4" - self.cmd = os.path.join(workspace_dir, build_dir, "bin/px4") self.cwd = os.path.join(workspace_dir, build_dir, "tmp_mavsdk_tests/rootfs") + self.cmd = "nice" self.args = [ + "--20", + os.path.join(workspace_dir, build_dir, "bin/px4"), os.path.join(workspace_dir, build_dir, "etc"), "-s", "etc/init.d-posix/rcS", @@ -329,8 +331,10 @@ def __init__(self, self.env = dict(os.environ, **{ "GAZEBO_MODEL_PATH": os.path.join(workspace_dir, PX4_GAZEBO_MODELS)}) - self.cmd = "gzclient" - self.args = ["--verbose"] + self.cmd = "nice" + self.args = ["--19", + "gzclient", + "--verbose"] class TestRunner(Runner): @@ -347,7 +351,7 @@ def __init__(self, self.name = "mavsdk_tests" self.cwd = workspace_dir self.cmd = "nice" - self.args = ["-5", + self.args = ["--18", os.path.join( workspace_dir, build_dir, From 1cfefc4c63b905872ce62a984890b8cbcdce2465 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 20 Nov 2024 10:46:27 -0900 Subject: [PATCH 009/211] ark_fpv: fix bootloader serial fw update (#23984) --- boards/ark/fpv/extras/ark_fpv_bootloader.bin | Bin 46268 -> 46268 bytes boards/ark/fpv/src/hw_config.h | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin index 44a8f3a0e50c62d5273ea2e44e3e9699af60ac5d..9ca59954b374a13b5a6b739d301164a0ff7b46a1 100755 GIT binary patch delta 32 ocmdn Date: Wed, 20 Nov 2024 11:43:28 -0800 Subject: [PATCH 010/211] ci: disable spot instances guarantee execution of jobs --- .github/workflows/build_all_targets.yml | 8 ++++---- .github/workflows/compile_ubuntu.yml | 2 +- .github/workflows/dev_container.yml | 2 +- .github/workflows/sitl_tests.yml | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml index b8b66854e6dd..43f98e4f6f49 100644 --- a/.github/workflows/build_all_targets.yml +++ b/.github/workflows/build_all_targets.yml @@ -27,7 +27,7 @@ jobs: group_targets: name: Scan for Board Targets # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] outputs: matrix: ${{ steps.set-matrix.outputs.matrix }} timestamp: ${{ steps.set-timestamp.outputs.timestamp }} @@ -75,7 +75,7 @@ jobs: setup: name: Build Group [${{ matrix.group }}] # runs-on: ubuntu-latest - runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: group_targets strategy: matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} @@ -128,7 +128,7 @@ jobs: artifacts: name: Upload Artifacts to S3 # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: [setup, group_targets] if: contains(fromJSON('["main", "stable", "beta"]'), needs.group_targets.outputs.branchname) steps: @@ -157,7 +157,7 @@ jobs: release: name: Create Release and Upload Artifacts # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: [setup, group_targets] if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch' steps: diff --git a/.github/workflows/compile_ubuntu.yml b/.github/workflows/compile_ubuntu.yml index 61994162facb..31d1a1e53711 100644 --- a/.github/workflows/compile_ubuntu.yml +++ b/.github/workflows/compile_ubuntu.yml @@ -18,7 +18,7 @@ jobs: fail-fast: false matrix: version: ['ubuntu:22.04', 'ubuntu:24.04'] - runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}",spot=false] container: image: ${{ matrix.version }} volumes: diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index f4ac40476132..b9f6cfab69aa 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -15,7 +15,7 @@ on: jobs: build: name: Build Container - runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}",spot=false] steps: - uses: actions/checkout@v4 diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index da6ad69290de..3d326cda50dd 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -16,7 +16,7 @@ on: jobs: build: name: Testing PX4 ${{ matrix.config.model }} - runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=capacity-optimized] + runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] container: image: px4io/px4-dev-simulation-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined From cad2de228cfd75cd0354a57bb43eeed3fa2ecccf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Nov 2024 18:14:33 -0500 Subject: [PATCH 011/211] .github/workflows/clang-tidy.yml - clang-tidy checks retry if failed - this clang tidy runner occasionally fails intermittently --- .github/workflows/clang-tidy.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index a8cba7ce6bd7..2736767e0bb7 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -17,5 +17,7 @@ jobs: with: token: ${{secrets.ACCESS_TOKEN}} - - name: make clang-tidy-quiet - run: make clang-tidy-quiet + - uses: corrupt952/actions-retry-command@v1.0.7 + with: + command: make clang-tidy-quiet + max_attempts: 3 From 74447a3ae2010e668c2ac840f50aa98314524b83 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 20 Nov 2024 11:18:58 -0800 Subject: [PATCH 012/211] ci: px4-dev container --- .github/workflows/dev_container.yml | 52 ++++++++++++++++++++++++++--- 1 file changed, 47 insertions(+), 5 deletions(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index b9f6cfab69aa..de89addaea42 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -8,38 +8,80 @@ on: - 'stable' - 'beta' - 'release/**' + tags: + - 'v*' pull_request: branches: - '*' jobs: build: - name: Build Container - runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}",spot=false] + name: Build and Push Container + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] steps: - - uses: actions/checkout@v4 with: fetch-tags: true submodules: false + fetch-depth: 0 + + - name: Login to Docker Hub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract metadata (tags, labels) for Docker + id: meta + uses: docker/metadata-action@v5 + with: + images: | + ghcr.io/PX4/px4-dev + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 - name: Build and load container image uses: docker/build-push-action@v6 + id: docker with: context: Tools/setup - tags: px4-dev:latest + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} platforms: | linux/amd64 load: true push: false + cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} + cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max - name: make quick_check uses: addnab/docker-run-action@v3 with: - image: px4-dev:latest + image: ${{ fromJSON(steps.docker.outputs.metadata)['image.name'] }} options: -v ${{ github.workspace }}:/workspace run: | cd /workspace git config --global --add safe.directory /workspace make px4_sitl_default make px4_fmu-v6x_default + + - name: Push container image + uses: docker/build-push-action@v6 + with: + context: Tools/setup + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + platforms: | + linux/amd64 + provenance: mode=max + push: ${{ github.event_name == 'push' }} + cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} + cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max From fb0f9b8aa7e0df52c2c3419fe64b8924dfe419bf Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 20 Nov 2024 15:51:09 -0800 Subject: [PATCH 013/211] ci: fix container image name when multiple given when this is workflow triggered by a tag push event, then it tags the image in multiple ways eg: "image.name": "ghcr.io/px4/px4-dev:v1.16.0-alpha1,ghcr.io/px4/px4-dev:latest" Having multiple names messes up the exectuion of the next step in the chain since we depend on its name to complete the testing --- .github/workflows/dev_container.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index de89addaea42..5cb9bd7be6a2 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -62,10 +62,15 @@ jobs: cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max + - name: Get Tag Name + id: tag_name + run: | + echo "::set-output name=tag_name::$(echo '${{ fromJSON(steps.docker.outputs.metadata)['image.name'] }}' | sed 's/,.*$//')" + - name: make quick_check uses: addnab/docker-run-action@v3 with: - image: ${{ fromJSON(steps.docker.outputs.metadata)['image.name'] }} + image: ${{ steps.tag_name.outputs.tag_name }} options: -v ${{ github.workspace }}:/workspace run: | cd /workspace From dbc2e56b2ebf3232aeb8f11abdfcadadd55a3ed9 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 20 Nov 2024 16:45:50 -0800 Subject: [PATCH 014/211] ci: disable docker hub access --- .github/workflows/dev_container.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index 5cb9bd7be6a2..22bd291d0e57 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -25,12 +25,6 @@ jobs: submodules: false fetch-depth: 0 - - name: Login to Docker Hub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Login to GitHub Container Registry uses: docker/login-action@v3 with: From 2f8460da9111132ca565241be9081b0e90cd9d74 Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 20 Nov 2024 13:16:31 +0000 Subject: [PATCH 015/211] Update submodule mavlink to latest Wed Nov 20 13:16:31 UTC 2024 - mavlink in PX4/Firmware (7e594e898b2281e7d3f6ef4057f1add829643058): https://github.com/mavlink/mavlink/commit/e221d46d4631a5b6f1e91040cb1fe4b6fa91ea43 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/0e420102dbdd7e9f59617dd00c05a2470f22eef2 - Changes: https://github.com/mavlink/mavlink/compare/e221d46d4631a5b6f1e91040cb1fe4b6fa91ea43...0e420102dbdd7e9f59617dd00c05a2470f22eef2 0e420102 2024-11-20 David Sastre - development.xml: change OPERATOR_CONTROL id to 32100: (#2174) 01e0cc2f 2024-11-14 Hamish Willee - SMART_BATTERY_INFO - revert and renumber BATTERY_INFO (#2173) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index e221d46d4631..0e420102dbdd 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit e221d46d4631a5b6f1e91040cb1fe4b6fa91ea43 +Subproject commit 0e420102dbdd7e9f59617dd00c05a2470f22eef2 From a49f0347577a421612ea109848d15d15a64624b6 Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 20 Nov 2024 11:30:33 +0000 Subject: [PATCH 016/211] update all px4board kconfig --- boards/micoair/h743-aio/default.px4board | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index e84ef5b818db..97068102d2aa 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -20,10 +20,10 @@ CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_TAP_ESC=y +CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_INTERFACES=1 -CONFIG_COMMON_TELEMETRY=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_COMMANDER=y From b8c2805263f7761772117cbaf93766909a8dabac Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Nov 2024 15:31:54 +0100 Subject: [PATCH 017/211] spi: Suppress null pointer warning Depending on defines px4_spi_buses can be NULL but often it's not and in those cases the compiler correctly warns about it. --- platforms/common/spi.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index 963372bdbe0f..bf553fbbf19b 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -87,7 +87,12 @@ const px4_spi_bus_t *px4_spi_buses{nullptr}; int px4_find_spi_bus(uint32_t devid) { +// px4_spi_buses is only NULL on certain targets depending on defines +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Waddress" + for (int i = 0; ((px4_spi_bus_t *) px4_spi_buses) != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) { +#pragma GCC diagnostic pop const px4_spi_bus_t &bus_data = px4_spi_buses[i]; if (bus_data.bus == -1) { From 0cd6a553b9ed14ab1b722837787fd588ca3ecac6 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 9 Aug 2024 14:58:13 +0200 Subject: [PATCH 018/211] CollisionPrevention: rewrite for Acceleration based manual flight mode --- .../CollisionPrevention.cpp | 298 +++++++++++------- .../CollisionPrevention.hpp | 98 ++++-- .../CollisionPreventionTest.cpp | 128 ++++---- .../FlightTaskManualPosition.cpp | 5 - .../tasks/Utility/StickAccelerationXY.cpp | 8 + .../tasks/Utility/StickAccelerationXY.hpp | 3 + .../simulation/gz_bridge/CMakeLists.txt | 1 + 7 files changed, 348 insertions(+), 193 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b6982..8dfc27edf189 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -40,8 +40,6 @@ #include "CollisionPrevention.hpp" #include -using namespace matrix; -using namespace time_literals; namespace { @@ -113,7 +111,7 @@ bool CollisionPrevention::is_active() } void -CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude) +CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude) { int msg_index = 0; float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); @@ -197,6 +195,36 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_ return false; } +bool +CollisionPrevention::_checkSetpointDirectionFeasability() +{ + bool setpoint_feasible = true; + + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + // check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data + if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_nodata.get() + || (_param_cp_go_nodata.get() && _data_fov[i]))) { + setpoint_feasible = false; + + } + } + + return setpoint_feasible; +} + +void +CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) +{ + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + _setpoint_dir = setpoint / setpoint.norm();; + const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - + _obstacle_map_body_frame.angle_offset); + _setpoint_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps + _adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad); +} + void CollisionPrevention::_updateObstacleMap() { @@ -244,12 +272,49 @@ CollisionPrevention::_updateObstacleMap() _obstacle_distance_pub.publish(_obstacle_map_body_frame); } +void CollisionPrevention::_updateObstacleData() +{ + _obstacle_data_present = false; + _closest_dist = UINT16_MAX; + _closest_dist_dir.setZero(); + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + + // if the data is stale, reset the bin + if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + + _obstacle_map_body_frame.angle_offset)); + const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + uint bin_distance = _obstacle_map_body_frame.distances[i]; + + // check if there is avaliable data and the data of the map is not stale + if (bin_distance < UINT16_MAX + && (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { + _obstacle_data_present = true; + } + + if (bin_distance * 0.01f < _closest_dist) { + _closest_dist = bin_distance * 0.01f; + _closest_dist_dir = bin_direction; + } + } +} + void -CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude) +CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude) { // clamp at maximum sensor range float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance); + // negative values indicate out of range but valid measurements. + if (fabsf(distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) { + distance_reading = distance_sensor.max_distance; + } + // discard values below min range if ((distance_reading > distance_sensor.min_distance)) { @@ -268,7 +333,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, if (upper_bound < 0) { upper_bound++; } // rotate vehicle attitude into the sensor body frame - matrix::Quatf attitude_sensor_frame = vehicle_attitude; + Quatf attitude_sensor_frame = vehicle_attitude; attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); @@ -294,7 +359,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { - const float col_prev_d = _param_cp_dist.get(); const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); const int sp_index_original = setpoint_index; float best_cost = 9999.f; @@ -310,7 +374,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi int bin = wrap_bin(j); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { - mean_dist += col_prev_d * 100.f; + mean_dist += _param_cp_dist.get() * 100.f; } else { mean_dist += _obstacle_map_body_frame.distances[bin]; @@ -319,7 +383,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi const int bin = wrap_bin(i); mean_dist = mean_dist / (2.f * filter_size + 1.f); - const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original); + const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original); const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin]; if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) { @@ -376,7 +440,7 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist break; case distance_sensor_s::ROTATION_CUSTOM: - offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi(); + offset = Eulerf(Quatf(distance_sensor.q)).psi(); break; } @@ -384,154 +448,164 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist } void -CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, - const Vector2f &curr_vel) +CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { _updateObstacleMap(); + _updateObstacleData(); - // read parameters - const float col_prev_d = _param_cp_dist.get(); - const float col_prev_dly = _param_cp_delay.get(); - const bool move_no_data = _param_cp_go_nodata.get(); - const float xy_p = _param_mpc_xy_p.get(); - const float max_jerk = _param_mpc_jerk_max.get(); - const float max_accel = _param_mpc_acc_hor.get(); - const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); + const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); - const float setpoint_length = setpoint.norm(); + const float setpoint_length = setpoint_accel.norm(); + _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); - const hrt_abstime constrain_time = getTime(); - int num_fov_bins = 0; + const hrt_abstime now = getTime(); - if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { - if (setpoint_length > 0.001f) { + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + Vector2f constr_accel_setpoint{}; - Vector2f setpoint_dir = setpoint / setpoint_length; - float vel_max = setpoint_length; - const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d); + const bool is_stick_deflected = setpoint_length > 0.001f; - const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - - _obstacle_map_body_frame.angle_offset); - int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + if (_obstacle_data_present && is_stick_deflected) { - // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps - _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + _transformSetpoint(setpoint_accel); - // limit speed for safe flight - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message + _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, + vel_comp_accel, vel_comp_accel_dir); - // delete stale values - const hrt_abstime data_age = constrain_time - _data_timestamps[i]; + if (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); + } - if (data_age > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; - const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters - const float max_range = _data_maxranges[i] * 0.01f; // convert to meters - float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + } else if (!_obstacle_data_present) - // convert from body to local frame in the range [0, 2*pi] - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + { + // allow no movement + PX4_WARN("No obstacle data, not moving..."); + setpoint_accel.setZero(); - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + // if distance data is stale, switch to Loiter + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US && + getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + } - //count number of bins in the field of valid_new - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } + _last_timeout_warning = getTime(); + } + } +} - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { +float +CollisionPrevention::_getObstacleDistance(const Vector2f &direction) +{ + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + Vector2f dir = direction / direction.norm(); + const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - + _obstacle_map_body_frame.angle_offset); + int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + + return _obstacle_map_body_frame.distances[dir_index] * 0.01f; +} - if (setpoint_dir.dot(bin_direction) > 0) { - // calculate max allowed velocity with a P-controller (same gain as in the position controller) - const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - float delay_distance = curr_vel_parallel * col_prev_dly; +Vector2f +CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length) +{ + Vector2f new_setpoint{}; + const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir)); + const Vector2f tangential_component = _setpoint_dir - normal_component; - if (distance < max_range) { - delay_distance += curr_vel_parallel * (data_age * 1e-6f); - } + const float normal_scale = _getScale(_closest_dist); - const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); - const float vel_max_posctrl = xy_p * stop_distance; - const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); - const float projection = bin_direction.dot(setpoint_dir); - float vel_max_bin = vel_max; + const float closest_dist_tangential = _getObstacleDistance(tangential_component); + const float tangential_scale = _getScale(closest_dist_tangential); - if (projection > 0.01f) { - vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection; - } - // constrain the velocity - if (vel_max_bin >= 0) { - vel_max = math::min(vel_max, vel_max_bin); - } - } + // only scale accelerations towards the obstacle + if (_closest_dist_dir.dot(_setpoint_dir) > 0) { + new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length; - } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { - if (!move_no_data || (move_no_data && _data_fov[i])) { - vel_max = 0.f; - } - } - } + } else { + new_setpoint = _setpoint_dir * setpoint_length; + } - //if the sensor field of view is zero, never allow to move (even if move_no_data=1) - if (num_fov_bins == 0) { - vel_max = 0.f; - } + return new_setpoint; +} - setpoint = setpoint_dir * vel_max; - } +float +CollisionPrevention::_getScale(const float &reference_distance) +{ + float scale = (reference_distance - _min_dist_to_keep); + const float scale_distance = math::max(_min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get()); - } else { - //allow no movement - float vel_max = 0.f; - setpoint = setpoint * vel_max; + // if scale is positive, square it and scale it with the scale_distance + scale = scale > 0 ? powf(scale / scale_distance, 2) : scale; + scale = math::min(scale, 1.0f); + return scale; +} - // if distance data is stale, switch to Loiter - if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { +void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, + const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir) +{ + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + const float max_range = _data_maxranges[i] * 0.01f; - if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US - && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { - _publishVehicleCmdDoLoiter(); + // get the vector pointing into the direction of current bin + float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + + _obstacle_map_body_frame.angle_offset)); + + const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) }; + float bin_distance = _obstacle_map_body_frame.distances[i]; + + // only consider bins which are between min and max values + if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) { + const float distance = bin_distance * 0.01f; + + // Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high + // acceleration maneuvers + const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction)); + float delay_distance = curr_vel_parallel * _param_cp_delay.get(); + + const hrt_abstime data_age = now - _data_timestamps[i]; + + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); } - _last_timeout_warning = getTime(); - } + const float stop_distance = math::max(0.f, distance - _min_dist_to_keep - delay_distance); + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + // we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down. + const float curr_acc_vel_constraint = _param_mpc_vel_p_acc.get() * (max_vel - curr_vel_parallel); + if (curr_acc_vel_constraint < vel_comp_accel) { + vel_comp_accel = curr_acc_vel_constraint; + vel_comp_accel_dir = bin_direction; + } + } } } void -CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos, - const Vector2f &curr_vel) +CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { //calculate movement constraints based on range data - Vector2f new_setpoint = original_setpoint; - _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); - - //warn user if collision prevention starts to interfere - bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed - || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed - || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed - || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed); - - _interfering = currently_interfering; + Vector2f original_setpoint = setpoint_accel; + _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); // publish constraints collision_constraints_s constraints{}; - constraints.timestamp = getTime(); original_setpoint.copyTo(constraints.original_setpoint); - new_setpoint.copyTo(constraints.adapted_setpoint); + setpoint_accel.copyTo(constraints.adapted_setpoint); + constraints.timestamp = getTime(); _constraints_pub.publish(constraints); - - original_setpoint = new_setpoint; } void CollisionPrevention::_publishVehicleCmdDoLoiter() diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index aaff04e2cbdc..651e6372f4db 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -34,6 +34,7 @@ /** * @file CollisionPrevention.hpp * @author Tanja Baumann + * @author Claudio Chies * * CollisionPrevention controller. * @@ -60,6 +61,12 @@ #include using namespace time_literals; +using matrix::Vector2f; +using matrix::Vector3f; +using matrix::Quatf; +using matrix::Eulerf; +using matrix::wrap; +using matrix::wrap_2pi; class CollisionPrevention : public ModuleParams { @@ -74,13 +81,10 @@ class CollisionPrevention : public ModuleParams /** * Computes collision free setpoints - * @param original_setpoint, setpoint before collision prevention intervention - * @param max_speed, maximum xy speed - * @param curr_pos, current vehicle position - * @param curr_vel, current vehicle velocity + * @param setpoint_accel setpoint purely based on sticks, to be modified + * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed */ - void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, - const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); + void modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); protected: @@ -90,13 +94,13 @@ class CollisionPrevention : public ModuleParams uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( _obstacle_map_body_frame.distances[0])]; /**< in cm */ - void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); + void _addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude); /** * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -104,7 +108,40 @@ class CollisionPrevention : public ModuleParams * @param setpoint_index, index of the setpoint in the internal obstacle map * @param vehicle_yaw_angle_rad, vehicle orientation */ - void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + + /** + * Calculate the constrained setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint + */ + void _calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + + /** + * Constrain the acceleration setpoint based on the distance to the obstacle + * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above + * +1 ________ _ _ + * ┌─┐ │ // + * │X│ │ // + * │X│ │ // + * │X│ │ /// + * │X│ │ // + * │X│ │///// + * │X│──────┼─────────────┬───────────── + * │X│ /│ scale_distance + * │X│ / │ + * │X│ / │ + * │X│ / │ + * │X│ / │ + * └─┘/ │ + * -1 + */ + Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); + + void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir); + + float _getObstacleDistance(const Vector2f &direction); + + float _getScale(const float &reference_distance); /** * Determines whether a new sensor measurement is used @@ -114,6 +151,10 @@ class CollisionPrevention : public ModuleParams */ bool _enterData(int map_index, float sensor_range, float sensor_reading); + bool _checkSetpointDirectionFeasability(); + + void _transformSetpoint(const Vector2f &setpoint); + //Timing functions. Necessary to mock time in the tests virtual hrt_abstime getTime(); @@ -122,10 +163,20 @@ class CollisionPrevention : public ModuleParams private: - bool _interfering{false}; /**< states if the collision prevention interferes with the user input */ + bool _data_stale{true}; /**< states if the data is stale */ bool _was_active{false}; /**< states if the collision prevention interferes with the user input */ + bool _obstacle_data_present{false}; /**< states if obstacle data is present */ + + int _setpoint_index{}; /**< index of the setpoint*/ + Vector2f _setpoint_dir{}; /**< direction of the setpoint*/ + + float _closest_dist{}; /**< closest distance to an obstacle */ + Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */ + + float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ + Vector2f _DEBUG; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ @@ -142,13 +193,15 @@ class CollisionPrevention : public ModuleParams hrt_abstime _time_activated{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ - (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ - (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ - (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ - (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ + (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ + (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ + (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ + (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ + (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_mpc_vel_p_acc, /**< p gain from velocity controller*/ + (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) /** @@ -164,15 +217,15 @@ class CollisionPrevention : public ModuleParams * @param curr_pos, current vehicle position * @param curr_vel, current vehicle velocity */ - void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, - const matrix::Vector2f &curr_vel); + void _calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, + const Vector2f &curr_vel); /** * Publishes collision_constraints message * @param original_setpoint, setpoint before collision prevention intervention * @param adapted_setpoint, collision prevention adaped setpoint */ - void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + void _publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); /** * Publishes obstacle_distance message with fused data from offboard and from distance sensors @@ -185,6 +238,11 @@ class CollisionPrevention : public ModuleParams */ void _updateObstacleMap(); + /** + * Updates the obstacle data based on stale data and calculates values from the map + */ + void _updateObstacleData(); + /** * Publishes vehicle command. */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa00..45b01b2a7ed0 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -104,8 +104,6 @@ TEST_F(CollisionPreventionTest, noSensorData) // GIVEN: a simple setup condition TestCollisionPrevention cp; matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3.f; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); // AND: a parameter handle @@ -117,7 +115,7 @@ TEST_F(CollisionPreventionTest, noSensorData) cp.paramsChanged(); matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: collision prevention should be enabled and limit the speed to zero EXPECT_TRUE(cp.is_active()); @@ -130,8 +128,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) TestCollisionPrevention cp; matrix::Vector2f original_setpoint1(10, 0); matrix::Vector2f original_setpoint2(-10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); @@ -141,9 +137,12 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) attitude.q[3] = 0.0f; // AND: a parameter handle - param_t param = param_handle(px4::params::CP_DIST); - float value = 10; // try to keep 10m distance - param_set(param, &value); + param_t param1 = param_handle(px4::params::CP_DIST); + float value1 = 10; // try to keep 10m distance + param_set(param1, &value1); + param_t param2 = param_handle(px4::params::CP_GUIDE_ANG); + float value2 = 0; // dont guide sideways + param_set(param2, &value2); cp.paramsChanged(); // AND: an obstacle message @@ -174,19 +173,20 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); matrix::Vector2f modified_setpoint1 = original_setpoint1; matrix::Vector2f modified_setpoint2 = original_setpoint2; - cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); - cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint1, curr_vel); + cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(obstacle_distance_pub); orb_unadvertise(vehicle_attitude_pub); // THEN: the internal map should know the obstacle - // case 1: the velocity setpoint should be cut down to zero - // case 2: the velocity setpoint should stay the same as the input + // case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low + // case 2: the acceleration setpoint should be lower EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); - - EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); - EXPECT_FLOAT_EQ(original_setpoint2.norm(), modified_setpoint2.norm()); + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); + EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1); } TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) @@ -195,8 +195,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) TestCollisionPrevention cp; matrix::Vector2f original_setpoint1(10, 0); matrix::Vector2f original_setpoint2(-10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); @@ -234,19 +232,23 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) //WHEN: We run the setpoint modification matrix::Vector2f modified_setpoint1 = original_setpoint1; matrix::Vector2f modified_setpoint2 = original_setpoint2; - cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); - cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint1, curr_vel); + cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(distance_sensor_pub); orb_unadvertise(vehicle_attitude_pub); // THEN: the internal map should know the obstacle - // case 1: the velocity setpoint should be cut down to zero because there is an obstacle - // case 2: the velocity setpoint should be cut down to zero because there is no data + // case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low + // case 2: the acceleration setpoint should be lower EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); - EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); - EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1); + EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); + EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); + EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1); } TEST_F(CollisionPreventionTest, testPurgeOldData) @@ -256,8 +258,6 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; @@ -268,7 +268,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 10; // try to keep 10m distance + float value = 1; // try to keep 10m distance param_set(param, &value); cp.paramsChanged(); @@ -308,7 +308,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) for (int i = 0; i < 10; i++) { matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); mocked_time = mocked_time + 100000; //advance time by 0.1 seconds message_lost_data.timestamp = mocked_time; @@ -345,8 +345,6 @@ TEST_F(CollisionPreventionTest, testNoRangeData) hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; @@ -386,7 +384,7 @@ TEST_F(CollisionPreventionTest, testNoRangeData) for (int i = 0; i < 10; i++) { matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); //advance time by 0.1 seconds but no new message comes in mocked_time = mocked_time + 100000; @@ -411,13 +409,11 @@ TEST_F(CollisionPreventionTest, noBias) // GIVEN: a simple setup condition TestCollisionPrevention cp; matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 5; // try to keep 5m distance + float value = 2; // try to keep 2m distance param_set(param, &value); cp.paramsChanged(); @@ -439,7 +435,7 @@ TEST_F(CollisionPreventionTest, noBias) orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); orb_unadvertise(obstacle_distance_pub); // THEN: setpoint should go into the same direction as the stick input @@ -451,8 +447,6 @@ TEST_F(CollisionPreventionTest, outsideFOV) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); // AND: a parameter handle @@ -493,7 +487,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) matrix::Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); //THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV float setpoint_length = modified_setpoint.norm(); @@ -514,8 +508,6 @@ TEST_F(CollisionPreventionTest, goNoData) { // GIVEN: a simple setup condition with the initial state (no distance data) TestCollisionPrevention cp; - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); // AND: an obstacle message @@ -532,7 +524,7 @@ TEST_F(CollisionPreventionTest, goNoData) float angle = i * message.increment; if (angle > 0.f && angle < 40.f) { - message.distances[i] = 700; + message.distances[i] = 1000; } else { message.distances[i] = UINT16_MAX; @@ -541,7 +533,7 @@ TEST_F(CollisionPreventionTest, goNoData) // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 5; // try to keep 5m distance + float value = 2; // try to keep 5m distance param_set(param, &value); cp.paramsChanged(); @@ -549,8 +541,8 @@ TEST_F(CollisionPreventionTest, goNoData) matrix::Vector2f original_setpoint = {-5, 0}; matrix::Vector2f modified_setpoint = original_setpoint; - //THEN: the modified setpoint should be zero velocity - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + //THEN: the modified setpoint should be zero acceleration + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); //WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV @@ -561,7 +553,7 @@ TEST_F(CollisionPreventionTest, goNoData) //THEN: When all bins contain UINT_16MAX the setpoint should be zero even if CP_GO_NO_DATA=1 modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); //THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed @@ -570,7 +562,7 @@ TEST_F(CollisionPreventionTest, goNoData) orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm()); orb_unadvertise(obstacle_distance_pub); } @@ -580,8 +572,6 @@ TEST_F(CollisionPreventionTest, jerkLimit) // GIVEN: a simple setup condition TestCollisionPrevention cp; matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); // AND: distance set to 5m @@ -608,7 +598,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; - cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint_default_jerk, curr_vel); orb_unadvertise(obstacle_distance_pub); // AND: we now set max jerk to 0.1 @@ -619,11 +609,39 @@ TEST_F(CollisionPreventionTest, jerkLimit) // WHEN: we run the setpoint modification again matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint; - cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel); // THEN: the new setpoint should be much slower than the one with default jerk EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm()); } +TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + cp.getObstacleMap().increment = 10.f; + matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + // Distance is out of Range + distance_sensor.current_distance = -1.f; + distance_sensor.signal_quality = 0; + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], distance_sensor.max_distance * 100.f); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + } +} TEST_F(CollisionPreventionTest, addDistanceSensorData) { @@ -1062,8 +1080,6 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // GIVEN: a simple setup condition TestCollisionPrevention cp; matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); matrix::Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); @@ -1115,7 +1131,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors) orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the long range measurement EXPECT_EQ(500, cp.getObstacleMap().distances[2]); @@ -1124,10 +1140,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // WHEN: we publish the short range message followed by a long range message short_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the short range measurement EXPECT_EQ(150, cp.getObstacleMap().distances[2]); @@ -1136,10 +1152,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // WHEN: we publish the short range message with values out of range followed by a long range message short_range_msg_no_obstacle.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the short range measurement EXPECT_EQ(500, cp.getObstacleMap().distances[2]); diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 71dedaf0566a..352875764e30 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -94,11 +94,6 @@ void FlightTaskManualPosition::_scaleSticks() // Rotate setpoint into local frame Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint); - // collision prevention - if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy()); - } - _velocity_setpoint.xy() = vel_sp_xy; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c0fe9b315b67..e9e9ef0dbfdf 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -96,6 +96,14 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp); _acceleration_setpoint = stick_xy.emult(acceleration_scale); + if (_collision_prevention.is_active()) { + matrix::Vector2f accel_setpoint_xy = _acceleration_setpoint; + matrix::Vector2f vel_setpoint_xy = _velocity_setpoint; + _collision_prevention.modifySetpoint(accel_setpoint_xy, vel_setpoint_xy); + _acceleration_setpoint = accel_setpoint_xy; + + } + // Add drag to limit speed and brake again Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 8bc2bb75313d..9fff1da1c426 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -65,6 +66,8 @@ class StickAccelerationXY : public ModuleParams void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; private: + CollisionPrevention _collision_prevention{this}; + void applyJerkLimit(const float dt); matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, const matrix::Vector2f &vel_sp); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 358a4e703226..a094298fab90 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -90,6 +90,7 @@ if(gz-transport_FOUND) lawn aruco rover + walls ) # find corresponding airframes From 079f3ca67cd53daefd68738aaba52d6af6799cc8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 11 Nov 2024 15:53:30 +0100 Subject: [PATCH 019/211] StickAccelerationXY: Simplify collision prevention call --- .../tasks/Utility/StickAccelerationXY.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index e9e9ef0dbfdf..f823f929e819 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -97,11 +97,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, _acceleration_setpoint = stick_xy.emult(acceleration_scale); if (_collision_prevention.is_active()) { - matrix::Vector2f accel_setpoint_xy = _acceleration_setpoint; - matrix::Vector2f vel_setpoint_xy = _velocity_setpoint; - _collision_prevention.modifySetpoint(accel_setpoint_xy, vel_setpoint_xy); - _acceleration_setpoint = accel_setpoint_xy; - + _collision_prevention.modifySetpoint(_acceleration_setpoint, _velocity_setpoint); } // Add drag to limit speed and brake again From eb06ace8e43344e2053864fba087c78fb1b1238b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 11 Nov 2024 15:56:07 +0100 Subject: [PATCH 020/211] CollisionPrevention: fix matrix namespace don't use it in header such that clients are free to redefine the names but include it in cpp files and make use of that. --- .../CollisionPrevention.cpp | 3 +- .../CollisionPrevention.hpp | 36 ++--- .../CollisionPreventionTest.cpp | 142 +++++++++--------- 3 files changed, 87 insertions(+), 94 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 8dfc27edf189..2bc3b6626664 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -40,6 +40,7 @@ #include "CollisionPrevention.hpp" #include +using namespace matrix; namespace { @@ -550,7 +551,7 @@ CollisionPrevention::_getScale(const float &reference_distance) } void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, - const matrix::Vector2f &setpoint_vel, + const Vector2f &setpoint_vel, const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir) { for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 651e6372f4db..b7720b047e00 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -61,12 +61,6 @@ #include using namespace time_literals; -using matrix::Vector2f; -using matrix::Vector3f; -using matrix::Quatf; -using matrix::Eulerf; -using matrix::wrap; -using matrix::wrap_2pi; class CollisionPrevention : public ModuleParams { @@ -84,7 +78,7 @@ class CollisionPrevention : public ModuleParams * @param setpoint_accel setpoint purely based on sticks, to be modified * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed */ - void modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); protected: @@ -94,13 +88,13 @@ class CollisionPrevention : public ModuleParams uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( _obstacle_map_body_frame.distances[0])]; /**< in cm */ - void _addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude); + void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); /** * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -108,12 +102,12 @@ class CollisionPrevention : public ModuleParams * @param setpoint_index, index of the setpoint in the internal obstacle map * @param vehicle_yaw_angle_rad, vehicle orientation */ - void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); /** * Calculate the constrained setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint */ - void _calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); /** * Constrain the acceleration setpoint based on the distance to the obstacle @@ -134,12 +128,12 @@ class CollisionPrevention : public ModuleParams * └─┘/ │ * -1 */ - Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); + matrix::Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel, - const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir); + const hrt_abstime now, float &vel_comp_accel, matrix::Vector2f &vel_comp_accel_dir); - float _getObstacleDistance(const Vector2f &direction); + float _getObstacleDistance(const matrix::Vector2f &direction); float _getScale(const float &reference_distance); @@ -153,7 +147,7 @@ class CollisionPrevention : public ModuleParams bool _checkSetpointDirectionFeasability(); - void _transformSetpoint(const Vector2f &setpoint); + void _transformSetpoint(const matrix::Vector2f &setpoint); //Timing functions. Necessary to mock time in the tests @@ -168,15 +162,15 @@ class CollisionPrevention : public ModuleParams bool _obstacle_data_present{false}; /**< states if obstacle data is present */ int _setpoint_index{}; /**< index of the setpoint*/ - Vector2f _setpoint_dir{}; /**< direction of the setpoint*/ + matrix::Vector2f _setpoint_dir{}; /**< direction of the setpoint*/ float _closest_dist{}; /**< closest distance to an obstacle */ - Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */ + matrix::Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */ float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ - Vector2f _DEBUG; + matrix::Vector2f _DEBUG; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ @@ -217,15 +211,15 @@ class CollisionPrevention : public ModuleParams * @param curr_pos, current vehicle position * @param curr_vel, current vehicle velocity */ - void _calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, - const Vector2f &curr_vel); + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, + const matrix::Vector2f &curr_vel); /** * Publishes collision_constraints message * @param original_setpoint, setpoint before collision prevention intervention * @param adapted_setpoint, collision prevention adaped setpoint */ - void _publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); + void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); /** * Publishes obstacle_distance message with fused data from offboard and from distance sensors diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 45b01b2a7ed0..a75cd57662d3 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -34,6 +34,8 @@ #include #include "CollisionPrevention.hpp" +using namespace matrix; + // to run: make tests TESTFILTER=CollisionPrevention hrt_abstime mocked_time = 0; @@ -53,15 +55,15 @@ class TestCollisionPrevention : public CollisionPrevention TestCollisionPrevention() : CollisionPrevention(nullptr) {} void paramsChanged() {CollisionPrevention::updateParamsImpl();} obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;} - void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude) + void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &attitude) { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &attitude) { _addObstacleSensorData(obstacle, attitude); } - void test_adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, + void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { _adaptSetpointDirection(setpoint_dir, setpoint_index, vehicle_yaw_angle_rad); @@ -103,8 +105,8 @@ TEST_F(CollisionPreventionTest, noSensorData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -114,7 +116,7 @@ TEST_F(CollisionPreventionTest, noSensorData) param_set(param, &value); cp.paramsChanged(); - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: collision prevention should be enabled and limit the speed to zero @@ -126,9 +128,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -163,7 +165,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) } else { message.distances[i] = 10001; } - } // WHEN: we publish the message and set the parameter and then run the setpoint modification @@ -171,8 +172,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; cp.modifySetpoint(modified_setpoint1, curr_vel); cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -193,9 +194,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -230,8 +231,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); //WHEN: We run the setpoint modification - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; cp.modifySetpoint(modified_setpoint1, curr_vel); cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(distance_sensor_pub); @@ -257,8 +258,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -306,8 +307,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); mocked_time = mocked_time + 100000; //advance time by 0.1 seconds @@ -344,8 +344,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -382,8 +382,7 @@ TEST_F(CollisionPreventionTest, testNoRangeData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); //advance time by 0.1 seconds but no new message comes in @@ -408,8 +407,8 @@ TEST_F(CollisionPreventionTest, noBias) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -434,7 +433,7 @@ TEST_F(CollisionPreventionTest, noBias) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -447,7 +446,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -480,11 +479,10 @@ TEST_F(CollisionPreventionTest, outsideFOV) orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); for (int i = 0; i < distances_array_size; i++) { - float angle_deg = (float)i * message.increment; float angle_rad = math::radians(angle_deg); - matrix::Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; + Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); cp.modifySetpoint(modified_setpoint, curr_vel); @@ -493,9 +491,9 @@ TEST_F(CollisionPreventionTest, outsideFOV) float setpoint_length = modified_setpoint.norm(); if (setpoint_length > 0.f) { - matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length; + Vector2f setpoint_dir = modified_setpoint / setpoint_length; float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)); - float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame)); + float sp_angle_deg = math::degrees(wrap_2pi(sp_angle_body_frame)); EXPECT_GE(sp_angle_deg, 45.f); EXPECT_LE(sp_angle_deg, 225.f); } @@ -508,7 +506,7 @@ TEST_F(CollisionPreventionTest, goNoData) { // GIVEN: a simple setup condition with the initial state (no distance data) TestCollisionPrevention cp; - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: an obstacle message obstacle_distance_s message; @@ -538,8 +536,8 @@ TEST_F(CollisionPreventionTest, goNoData) cp.paramsChanged(); // AND: a setpoint outside the field of view - matrix::Vector2f original_setpoint = {-5, 0}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {-5, 0}; + Vector2f modified_setpoint = original_setpoint; //THEN: the modified setpoint should be zero acceleration cp.modifySetpoint(modified_setpoint, curr_vel); @@ -571,8 +569,8 @@ TEST_F(CollisionPreventionTest, jerkLimit) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: distance set to 5m param_t param = param_handle(px4::params::CP_DIST); @@ -597,7 +595,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) // AND: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; + Vector2f modified_setpoint_default_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_default_jerk, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -608,7 +606,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) cp.paramsChanged(); // WHEN: we run the setpoint modification again - matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint; + Vector2f modified_setpoint_limited_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel); // THEN: the new setpoint should be much slower than the one with default jerk @@ -619,7 +617,7 @@ TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; distance_sensor.max_distance = 20.f; @@ -648,7 +646,7 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; distance_sensor.max_distance = 20.f; @@ -732,13 +730,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw + Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform + Euler attitude2_euler(0, 0, M_PI / 2.0); + Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + Euler attitude3_euler(0, 0, -M_PI / 4.0); + Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + Euler attitude4_euler(0, 0, M_PI); + Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -833,13 +831,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw + Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform + Euler attitude2_euler(0, 0, M_PI / 2.0); + Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + Euler attitude3_euler(0, 0, -M_PI / 4.0); + Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + Euler attitude4_euler(0, 0, M_PI); + Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -934,7 +932,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -991,8 +989,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1004,10 +1002,10 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.distances[2] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); + Vector2f setpoint_dir(1, 0); float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -1038,8 +1036,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1053,10 +1051,10 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.distances[3] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); + Vector2f setpoint_dir(1, 0); float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -1079,8 +1077,8 @@ TEST_F(CollisionPreventionTest, overlappingSensors) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -1130,7 +1128,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the long range measurement @@ -1169,7 +1167,7 @@ TEST_F(CollisionPreventionTest, enterData) // GIVEN: a simple setup condition TestCollisionPrevention cp; cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range From 50ee5bd1b4399f63875af41b6dad45b309bc1ea4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 11 Nov 2024 18:31:39 +0100 Subject: [PATCH 021/211] CollisionPrevention: Sanitize input of _getObstacleDistance() It could cause array out of bound problems before. --- .../CollisionPrevention.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 2bc3b6626664..eaa0c011725c 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -500,17 +500,23 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, con } } -float -CollisionPrevention::_getObstacleDistance(const Vector2f &direction) +float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); - Vector2f dir = direction / direction.norm(); - const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; - const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - - _obstacle_map_body_frame.angle_offset); - int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + const float direction_norm = direction.norm(); + + if (direction_norm > FLT_EPSILON) { + Vector2f dir = direction / direction_norm; + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_with_offset_deg = + wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); + int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + dir_index = math::constrain(dir_index, 0, 71); + return _obstacle_map_body_frame.distances[dir_index] * 0.01f; - return _obstacle_map_body_frame.distances[dir_index] * 0.01f; + } else { + return 0.f; + } } Vector2f From 84dbbb43510d2394942649fa548c680749f2764f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 18:44:09 +0100 Subject: [PATCH 022/211] CollisionPrevention: clarify mode switch command to hold/loiter --- src/lib/collision_prevention/CollisionPrevention.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index eaa0c011725c..8dacd66723e5 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -618,19 +618,16 @@ CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &se void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; - command.timestamp = getTime(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode + command.param1 = 1.f; // base mode VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; command.target_system = 1; command.target_component = 1; command.source_system = 1; command.source_component = 1; command.confirmation = false; command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command + command.timestamp = getTime(); _vehicle_command_pub.publish(command); } From c879ca531dada1eee67282f0b01de2b83e5663ae Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 20:16:36 +0100 Subject: [PATCH 023/211] CollisionPrevention: Clarify bin size definitions, move wrap functions into class --- .../CollisionPrevention.cpp | 101 ++++++++---------- .../CollisionPrevention.hpp | 18 ++-- 2 files changed, 57 insertions(+), 62 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 8dacd66723e5..4f6ff22974a3 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -42,46 +42,24 @@ using namespace matrix; -namespace -{ -static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly -static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG; - -static float wrap_360(float f) -{ - return wrap(f, 0.f, 360.f); -} - -static int wrap_bin(int i) -{ - i = i % INTERNAL_MAP_USED_BINS; - - while (i < 0) { - i += INTERNAL_MAP_USED_BINS; - } - - return i; -} - -} // namespace - CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { - static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5"); - static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly"); + static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5"); + static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly"); + static_assert(sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]) + >= BIN_COUNT, "BIN_COUNT must not overflow obstacle_distance.distances"); // initialize internal obstacle map _obstacle_map_body_frame.timestamp = getTime(); _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - _obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG; + _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; _obstacle_map_body_frame.max_distance = 0; _obstacle_map_body_frame.angle_offset = 0.f; - uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - uint64_t current_time = getTime(); + const uint64_t current_time = getTime(); - for (uint32_t i = 0 ; i < internal_bins; i++) { + for (uint32_t i = 0 ; i < BIN_COUNT_EXTERNAL; i++) { _data_timestamps[i] = current_time; _data_maxranges[i] = 0; _data_fov[i] = 0; @@ -121,9 +99,9 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); + for (int i = 0; i < BIN_COUNT; i++) { + float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset; + msg_index = ceil(_wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { @@ -139,10 +117,10 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, } else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) { // Obstacle message arrives in body frame (front aligned) // corresponding data index (shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + + for (int i = 0; i < BIN_COUNT; i++) { + float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); + msg_index = ceil(_wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { @@ -201,7 +179,7 @@ CollisionPrevention::_checkSetpointDirectionFeasability() { bool setpoint_feasible = true; - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + for (int i = 0; i < BIN_COUNT; i++) { // check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_nodata.get() || (_param_cp_go_nodata.get() && _data_fov[i]))) { @@ -219,9 +197,9 @@ CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); _setpoint_dir = setpoint / setpoint.norm();; const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - vehicle_yaw_angle_rad; - const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - + const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); - _setpoint_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps _adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad); } @@ -280,14 +258,13 @@ void CollisionPrevention::_updateObstacleData() _closest_dist_dir.setZero(); const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - + for (int i = 0; i < BIN_COUNT; i++) { // if the data is stale, reset the bin if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { _obstacle_map_body_frame.distances[i] = UINT16_MAX; } - float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + + float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset)); const Vector2f bin_direction = {cosf(angle), sinf(angle)}; uint bin_distance = _obstacle_map_body_frame.distances[i]; @@ -323,10 +300,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); - int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); + int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); // floor values above zero, ceil values below zero if (lower_bound < 0) { lower_bound++; } @@ -345,7 +320,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrapped_bin = wrap_bin(bin); + int wrapped_bin = _wrap_bin(bin); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); @@ -360,7 +335,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { - const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); + const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE); const int sp_index_original = setpoint_index; float best_cost = 9999.f; int new_sp_index = setpoint_index; @@ -372,7 +347,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float mean_dist = 0; for (int j = i - filter_size; j <= i + filter_size; j++) { - int bin = wrap_bin(j); + int bin = _wrap_bin(j); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { mean_dist += _param_cp_dist.get() * 100.f; @@ -382,7 +357,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi } } - const int bin = wrap_bin(i); + const int bin = _wrap_bin(i); mean_dist = mean_dist / (2.f * filter_size + 1.f); const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original); const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin]; @@ -395,7 +370,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi //only change setpoint direction if it was moved to a different bin if (new_sp_index != setpoint_index) { - float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + float angle = math::radians((float)new_sp_index * BIN_SIZE + _obstacle_map_body_frame.angle_offset); angle = wrap_2pi(vehicle_yaw_angle_rad + angle); setpoint_dir = {cosf(angle), sinf(angle)}; setpoint_index = new_sp_index; @@ -509,8 +484,8 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; const float sp_angle_with_offset_deg = - wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); - int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); + int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); dir_index = math::constrain(dir_index, 0, 71); return _obstacle_map_body_frame.distances[dir_index] * 0.01f; @@ -560,12 +535,12 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic const Vector2f &setpoint_vel, const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir) { - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + for (int i = 0; i < BIN_COUNT; i++) { const float max_range = _data_maxranges[i] * 0.01f; // get the vector pointing into the direction of current bin - float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + - _obstacle_map_body_frame.angle_offset)); + float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset)); const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) }; float bin_distance = _obstacle_map_body_frame.distances[i]; @@ -631,3 +606,19 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter() command.timestamp = getTime(); _vehicle_command_pub.publish(command); } + +float CollisionPrevention::_wrap_360(const float f) +{ + return wrap(f, 0.f, 360.f); +} + +int CollisionPrevention::_wrap_bin(int i) +{ + i = i % BIN_COUNT; + + while (i < 0) { + i += BIN_COUNT; + } + + return i; +} diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index b7720b047e00..bf30c7a4559d 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -81,12 +81,16 @@ class CollisionPrevention : public ModuleParams void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); protected: + static constexpr int BIN_COUNT = 36; + static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly - obstacle_distance_s _obstacle_map_body_frame {}; - bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( - _obstacle_map_body_frame.distances[0])]; /**< in cm */ + obstacle_distance_s _obstacle_map_body_frame{}; + static constexpr int BIN_COUNT_EXTERNAL = + sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); + + bool _data_fov[BIN_COUNT_EXTERNAL]; + uint64_t _data_timestamps[BIN_COUNT_EXTERNAL]; + uint16_t _data_maxranges[BIN_COUNT_EXTERNAL]; /**< in cm */ void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); @@ -154,9 +158,7 @@ class CollisionPrevention : public ModuleParams virtual hrt_abstime getTime(); virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr); - private: - bool _data_stale{true}; /**< states if the data is stale */ bool _was_active{false}; /**< states if the collision prevention interferes with the user input */ bool _obstacle_data_present{false}; /**< states if obstacle data is present */ @@ -242,4 +244,6 @@ class CollisionPrevention : public ModuleParams */ void _publishVehicleCmdDoLoiter(); + static float _wrap_360(const float f); + static int _wrap_bin(int i); }; From 1fa76ac71d603ccf489457d1e7208439e11fc194 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 21:25:09 +0100 Subject: [PATCH 024/211] CollisionPrevention: reduce internal array size, zero initialize what's possible --- src/lib/collision_prevention/CollisionPrevention.cpp | 10 +++------- src/lib/collision_prevention/CollisionPrevention.hpp | 9 +++------ 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 4f6ff22974a3..dd0049f5e96e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -51,18 +51,14 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) : >= BIN_COUNT, "BIN_COUNT must not overflow obstacle_distance.distances"); // initialize internal obstacle map - _obstacle_map_body_frame.timestamp = getTime(); _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; - _obstacle_map_body_frame.max_distance = 0; - _obstacle_map_body_frame.angle_offset = 0.f; - const uint64_t current_time = getTime(); + + static constexpr int BIN_COUNT_EXTERNAL = + sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); for (uint32_t i = 0 ; i < BIN_COUNT_EXTERNAL; i++) { - _data_timestamps[i] = current_time; - _data_maxranges[i] = 0; - _data_fov[i] = 0; _obstacle_map_body_frame.distances[i] = UINT16_MAX; } } diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index bf30c7a4559d..547c63c3aa8a 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -85,12 +85,9 @@ class CollisionPrevention : public ModuleParams static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly obstacle_distance_s _obstacle_map_body_frame{}; - static constexpr int BIN_COUNT_EXTERNAL = - sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - - bool _data_fov[BIN_COUNT_EXTERNAL]; - uint64_t _data_timestamps[BIN_COUNT_EXTERNAL]; - uint16_t _data_maxranges[BIN_COUNT_EXTERNAL]; /**< in cm */ + bool _data_fov[BIN_COUNT] {}; + uint64_t _data_timestamps[BIN_COUNT] {}; + uint16_t _data_maxranges[BIN_COUNT] {}; /**< in cm */ void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); From 001d722abd4f6e8a0f61c396a5650e2262af5681 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 00:06:19 +0100 Subject: [PATCH 025/211] CollisionPrevention: move main functions to the top in the order they get called --- .../CollisionPrevention.cpp | 286 +++++++++--------- .../CollisionPrevention.hpp | 24 +- 2 files changed, 150 insertions(+), 160 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index dd0049f5e96e..5fdd9c801016 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -85,6 +85,147 @@ bool CollisionPrevention::is_active() return activated; } +void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) +{ + //calculate movement constraints based on range data + const Vector2f original_setpoint = setpoint_accel; + _updateObstacleMap(); + _updateObstacleData(); + _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); + + // publish constraints + collision_constraints_s constraints{}; + original_setpoint.copyTo(constraints.original_setpoint); + setpoint_accel.copyTo(constraints.adapted_setpoint); + constraints.timestamp = getTime(); + _constraints_pub.publish(constraints); +} + +void CollisionPrevention::_updateObstacleMap() +{ + _sub_vehicle_attitude.update(); + + // add distance sensor data + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; + + if (dist_sens_sub.update(&distance_sensor)) { + // consider only instances with valid data and orientations useful for collision prevention + if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { + + // update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); + _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, + (uint16_t)(distance_sensor.max_distance * 100.0f)); + _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, + (uint16_t)(distance_sensor.min_distance * 100.0f)); + + _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); + } + } + } + + // add obstacle distance data + if (_sub_obstacle_distance.update()) { + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); + + // Update map with obstacle data if the data is not stale + if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { + //update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); + _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, + obstacle_distance.max_distance); + _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, + obstacle_distance.min_distance); + _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); + } + } + + // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor + _obstacle_distance_pub.publish(_obstacle_map_body_frame); +} + +void CollisionPrevention::_updateObstacleData() +{ + _obstacle_data_present = false; + _closest_dist = UINT16_MAX; + _closest_dist_dir.setZero(); + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + + for (int i = 0; i < BIN_COUNT; i++) { + // if the data is stale, reset the bin + if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * BIN_SIZE + + _obstacle_map_body_frame.angle_offset)); + const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + uint bin_distance = _obstacle_map_body_frame.distances[i]; + + // check if there is avaliable data and the data of the map is not stale + if (bin_distance < UINT16_MAX + && (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { + _obstacle_data_present = true; + } + + if (bin_distance * 0.01f < _closest_dist) { + _closest_dist = bin_distance * 0.01f; + _closest_dist_dir = bin_direction; + } + } +} + +void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) +{ + const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); + const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); + + const float setpoint_length = setpoint_accel.norm(); + _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); + + const hrt_abstime now = getTime(); + + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + Vector2f constr_accel_setpoint{}; + + const bool is_stick_deflected = setpoint_length > 0.001f; + + if (_obstacle_data_present && is_stick_deflected) { + + _transformSetpoint(setpoint_accel); + + _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, + vel_comp_accel, vel_comp_accel_dir); + + if (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); + } + + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; + + } else if (!_obstacle_data_present) + + { + // allow no movement + PX4_WARN("No obstacle data, not moving..."); + setpoint_accel.setZero(); + + // if distance data is stale, switch to Loiter + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US && + getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + } + + _last_timeout_warning = getTime(); + } + } +} + void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude) { @@ -200,84 +341,6 @@ CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) _adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad); } -void -CollisionPrevention::_updateObstacleMap() -{ - _sub_vehicle_attitude.update(); - - // add distance sensor data - for (auto &dist_sens_sub : _distance_sensor_subs) { - distance_sensor_s distance_sensor; - - if (dist_sens_sub.update(&distance_sensor)) { - // consider only instances with valid data and orientations useful for collision prevention - if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { - - // update message description - _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); - _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, - (uint16_t)(distance_sensor.max_distance * 100.0f)); - _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, - (uint16_t)(distance_sensor.min_distance * 100.0f)); - - _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); - } - } - } - - // add obstacle distance data - if (_sub_obstacle_distance.update()) { - const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); - - // Update map with obstacle data if the data is not stale - if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { - //update message description - _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); - _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, - obstacle_distance.max_distance); - _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, - obstacle_distance.min_distance); - _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); - } - } - - // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor - _obstacle_distance_pub.publish(_obstacle_map_body_frame); -} - -void CollisionPrevention::_updateObstacleData() -{ - _obstacle_data_present = false; - _closest_dist = UINT16_MAX; - _closest_dist_dir.setZero(); - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); - - for (int i = 0; i < BIN_COUNT; i++) { - // if the data is stale, reset the bin - if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } - - float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * BIN_SIZE + - _obstacle_map_body_frame.angle_offset)); - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; - uint bin_distance = _obstacle_map_body_frame.distances[i]; - - // check if there is avaliable data and the data of the map is not stale - if (bin_distance < UINT16_MAX - && (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { - _obstacle_data_present = true; - } - - if (bin_distance * 0.01f < _closest_dist) { - _closest_dist = bin_distance * 0.01f; - _closest_dist_dir = bin_direction; - } - } -} - void CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude) { @@ -419,58 +482,6 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist return offset; } -void -CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) -{ - _updateObstacleMap(); - _updateObstacleData(); - - const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); - - const float setpoint_length = setpoint_accel.norm(); - _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); - - const hrt_abstime now = getTime(); - - float vel_comp_accel = INFINITY; - Vector2f vel_comp_accel_dir{}; - Vector2f constr_accel_setpoint{}; - - const bool is_stick_deflected = setpoint_length > 0.001f; - - if (_obstacle_data_present && is_stick_deflected) { - - _transformSetpoint(setpoint_accel); - - _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, - vel_comp_accel, vel_comp_accel_dir); - - if (_checkSetpointDirectionFeasability()) { - constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); - } - - setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; - - } else if (!_obstacle_data_present) - - { - // allow no movement - PX4_WARN("No obstacle data, not moving..."); - setpoint_accel.setZero(); - - // if distance data is stale, switch to Loiter - if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { - if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US && - getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { - _publishVehicleCmdDoLoiter(); - } - - _last_timeout_warning = getTime(); - } - } -} - float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { const float direction_norm = direction.norm(); @@ -571,21 +582,6 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic } } -void -CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) -{ - //calculate movement constraints based on range data - Vector2f original_setpoint = setpoint_accel; - _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); - - // publish constraints - collision_constraints_s constraints{}; - original_setpoint.copyTo(constraints.original_setpoint); - setpoint_accel.copyTo(constraints.adapted_setpoint); - constraints.timestamp = getTime(); - _constraints_pub.publish(constraints); -} - void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 547c63c3aa8a..1c52724d9b70 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -81,6 +81,15 @@ class CollisionPrevention : public ModuleParams void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); protected: + /** Aggregates the sensor data into an internal obstacle map in body frame */ + void _updateObstacleMap(); + + /** Updates the obstacle data based on stale data and calculates values from the map */ + void _updateObstacleData(); + + /** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */ + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + static constexpr int BIN_COUNT = 36; static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly @@ -105,11 +114,6 @@ class CollisionPrevention : public ModuleParams */ void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); - /** - * Calculate the constrained setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint - */ - void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); - /** * Constrain the acceleration setpoint based on the distance to the obstacle * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above @@ -226,16 +230,6 @@ class CollisionPrevention : public ModuleParams */ void _publishObstacleDistance(obstacle_distance_s &obstacle); - /** - * Aggregates the sensor data into a internal obstacle map in body frame - */ - void _updateObstacleMap(); - - /** - * Updates the obstacle data based on stale data and calculates values from the map - */ - void _updateObstacleData(); - /** * Publishes vehicle command. */ From 4c8c5fbb37cb92361411b983b3f542aee4779dc8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 00:56:04 +0100 Subject: [PATCH 026/211] CollisionPrevention: only save quaternion and yaw on attitude update --- .../CollisionPrevention.cpp | 36 +++++------ .../CollisionPrevention.hpp | 8 ++- .../CollisionPreventionTest.cpp | 60 +++++++------------ 3 files changed, 43 insertions(+), 61 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 5fdd9c801016..d5c19b3d09e5 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -87,6 +87,15 @@ bool CollisionPrevention::is_active() void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = Quatf(vehicle_attitude.q); + _vehicle_yaw = Eulerf(_vehicle_attitude).psi(); + } + } + //calculate movement constraints based on range data const Vector2f original_setpoint = setpoint_accel; _updateObstacleMap(); @@ -103,8 +112,6 @@ void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2 void CollisionPrevention::_updateObstacleMap() { - _sub_vehicle_attitude.update(); - // add distance sensor data for (auto &dist_sens_sub : _distance_sensor_subs) { distance_sensor_s distance_sensor; @@ -122,7 +129,7 @@ void CollisionPrevention::_updateObstacleMap() _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, (uint16_t)(distance_sensor.min_distance * 100.0f)); - _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); + _addDistanceSensorData(distance_sensor, _vehicle_attitude); } } } @@ -139,7 +146,7 @@ void CollisionPrevention::_updateObstacleMap() obstacle_distance.max_distance); _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, obstacle_distance.min_distance); - _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); + _addObstacleSensorData(obstacle_distance, _vehicle_yaw); } } @@ -152,7 +159,6 @@ void CollisionPrevention::_updateObstacleData() _obstacle_data_present = false; _closest_dist = UINT16_MAX; _closest_dist_dir.setZero(); - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); for (int i = 0; i < BIN_COUNT; i++) { // if the data is stale, reset the bin @@ -160,7 +166,7 @@ void CollisionPrevention::_updateObstacleData() _obstacle_map_body_frame.distances[i] = UINT16_MAX; } - float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * BIN_SIZE + + float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset)); const Vector2f bin_direction = {cosf(angle), sinf(angle)}; uint bin_distance = _obstacle_map_body_frame.distances[i]; @@ -180,9 +186,6 @@ void CollisionPrevention::_updateObstacleData() void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { - const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); - const float setpoint_length = setpoint_accel.norm(); _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); @@ -198,7 +201,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel _transformSetpoint(setpoint_accel); - _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, vel_comp_accel, vel_comp_accel_dir); if (_checkSetpointDirectionFeasability()) { @@ -226,11 +229,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel } } -void -CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude) +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { int msg_index = 0; - float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); + float vehicle_orientation_deg = math::degrees(vehicle_yaw); float increment_factor = 1.f / obstacle.increment; if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { @@ -331,14 +333,13 @@ CollisionPrevention::_checkSetpointDirectionFeasability() void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); _setpoint_dir = setpoint / setpoint.norm();; - const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps - _adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad); + _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); } void @@ -488,8 +489,7 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) if (direction_norm > FLT_EPSILON) { Vector2f dir = direction / direction_norm; - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); - const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 1c52724d9b70..0023f7716c27 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -104,7 +104,7 @@ class CollisionPrevention : public ModuleParams * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -173,14 +173,16 @@ class CollisionPrevention : public ModuleParams float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ - matrix::Vector2f _DEBUG; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + matrix::Quatf _vehicle_attitude{}; + float _vehicle_yaw{0.f}; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ - uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index a75cd57662d3..955293dad5ef 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -59,9 +59,9 @@ class TestCollisionPrevention : public CollisionPrevention { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - _addObstacleSensorData(obstacle, attitude); + _addObstacleSensorData(obstacle, vehicle_yaw); } void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) @@ -730,14 +730,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - Euler attitude2_euler(0, 0, M_PI / 2.0); - Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - Euler attitude3_euler(0, 0, -M_PI / 4.0); - Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - Euler attitude4_euler(0, 0, M_PI); - Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -754,7 +746,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -771,7 +763,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //WHEN: we add distance sensor data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -787,7 +779,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -803,7 +795,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -831,14 +823,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - Euler attitude2_euler(0, 0, M_PI / 2.0); - Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - Euler attitude3_euler(0, 0, -M_PI / 4.0); - Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - Euler attitude4_euler(0, 0, M_PI); - Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -855,7 +839,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -871,7 +855,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -887,7 +871,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -903,7 +887,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -932,8 +916,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -942,7 +924,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) } //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -961,7 +943,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) //WHEN: we add distance sensor data with an angle offset obstacle_msg.angle_offset = 30.f; - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -989,8 +971,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1003,7 +984,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //define setpoint Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); @@ -1015,8 +996,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); @@ -1036,8 +1017,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1052,7 +1032,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) //define setpoint Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); @@ -1064,8 +1044,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); From b74dd57e7c9d87f81665c7c34927e11a1a9bd51c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:03:52 +0100 Subject: [PATCH 027/211] CollisionPrevention: restore rate limited warning for no data, minor cleanup --- .../CollisionPrevention.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d5c19b3d09e5..e3aff2531093 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -169,7 +169,7 @@ void CollisionPrevention::_updateObstacleData() float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset)); const Vector2f bin_direction = {cosf(angle), sinf(angle)}; - uint bin_distance = _obstacle_map_body_frame.distances[i]; + const uint16_t bin_distance = _obstacle_map_body_frame.distances[i]; // check if there is avaliable data and the data of the map is not stale if (bin_distance < UINT16_MAX @@ -191,30 +191,28 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel const hrt_abstime now = getTime(); - float vel_comp_accel = INFINITY; - Vector2f vel_comp_accel_dir{}; - Vector2f constr_accel_setpoint{}; - const bool is_stick_deflected = setpoint_length > 0.001f; if (_obstacle_data_present && is_stick_deflected) { _transformSetpoint(setpoint_accel); + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, vel_comp_accel, vel_comp_accel_dir); + Vector2f constr_accel_setpoint{}; + if (_checkSetpointDirectionFeasability()) { constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); } setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; - } else if (!_obstacle_data_present) - - { + } else if (!_obstacle_data_present) { // allow no movement - PX4_WARN("No obstacle data, not moving..."); setpoint_accel.setZero(); // if distance data is stale, switch to Loiter @@ -224,7 +222,8 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel _publishVehicleCmdDoLoiter(); } - _last_timeout_warning = getTime(); + PX4_WARN("No obstacle data, not moving..."); + _last_timeout_warning = now; } } } @@ -263,7 +262,6 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; _data_timestamps[i] = _obstacle_map_body_frame.timestamp; @@ -354,8 +352,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } // discard values below min range - if ((distance_reading > distance_sensor.min_distance)) { - + if (distance_reading > distance_sensor.min_distance) { float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); @@ -440,7 +437,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const { - float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f; + float offset = math::max(math::radians(angle_offset), 0.f); switch (distance_sensor.orientation) { case distance_sensor_s::ROTATION_YAW_0: From 2ef2911c3664eb9157e1bfd697d14de50fe4d123 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:30:52 +0100 Subject: [PATCH 028/211] Remove FlightTaskManualPositionSmoothVel The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation. --- .../init.d-posix/airframes/4006_gz_px4vision | 1 - .../flight_mode_manager/CMakeLists.txt | 1 - .../flight_mode_manager/FlightModeManager.cpp | 4 - .../ManualPositionSmoothVel/CMakeLists.txt | 39 ---- .../FlightTaskManualPositionSmoothVel.cpp | 185 ------------------ .../FlightTaskManualPositionSmoothVel.hpp | 92 --------- .../MulticopterPositionControl.hpp | 1 - .../multicopter_position_mode_params.c | 7 +- 8 files changed, 1 insertion(+), 329 deletions(-) delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index c5909381780f..65d290e0cc79 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -75,7 +75,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 81ff6a7a334e..84ccba6ac052 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -47,7 +47,6 @@ list(APPEND flight_tasks_all ManualAltitude ManualAltitudeSmoothVel ManualPosition - ManualPositionSmoothVel Transition ) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 3d7072607358..c58a7fa1572b 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -213,10 +213,6 @@ void FlightModeManager::start_flight_task() error = switchTask(FlightTaskIndex::ManualPosition); break; - case 3: - error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); - break; - case 4: default: if (_param_mpc_pos_mode.get() != 4) { diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt deleted file mode 100644 index 8d4489fa3e55..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 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(FlightTaskManualPositionSmoothVel - FlightTaskManualPositionSmoothVel.cpp -) - -target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility) -target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp deleted file mode 100644 index a89283e14619..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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. - * - ****************************************************************************/ - -#include "FlightTaskManualPositionSmoothVel.hpp" - -#include -#include - -using namespace matrix; - -bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskManualPosition::activate(last_setpoint); - - // Check if the previous FlightTask provided setpoints - Vector3f accel_prev{last_setpoint.acceleration}; - Vector3f vel_prev{last_setpoint.velocity}; - Vector3f pos_prev{last_setpoint.position}; - - for (int i = 0; i < 3; i++) { - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } - } - - _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); - _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); - - return ret; -} - -void FlightTaskManualPositionSmoothVel::reActivate() -{ - FlightTaskManualPosition::reActivate(); - // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly - // using the generated jerk, reset the z derivatives to zero - _smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy()); - _smoothing_z.reset(0.f, 0.f, _position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) -{ - _smoothing_xy.setCurrentPosition(_position.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) -{ - _smoothing_xy.setCurrentVelocity(_velocity.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ(float delta_z) -{ - _smoothing_z.setCurrentPosition(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz) -{ - _smoothing_z.setCurrentVelocity(_velocity(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateSetpoints() -{ - // Set max accel/vel/jerk - // Has to be done before _updateTrajectories() - _updateTrajConstraints(); - _updateTrajVelFeedback(); - _updateTrajCurrentPositionEstimate(); - - // Get yaw setpoint, un-smoothed position setpoints - FlightTaskManualPosition::_updateSetpoints(); - - _updateTrajectories(_velocity_setpoint); - - // Fill the jerk, acceleration, velocity and position setpoint vectors - _setOutputState(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() -{ - _updateTrajConstraintsXY(); - _updateTrajConstraintsZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() -{ - _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy.setMaxVel(_param_mpc_vel_manual.get()); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() -{ - _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); - - _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); - _smoothing_z.setMaxVelUp(_constraints.speed_up); - - _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); - _smoothing_z.setMaxVelDown(_constraints.speed_down); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() -{ - _smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy()); - _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() -{ - _smoothing_xy.setCurrentPositionEstimate(_position.xy()); - _smoothing_z.setCurrentPositionEstimate(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) -{ - _smoothing_xy.update(_deltatime, vel_target.xy()); - _smoothing_z.update(_deltatime, vel_target(2)); -} - -void FlightTaskManualPositionSmoothVel::_setOutputState() -{ - _setOutputStateXY(); - _setOutputStateZ(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateXY() -{ - _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); - _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); - _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateZ() -{ - _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); - _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - - if (!_terrain_hold) { - if (_terrain_hold_previous) { - // Reset position setpoint to current position when switching from terrain hold to non-terrain hold - _smoothing_z.setCurrentPosition(_position(2)); - } - - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); - } - - _terrain_hold_previous = _terrain_hold; -} diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp deleted file mode 100644 index c8fbdf42c95a..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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 FlightTaskManualPositionSmoothVel.hpp - * - * Flight task for smooth manual controlled position. - */ - -#pragma once - -#include "FlightTaskManualPosition.hpp" -#include -#include - -using matrix::Vector2f; -using matrix::Vector3f; - -class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition -{ -public: - FlightTaskManualPositionSmoothVel() = default; - virtual ~FlightTaskManualPositionSmoothVel() = default; - - bool activate(const trajectory_setpoint_s &last_setpoint) override; - void reActivate() override; - -protected: - - virtual void _updateSetpoints() override; - - /** Reset position or velocity setpoints in case of EKF reset event */ - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - void _ekfResetHandlerPositionZ(float delta_z) override; - void _ekfResetHandlerVelocityZ(float delta_vz) override; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ) - -private: - void _updateTrajConstraints(); - void _updateTrajConstraintsXY(); - void _updateTrajConstraintsZ(); - - void _updateTrajVelFeedback(); - void _updateTrajCurrentPositionEstimate(); - - void _updateTrajectories(Vector3f vel_target); - - void _setOutputState(); - void _setOutputStateXY(); - void _setOutputStateZ(); - - ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions - ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction - - bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ -}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index f7cd9bbbe09a..6d58b85cd7ad 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -167,7 +167,6 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_vel_man_side, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index a65ef4f5713d..9fce344319c8 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -39,14 +39,10 @@ * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * - "Smoothed velocity": - * Sticks map to velocity but with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). * - "Acceleration based": * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity - * @value 3 Smoothed velocity * @value 4 Acceleration based * @group Multicopter Position Control */ @@ -104,7 +100,6 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); * * MPC_POS_MODE * 1 just deceleration - * 3 acceleration and deceleration * 4 not used, use MPC_ACC_HOR instead * * @unit m/s^2 @@ -125,7 +120,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. + * Only used with MPC_POS_MODE Acceleration based. * * @unit m/s^3 * @min 0.5 From f41a08aea867c15b369e7467fec9b6ff1edcd324 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Thu, 14 Nov 2024 14:32:59 +0100 Subject: [PATCH 029/211] CollisionPrevention: changed to resolution of 5 degrees, and adapted tests to reflect the change rewrite of obstacle_distance merging methods, and fix of various issues --- .../CollisionPrevention.cpp | 103 +++-- .../CollisionPrevention.hpp | 8 +- .../CollisionPreventionTest.cpp | 358 +++++++++++++----- src/modules/simulation/gz_bridge/GZBridge.cpp | 2 +- 4 files changed, 341 insertions(+), 130 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index e3aff2531093..324be7357f40 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -151,7 +151,7 @@ void CollisionPrevention::_updateObstacleMap() } // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor - _obstacle_distance_pub.publish(_obstacle_map_body_frame); + _obstacle_distance_fused_pub.publish(_obstacle_map_body_frame); } void CollisionPrevention::_updateObstacleData() @@ -228,27 +228,51 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel } } +// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed... void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - int msg_index = 0; + float vehicle_orientation_deg = math::degrees(vehicle_yaw); - float increment_factor = 1.f / obstacle.increment; + if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { - float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(_wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); - - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + for (int j = 0; j < 360 / obstacle.increment; j++) { + float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + - (float)_obstacle_map_body_frame.increment / 2.f); + float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + + (float)_obstacle_map_body_frame.increment / 2.f); + float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg - + obstacle.increment / 2.f); + float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg + + obstacle.increment / 2.f); + + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; + } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } @@ -256,18 +280,39 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // Obstacle message arrives in body frame (front aligned) // corresponding data index (shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { - float bin_angle_deg = (float)i * BIN_SIZE + - _obstacle_map_body_frame.angle_offset; - msg_index = ceil(_wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); - - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + for (int j = 0; j < 360 / obstacle.increment; j++) { + float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + - (float)_obstacle_map_body_frame.increment / 2.f); + float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + + (float)_obstacle_map_body_frame.increment / 2.f); + float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f); + float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f); + + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; + } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } @@ -357,18 +402,14 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - - // floor values above zero, ceil values below zero - if (lower_bound < 0) { lower_bound++; } + int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - if (upper_bound < 0) { upper_bound++; } // rotate vehicle attitude into the sensor body frame Quatf attitude_sensor_frame = vehicle_attitude; attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); - float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); + float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 0023f7716c27..80500ecc34ef 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -80,6 +80,9 @@ class CollisionPrevention : public ModuleParams */ void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + static constexpr int BIN_COUNT = 72; + static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly + protected: /** Aggregates the sensor data into an internal obstacle map in body frame */ void _updateObstacleMap(); @@ -90,9 +93,6 @@ class CollisionPrevention : public ModuleParams /** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */ void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); - static constexpr int BIN_COUNT = 36; - static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly - obstacle_distance_s _obstacle_map_body_frame{}; bool _data_fov[BIN_COUNT] {}; uint64_t _data_timestamps[BIN_COUNT] {}; @@ -179,7 +179,7 @@ class CollisionPrevention : public ModuleParams float _vehicle_yaw{0.f}; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ + uORB::Publication _obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 955293dad5ef..aa98ef821d0a 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -38,6 +38,8 @@ using namespace matrix; // to run: make tests TESTFILTER=CollisionPrevention hrt_abstime mocked_time = 0; +const uint bin_size = CollisionPrevention::BIN_SIZE; +const uint bin_count = CollisionPrevention::BIN_COUNT; class CollisionPreventionTest : public ::testing::Test { @@ -246,6 +248,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); @@ -609,14 +612,14 @@ TEST_F(CollisionPreventionTest, jerkLimit) Vector2f modified_setpoint_limited_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel); - // THEN: the new setpoint should be much slower than the one with default jerk - EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm()); + // THEN: the new setpoint should be much higher than the one with default jerk, as the rate of change in acceleration is more limmited + EXPECT_GT(modified_setpoint_limited_jerk.norm(), modified_setpoint_default_jerk.norm()); + } TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) { // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; @@ -641,11 +644,65 @@ TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) } } +TEST_F(CollisionPreventionTest, addDistanceSensorDataNarrow) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(0.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } +} +TEST_F(CollisionPreventionTest, addDistanceSensorDataSlightlyLarger) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(1.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the the bins corresponding to -5°, 0° and 5° should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 71 || i <= 1) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } +} + TEST_F(CollisionPreventionTest, addDistanceSensorData) { // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; @@ -656,21 +713,24 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (uint32_t i = 0; i < distances_array_size; i++) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //WHEN: we add distance sensor data to the right distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING; distance_sensor.h_fov = math::radians(19.99f); cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + uint fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start = (90 - fov) / bin_size; + uint end = (90 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -679,17 +739,20 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(50.f); distance_sensor.current_distance = 8.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start2 = (270 - fov) / bin_size; + uint end2 = (270 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -698,20 +761,23 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(10.1f); distance_sensor.current_distance = 3.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start3 = (360 - fov) / bin_size; + uint end3 = (fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; - } else if (i == 0) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300); + } else if (i >= start3 || i <= end3) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -722,7 +788,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 5.f; @@ -733,11 +798,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 2; i < 6 ; i++) { + int start = 2; + int end = 6; + + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -750,11 +819,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -762,44 +831,50 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } - //WHEN: we add distance sensor data while vehicle yaw 90deg to the right + //WHEN: we add obstacle distance sensor data while vehicle yaw 90deg to the right cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled + int offset = bin_count - 90 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 28 || i == 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 45deg to the left + //WHEN: we add obstacle distance sensor data while vehicle yaw 45deg to the left cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled + offset = 45 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 6 || i == 7) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 180deg + //WHEN: we add obstacle distance sensor data while vehicle yaw 180deg cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled + offset = 180 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 19 || i == 20) { + if (i >= offset + start && i <= offset + end) { EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); } else { @@ -811,11 +886,84 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } } +TEST_F(CollisionPreventionTest, addObstacleSensorData_offset_bodyframe) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; // Body Frame + obstacle_msg.increment = 6.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; + + //obstacle at 363°-39° deg world frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment + obstacle_msg.distances[i] = 500; + } + + //WHEN: we add distance sensor data + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° + for (int i = 0; i < distances_array_size; i++) { + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } +} + TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned obstacle_msg.increment = 5.f; @@ -825,8 +973,10 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + int start = 2; + int end = 6; - for (int i = 2; i < 6 ; i++) { + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } @@ -842,12 +992,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -859,11 +1010,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -875,11 +1026,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -891,16 +1042,17 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } + } @@ -908,7 +1060,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 6.f; @@ -916,42 +1067,62 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - //obstacle at 0-30 deg world frame, distance 5 meters + //obstacle at 363°-39° deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 5 ; i++) { + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment obstacle_msg.distances[i] = 500; } //WHEN: we add distance sensor data cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (int i = 0; i < distances_array_size; i++) { - if (i >= 0 && i <= 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data with an angle offset - obstacle_msg.angle_offset = 30.f; + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° for (int i = 0; i < distances_array_size; i++) { - if (i >= 3 && i <= 5) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -963,10 +1134,9 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; @@ -976,7 +1146,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 7 ; i++) { + for (int i = 0; i <= 6 ; i++) { obstacle_msg.distances[i] = 500; } @@ -1001,18 +1171,17 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; @@ -1049,8 +1218,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262f); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012f); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, overlappingSensors) @@ -1146,16 +1315,15 @@ TEST_F(CollisionPreventionTest, enterData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with a valid reading distance_sensor_s distance_sensor {}; @@ -1167,36 +1335,38 @@ TEST_F(CollisionPreventionTest, enterData) cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(500, cp.getObstacleMap().distances[8]); - EXPECT_EQ(500, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(500, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain valid readings // a valid reading should only be accepted from sensors with shorter or equal range // a out of range reading should only be accepted from sensors with the same range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with an out of range reading distance_sensor.current_distance = 21.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(2000, cp.getObstacleMap().distances[8]); - EXPECT_EQ(2000, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(2000, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain readings out of range // a reading in range will be accepted in any case // out of range readings will only be accepted from sensors with bigger or equal range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range } diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 77858bb96452..95be696835e2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -816,7 +816,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) { - static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each + static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors double angle_min_deg = scan.angle_min() * 180 / M_PI; double angle_step_deg = scan.angle_step() * 180 / M_PI; From 61d999073b306aa96f84d1622ff0e968724ac88b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:30:52 +0100 Subject: [PATCH 030/211] Remove FlightTaskManualPositionSmoothVel The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation. --- ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision | 1 - ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 | 1 - 2 files changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index 090415e5ec49..d60de46e7bea 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -67,7 +67,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index a73c357437f4..c9c3e5843ac7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -68,7 +68,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters From 399a8ad5b7592be77d9da3fd64ff34c79c302726 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 17:09:46 +0100 Subject: [PATCH 031/211] CollisionPrevention: fix early return and use unified bin count + array size 72 --- .../CollisionPrevention.cpp | 17 ++++++----------- .../CollisionPrevention.hpp | 3 ++- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 324be7357f40..801e29e34da7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -47,18 +47,13 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) : { static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5"); static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly"); - static_assert(sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]) - >= BIN_COUNT, "BIN_COUNT must not overflow obstacle_distance.distances"); // initialize internal obstacle map _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; - static constexpr int BIN_COUNT_EXTERNAL = - sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - - for (uint32_t i = 0 ; i < BIN_COUNT_EXTERNAL; i++) { + for (uint32_t i = 0 ; i < BIN_COUNT; i++) { _obstacle_map_body_frame.distances[i] = UINT16_MAX; } } @@ -523,6 +518,7 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { + float obstacle_distance = 0.f; const float direction_norm = direction.norm(); if (direction_norm > FLT_EPSILON) { @@ -531,12 +527,11 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); - dir_index = math::constrain(dir_index, 0, 71); - return _obstacle_map_body_frame.distances[dir_index] * 0.01f; - - } else { - return 0.f; + dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1); + obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; } + + return obstacle_distance; } Vector2f diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 80500ecc34ef..176bcf46cda6 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -80,7 +80,8 @@ class CollisionPrevention : public ModuleParams */ void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); - static constexpr int BIN_COUNT = 72; + static constexpr int BIN_COUNT = + sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); // 72 static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly protected: From 30eec33e09a70b763e0df1d0e97383b8253bdf7e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Nov 2024 19:59:09 +0100 Subject: [PATCH 032/211] CollisionPrevention: slightly simplify _transformSetpoint() --- src/lib/collision_prevention/CollisionPrevention.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 801e29e34da7..534426a30475 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -371,12 +371,12 @@ CollisionPrevention::_checkSetpointDirectionFeasability() void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { - _setpoint_dir = setpoint / setpoint.norm();; - const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - _vehicle_yaw; + const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps + _setpoint_dir = setpoint.unit_or_zero(); _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); } From 1410325c62f18af00c5e549dbe3feb9b33e33fab Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Nov 2024 20:42:12 +0100 Subject: [PATCH 033/211] CollisionPrevention: follow parameter variable naming convention --- src/lib/collision_prevention/CollisionPrevention.cpp | 6 +++--- src/lib/collision_prevention/CollisionPrevention.hpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 534426a30475..7d34097644f2 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -358,8 +358,8 @@ CollisionPrevention::_checkSetpointDirectionFeasability() for (int i = 0; i < BIN_COUNT; i++) { // check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data - if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_nodata.get() - || (_param_cp_go_nodata.get() && _data_fov[i]))) { + if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get() + || (_param_cp_go_no_data.get() && _data_fov[i]))) { setpoint_feasible = false; } @@ -605,7 +605,7 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), _param_mpc_acc_hor.get(), stop_distance, 0.f); // we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down. - const float curr_acc_vel_constraint = _param_mpc_vel_p_acc.get() * (max_vel - curr_vel_parallel); + const float curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel); if (curr_acc_vel_constraint < vel_comp_accel) { vel_comp_accel = curr_acc_vel_constraint; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 176bcf46cda6..8fcc7c28f3ec 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -196,11 +196,11 @@ class CollisionPrevention : public ModuleParams (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ - (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ + (ParamBool) _param_cp_go_no_data, /**< movement allowed where no data*/ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ - (ParamFloat) _param_mpc_vel_p_acc, /**< p gain from velocity controller*/ + (ParamFloat) _param_mpc_xy_vel_p_acc, /**< p gain from velocity controller*/ (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) From bbc59dcde7698eb08fed80fad0ea7d03e4273816 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Nov 2024 10:47:02 +0100 Subject: [PATCH 034/211] CollisionPrevention: prevent illegal array index with malicious obstacle_dsitance message --- src/lib/collision_prevention/CollisionPrevention.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 7d34097644f2..691f13c4dffb 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -234,7 +234,7 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { - for (int j = 0; j < 360 / obstacle.increment; j++) { + for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) { float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - (float)_obstacle_map_body_frame.increment / 2.f); float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset From db13b9cb50c9af7ba61dede7795fd898bf346c84 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 20 Nov 2024 16:39:18 +0100 Subject: [PATCH 035/211] CollisionPrevention: Added Case where velocity gets reduced to zero if we are closer to the obstacle than the minimal distance --- .../collision_prevention/CollisionPrevention.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 691f13c4dffb..0733f1b289d4 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -600,12 +600,18 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic delay_distance += curr_vel_parallel * (data_age * 1e-6f); } - const float stop_distance = math::max(0.f, distance - _min_dist_to_keep - delay_distance); + const float stop_distance = distance - _min_dist_to_keep - delay_distance; - const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), - _param_mpc_acc_hor.get(), stop_distance, 0.f); - // we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down. - const float curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel); + float curr_acc_vel_constraint; + + if (stop_distance >= 0.f) { + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f); + + } else { + curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel; + } if (curr_acc_vel_constraint < vel_comp_accel) { vel_comp_accel = curr_acc_vel_constraint; From 044d13635d02b9397ffc9c1200c556570d833228 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Nov 2024 18:40:12 +0100 Subject: [PATCH 036/211] Commander: Change user facing messages to "Remote ID" instead of "OpenDroneID" Apprently users reference the system like that and OpenDroneID might already be too technical, less understandable to some. --- src/modules/commander/Commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cc1393b966f0..642973dda411 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2799,8 +2799,8 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = false; if (_datalink_last_heartbeat_open_drone_id_system != 0) { - mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t"); - events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained"); + mavlink_log_info(&_mavlink_log_pub, "Remote ID system regained\t"); + events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "Remote ID system regained"); } } @@ -2861,11 +2861,11 @@ void Commander::dataLinkCheck() _status_changed = true; } - // OpenDroneID system + // Remote ID system if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s) && !_open_drone_id_system_lost) { - mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost"); - events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost"); + mavlink_log_critical(&_mavlink_log_pub, "Remote ID system lost"); + events::send(events::ID("commander_remote_id_lost"), events::Log::Critical, "Remote ID system lost"); _vehicle_status.open_drone_id_system_present = false; _vehicle_status.open_drone_id_system_healthy = false; _open_drone_id_system_lost = true; From be300b767d779f237a6448ea980a503eae06d6a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 19 Nov 2024 11:37:35 +0100 Subject: [PATCH 037/211] commander: ensure health report is always sent out before failsafe notificaation As the failsafe message can reference the health report, the health report needs to be sent out first. This is generally the case, except there is a rate limiter set to 2 seconds. So if the report changes quickly, it is sent out delayed (potentially after the failsafe report). --- src/modules/commander/Commander.cpp | 16 ++++++++++++++++ src/modules/commander/Commander.hpp | 3 +++ .../commander/HealthAndArmingChecks/Common.cpp | 9 +++++++++ .../commander/HealthAndArmingChecks/Common.hpp | 5 +++++ .../HealthAndArmingChecks.cpp | 5 +++++ .../HealthAndArmingChecks.hpp | 2 ++ src/modules/commander/failsafe/framework.cpp | 4 ++++ src/modules/commander/failsafe/framework.h | 14 ++++++++++++++ 8 files changed, 58 insertions(+) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 642973dda411..8e41b2ac56df 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -716,6 +716,8 @@ Commander::Commander() : } updateParameters(); + + _failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this); } Commander::~Commander() @@ -3000,6 +3002,20 @@ void Commander::send_parachute_command() set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } +void Commander::onFailsafeNotifyUserTrampoline(void *arg) +{ + Commander *commander = static_cast(arg); + commander->onFailsafeNotifyUser(); +} + +void Commander::onFailsafeNotifyUser() +{ + // If we are about to inform about a failsafe, we need to ensure any pending health report is sent out first, + // as the failsafe message might reference that. This is only needed in case the report is currently rate-limited, + // i.e. it had a recent previous change already. + _health_and_arming_checks.reportIfUnreportedDifferences(); +} + int Commander::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6dddf0777f4d..e5bdb947aacb 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -200,6 +200,9 @@ class Commander : public ModuleBase, public ModuleParams void modeManagementUpdate(); + static void onFailsafeNotifyUserTrampoline(void *arg); + void onFailsafeNotifyUser(); + enum class PrearmedMode { DISABLED = 0, SAFETY_BUTTON = 1, diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 796427180fee..f8d3982c5be9 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -317,3 +317,12 @@ bool Report::report(bool is_armed, bool force) current_results.health.error, current_results.health.warning); return true; } + +bool Report::reportIfUnreportedDifferences(bool is_armed) +{ + if (_had_unreported_difference) { + return report(is_armed, true); + } + + return false; +} diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 7fb454cbeb7c..3e6a06248d7e 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -339,6 +339,11 @@ class Report bool report(bool is_armed, bool force); + /** + * Send out any unreported changes if there are any + */ + bool reportIfUnreportedDifferences(bool is_armed); + const hrt_abstime _min_reporting_interval; /// event buffer: stores current events + arguments. diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index dbfea11ce881..2ab153ad8a05 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -120,3 +120,8 @@ void HealthAndArmingChecks::updateParams() _checks[i]->updateParams(); } } + +bool HealthAndArmingChecks::reportIfUnreportedDifferences() +{ + return _reporter.reportIfUnreportedDifferences(_context.isArmed()); +} diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index f0dc8404f29f..d04b8d088ec9 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -88,6 +88,8 @@ class HealthAndArmingChecks : public ModuleParams */ bool update(bool force_reporting = false, bool is_arming_request = false); + bool reportIfUnreportedDifferences(); + /** * Whether arming is possible for a given navigation mode */ diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 6c0edb58c459..8d633e9f215b 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -170,6 +170,10 @@ void FailsafeBase::removeActions(ClearCondition condition) void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause) { + if (_on_notify_user_cb) { + _on_notify_user_cb(_on_notify_user_arg); + } + int delay_s = (_current_delay + 500_ms) / 1_s; PX4_DEBUG("User notification: failsafe triggered (action=%i, delayed_action=%i, cause=%i, delay=%is)", (int)action, (int)delayed_action, (int)cause, delay_s); diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 4cd3ac5202c5..a0ff26601bec 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -165,6 +165,17 @@ class FailsafeBase: public ModuleParams bool getDeferFailsafes() const { return _defer_failsafes; } bool failsafeDeferred() const { return _failsafe_defer_started != 0; } + using UserCallback = void(*)(void *); + + /** + * Register a callback that is called before notifying the user. + */ + void setOnNotifyUserCallback(UserCallback callback, void *arg) + { + _on_notify_user_cb = callback; + _on_notify_user_arg = arg; + } + protected: enum class UserTakeoverAllowed { Always, ///< allow takeover (immediately) @@ -278,6 +289,9 @@ class FailsafeBase: public ModuleParams orb_advert_t _mavlink_log_pub{nullptr}; + UserCallback _on_notify_user_cb{nullptr}; + void *_on_notify_user_arg{nullptr}; + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamFloat) _param_com_fail_act_t ); From 8ccd43dc1bd5fa3a3172c0d34b8d6e542f45b176 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 20 Nov 2024 14:37:35 +0100 Subject: [PATCH 038/211] refactor commander: remove unused argument is_armed in Report class --- .../commander/HealthAndArmingChecks/Common.cpp | 6 +++--- .../commander/HealthAndArmingChecks/Common.hpp | 4 ++-- .../HealthAndArmingChecks.cpp | 6 +++--- .../HealthAndArmingChecksTest.cpp | 14 +++++++------- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index f8d3982c5be9..fb5639087ea1 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -219,7 +219,7 @@ bool Report::finalize() return _results_changed; } -bool Report::report(bool is_armed, bool force) +bool Report::report(bool force) { const hrt_abstime now = hrt_absolute_time(); const bool has_difference = _had_unreported_difference || _results_changed; @@ -318,10 +318,10 @@ bool Report::report(bool is_armed, bool force) return true; } -bool Report::reportIfUnreportedDifferences(bool is_armed) +bool Report::reportIfUnreportedDifferences() { if (_had_unreported_difference) { - return report(is_armed, true); + return report(true); } return false; diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 3e6a06248d7e..826aa7629c0e 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -337,12 +337,12 @@ class Report */ bool finalize(); - bool report(bool is_armed, bool force); + bool report(bool force); /** * Send out any unreported changes if there are any */ - bool reportIfUnreportedDifferences(bool is_armed); + bool reportIfUnreportedDifferences(); const hrt_abstime _min_reporting_interval; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index 2ab153ad8a05..f4f91d045057 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -67,7 +67,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request) } const bool results_changed = _reporter.finalize(); - const bool reported = _reporter.report(_context.isArmed(), force_reporting); + const bool reported = _reporter.report(force_reporting); if (reported) { @@ -89,7 +89,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request) } _reporter.finalize(); - _reporter.report(_context.isArmed(), false); + _reporter.report(false); _reporter._mavlink_log_pub = nullptr; // LEGACY end @@ -123,5 +123,5 @@ void HealthAndArmingChecks::updateParams() bool HealthAndArmingChecks::reportIfUnreportedDifferences() { - return _reporter.reportIfUnreportedDifferences(_context.isArmed()); + return _reporter.reportIfUnreportedDifferences(); } diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index c06886320563..4bd7749ce435 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -69,7 +69,7 @@ TEST_F(ReporterTest, basic_no_checks) reporter.reset(); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, 0xff); @@ -92,7 +92,7 @@ TEST_F(ReporterTest, basic_fail_all_modes) reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, events::ID("arming_test_basic_fail_all_modes_fail1"), events::Log::Info, ""); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(nav_state)); ASSERT_TRUE(reporter.canRun(nav_state)); @@ -113,7 +113,7 @@ TEST_F(ReporterTest, arming_checks_mode_category) events::ID("arming_test_arming_checks_mode_category_fail2"), events::Log::Info, ""); reporter.setIsPresent(health_component_t::battery); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); ASSERT_TRUE(reporter.canRun(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); @@ -138,7 +138,7 @@ TEST_F(ReporterTest, arming_checks_mode_category2) reporter.healthFailure(NavModes::Mission, health_component_t::remote_control, events::ID("arming_test_arming_checks_mode_category2_fail1"), events::Log::Warning, ""); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); @@ -178,7 +178,7 @@ TEST_F(ReporterTest, reporting) } reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); if (i == 0) { @@ -219,7 +219,7 @@ TEST_F(ReporterTest, reporting) } reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); if (i == 0) { @@ -265,7 +265,7 @@ TEST_F(ReporterTest, reporting_multiple) reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, events::ID("arming_test_reporting_multiple_fail3"), events::Log::Warning, "", 55); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); if (i == 0) { From 10590fc597c7f8f10feffe5282313c96fa8c518c Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Thu, 21 Nov 2024 12:38:53 +0000 Subject: [PATCH 039/211] Update submodule libfc-sensor-api to latest Thu Nov 21 12:38:53 UTC 2024 - libfc-sensor-api in PX4/Firmware (044d13635d02b9397ffc9c1200c556570d833228): https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api/commit/ca16e99074641a10d153961291243ede7720c2e2 - libfc-sensor-api current upstream: https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api/commit/85151aaf6ba8b24ce82b387e088452c63f7e2096 - Changes: https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api/compare/ca16e99074641a10d153961291243ede7720c2e2...85151aaf6ba8b24ce82b387e088452c63f7e2096 85151aa 2024-06-05 Eric Katzfey - Revert addition on slpi link interface. It will be moved to a separate project e36286b 2024-06-04 Eric Katzfey - Merge branch 'master' of gitlab.com:voxl-public/voxl-sdk/core-libs/libfc-sensor-api 43a513f 2024-06-04 Eric Katzfey - Added new slpi link interface 490608b 2024-05-08 Eric Katzfey - Fixed gitignore for shared object 1085c02 2024-05-07 Eric Katzfey - Added gitignore for object files 37ecfc4 2024-05-03 Eric Katzfey - v1.0.3 2d1ebd3 2024-05-02 Eric Katzfey - Added a new kill slpi function --- boards/modalai/voxl2/libfc-sensor-api | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/modalai/voxl2/libfc-sensor-api b/boards/modalai/voxl2/libfc-sensor-api index ca16e9907464..85151aaf6ba8 160000 --- a/boards/modalai/voxl2/libfc-sensor-api +++ b/boards/modalai/voxl2/libfc-sensor-api @@ -1 +1 @@ -Subproject commit ca16e99074641a10d153961291243ede7720c2e2 +Subproject commit 85151aaf6ba8b24ce82b387e088452c63f7e2096 From 746e26bf9e218b185783c6d88c0c759e65ee868c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2024 13:27:21 +1300 Subject: [PATCH 040/211] listener: clear screen on multiple output I suggest to clear the screen when using the listener with -n. This way the updates don't just scroll by which isn't legible but instead stay in place and update, similar to the top command. --- src/systemcmds/topic_listener/listener_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index b4db038c1254..83ca22ddd710 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -113,6 +113,9 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, fds[1].fd = sub; fds[1].events = POLLIN; + // Clear screen + dprintf(1, "\033[2J\n"); + while (msgs_received < num_msgs) { if (poll(&fds[0], 2, int(MESSAGE_TIMEOUT_S * 1000)) > 0) { @@ -139,6 +142,8 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, if (fds[1].revents & POLLIN) { msgs_received++; + // Move cursor to home position + dprintf(1, "\033[H"); PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received); int ret = listener_print_topic(id, sub); From 2f65644aeb5cb4c2023e43ac45d8a9b01d0584ba Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 21 Nov 2024 11:53:07 +0100 Subject: [PATCH 041/211] mixer module: remove unused functions Signed-off-by: Silvan Fuhrer --- src/lib/mixer_module/mixer_module.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 40d7033107fc..ff400ca6d59f 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -170,8 +170,6 @@ class MixingOutput : public ModuleParams void setAllMinValues(uint16_t value); void setAllMaxValues(uint16_t value); - uint16_t &reverseOutputMask() { return _reverse_output_mask; } - uint16_t &failsafeValue(int index) { return _failsafe_value[index]; } /** Disarmed values: disarmedValue < minValue needs to hold */ uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } uint16_t &minValue(int index) { return _min_value[index]; } From 233cf17fa79feb528e3992ae24a178bc223602e6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 21 Nov 2024 12:10:42 -0500 Subject: [PATCH 042/211] Jenkins: delete Jenkinsfile-compile/Jenkinsfile-hardware - Jenkinsfile-compile is now redundant with github actions - Jenkinsfile-hardware will be migrated to github actions once new test rack hardware is available (old test rack server died) --- .ci/Jenkinsfile-compile | 233 ---------- .ci/Jenkinsfile-hardware | 904 --------------------------------------- 2 files changed, 1137 deletions(-) delete mode 100644 .ci/Jenkinsfile-compile delete mode 100644 .ci/Jenkinsfile-hardware diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile deleted file mode 100644 index 9a096301cdd7..000000000000 --- a/.ci/Jenkinsfile-compile +++ /dev/null @@ -1,233 +0,0 @@ -#!/usr/bin/env groovy - -pipeline { - agent none - stages { - - stage('Build') { - steps { - script { - def build_nodes = [:] - def docker_images = [ - armhf: "px4io/px4-dev-armhf:2023-06-26", - arm64: "px4io/px4-dev-aarch64:2022-08-12", - base: "px4io/px4-dev-ros2-foxy:2022-08-12", - nuttx: "px4io/px4-dev-nuttx-focal:2022-08-12", - ] - - def armhf_builds = [ - target: ["beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default", "scumaker_pilotpi_default"], - image: docker_images.armhf, - archive: false - ] - - def arm64_builds = [ - target: ["scumaker_pilotpi_arm64"], - image: docker_images.arm64, - archive: false - ] - - def base_builds = [ - target: ["px4_sitl_default"], - image: docker_images.base, - archive: false - ] - - def nuttx_builds_archive = [ - target: [ - "3dr_ctrl-zero-h7-oem-revg_default", - "airmind_mindpx-v2_default", - "ark_can-flow_canbootloader", - "ark_can-flow_default", - "ark_can-gps_canbootloader", - "ark_can-gps_default", - "ark_can-rtk-gps_canbootloader", - "ark_can-rtk-gps_default", - "ark_cannode_canbootloader", - "ark_cannode_default", - "ark_fmu-v6x_bootloader", - "ark_fmu-v6x_default", - "ark_fpv_bootloader", - "ark_fpv_default", - "ark_pi6x_bootloader", - "ark_pi6x_default", - "atl_mantis-edu_default", - "av_x-v1_default", - "bitcraze_crazyflie21_default", - "bitcraze_crazyflie_default", - "cuav_7-nano_default", - "cuav_can-gps-v1_canbootloader", - "cuav_can-gps-v1_default", - "cuav_nora_default", - "cuav_x7pro_default", - "cubepilot_cubeorange_default", - "cubepilot_cubeorangeplus_default", - "cubepilot_cubeyellow_default", - "diatone_mamba-f405-mk2_default", - "flywoo_gn-f405_default", - "freefly_can-rtk-gps_canbootloader", - "freefly_can-rtk-gps_default", - "holybro_can-gps-v1_canbootloader", - "holybro_can-gps-v1_default", - "holybro_durandal-v1_default", - "holybro_kakutef7_default", - "holybro_kakuteh7_default", - "holybro_kakuteh7mini_default", - "holybro_kakuteh7v2_default", - "holybro_pix32v5_default", - "matek_gnss-m9n-f4_canbootloader", - "matek_gnss-m9n-f4_default", - "matek_h743-mini_default", - "matek_h743-slim_default", - "matek_h743_default", - "micoair_h743_default", - "micoair_h743-aio_default", - "modalai_fc-v1_default", - "modalai_fc-v2_default", - "mro_ctrl-zero-classic_default", - "mro_ctrl-zero-f7-oem_default", - "mro_ctrl-zero-f7_default", - "mro_ctrl-zero-h7-oem_default", - "mro_ctrl-zero-h7_default", - "mro_pixracerpro_default", - "mro_x21-777_default", - "mro_x21_default", - "nxp_fmuk66-e_default", - "nxp_fmuk66-e_socketcan", - "nxp_fmuk66-v3_default", - "nxp_fmuk66-v3_socketcan", - "nxp_mr-canhubk3_default", - "nxp_mr-canhubk3_fmu", - "nxp_ucans32k146_canbootloader", - "nxp_ucans32k146_default", - "nxp_tropic-community_default", - "omnibus_f4sd_default", - "px4_fmu-v2_default", - "px4_fmu-v2_fixedwing", - "px4_fmu-v2_lto", - "px4_fmu-v2_multicopter", - "px4_fmu-v2_rover", - "px4_fmu-v3_default", - "px4_fmu-v4_default", - "px4_fmu-v4pro_default", - "px4_fmu-v5_cyphal", - "px4_fmu-v5_debug", - "px4_fmu-v5_default", - "px4_fmu-v5_lto", - "px4_fmu-v5_rover", - "px4_fmu-v5_stackcheck", - "px4_fmu-v5_uavcanv0periph", - "px4_fmu-v5x_default", - "px4_fmu-v5x_rover", - "px4_fmu-v6c_default", - "px4_fmu-v6c_rover", - "px4_fmu-v6u_default", - "px4_fmu-v6u_rover", - "px4_fmu-v6x_default", - "px4_fmu-v6x_rover", - "px4_fmu-v6xrt_bootloader", - "px4_fmu-v6xrt_default", - "px4_fmu-v6xrt_rover", - "px4_io-v2_default", - "raspberrypi_pico_default", - "siyi_n7_default", - "sky-drones_smartap-airlink_default", - "spracing_h7extreme_default", - "thepeach_k1_default", - "thepeach_r1_default", - "uvify_core_default", - "x-mav_ap-h743v2_default", - "zeroone_x6_bootloader", - "zeroone_x6_default", - ], - image: docker_images.nuttx, - archive: true - ] - - def docker_builds = [ - armhf_builds, base_builds, nuttx_builds_archive - ] - - for (def build_type = 0; build_type < docker_builds.size(); build_type++) { - for (def build_target = 0; build_target < docker_builds[build_type].target.size(); build_target++) { - build_nodes.put(docker_builds[build_type].target[build_target], - createBuildNode(docker_builds[build_type].archive, docker_builds[build_type].image, docker_builds[build_type].target[build_target]) - ) - } - } - - parallel build_nodes - - } // script - } // steps - } // stage Build - - // TODO: actually upload artifacts to S3 - // stage('S3 Upload') { - // agent { - // docker { image 'px4io/px4-dev-base-focal:2021-09-08' } - // } - // options { - // skipDefaultCheckout() - // } - // when { - // anyOf { - // branch 'master' - // branch 'beta' - // branch 'stable' - // branch 'pr-jenkins' // for testing - // } - // } - // steps { - // sh 'echo "uploading to S3"' - // } - // } - - } // stages - environment { - CCACHE_DIR = '/tmp/ccache' - CI = true - } - options { - buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14')) - timeout(time: 120, unit: 'MINUTES') - } -} - -def createBuildNode(Boolean archive, String docker_image, String target) { - return { - - bypass_entrypoint = '' - - node { - docker.withRegistry('https://registry.hub.docker.com', 'docker_hub_dagar') { - docker.image(docker_image).inside('-e CCACHE_BASEDIR=${WORKSPACE} -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + bypass_entrypoint) { - stage(target) { - try { - sh('export') - checkout(scm) - sh('make distclean; git clean -ff -x -d .') - sh('git fetch --tags') - sh('ccache -s') - sh('make ' + target) - sh('ccache -s') - sh('make sizes') - if (archive) { - archiveArtifacts(allowEmptyArchive: false, artifacts: 'build/*/*.px4, build/*/*.elf, build/*/*.bin', fingerprint: true, onlyIfSuccessful: true) - } - sh('make ' + target + ' package') - archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/*/*.tar.bz2', fingerprint: true, onlyIfSuccessful: true) - archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/*/*.deb', fingerprint: true, onlyIfSuccessful: true) - } - catch (exc) { - throw (exc) - } - finally { - sh('make distclean; git clean -ff -x -d .') - } - } - } - } - } - } -} diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware deleted file mode 100644 index 73483107a748..000000000000 --- a/.ci/Jenkinsfile-hardware +++ /dev/null @@ -1,904 +0,0 @@ -#!/usr/bin/env groovy - -pipeline { - agent none - stages { - stage('Hardware Test') { - - parallel { - - stage("cubepilot_cubeorange_test") { - stages { - stage("build cubepilot_cubeorange_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make cubepilot_cubeorange_bootloader' - sh 'make cubepilot_cubeorange_test' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*, build/cubepilot_cubeorange_test/etc/init.d/airframes/*', name: 'cubepilot_cubeorange_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'cubepilot_cubeorange' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'cubepilot_cubeorange_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_bootloader/cubepilot_cubeorange_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - // run tests - runTests() - - // load all airframes - // sh("./Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` `cd build/cubepilot_cubeorange_test/etc/init.d/airframes/; find . -regex '.*/[0-9].*' -exec basename {} \\; | cut -d '_' -f 1` || true") // test loading all airframes\ - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true' - } - } - } // stage test - } - } - - stage("cuav_x7pro_test") { - stages { - stage("build cuav_x7pro_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make cuav_x7pro_bootloader' - sh 'make cuav_x7pro_test' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'cuav_x7pro_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'cuav_x7pro' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'cuav_x7pro_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_bootloader/cuav_x7pro_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v4_test") { - stages { - stage("build px4_fmu-v4_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v4_test' - sh 'make px4_fmu-v4_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v4' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v4_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v4pro_test") { - stages { - stage("build px4_fmu-v4pro_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v4pro_test' - sh 'make px4_fmu-v4pro_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4pro_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v4pro' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v4pro_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v5_debug") { - stages { - stage("build px4_fmu-v5_debug") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v5_debug' - sh 'make px4_fmu-v5_debug bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_debug' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_debug' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600 || true' - resetBoard() - } - } - stage("tests") { - steps { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"' - - // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - checkStatus() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v5_stackcheck") { - stages { - stage("build px4_fmu-v5_stackcheck") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v5_stackcheck' - sh 'make px4_fmu-v5_stackcheck bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_stackcheck' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_stackcheck' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' - - // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - checkStatus() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v5_test") { - stages { - stage("build px4_fmu-v5_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v5_test' - sh 'make px4_fmu-v5_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true' - } - } - } // stage test - } - } - - stage("nxp_fmuk66-v3_test") { - stages { - stage("build nxp_fmuk66-v3_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make nxp_fmuk66-v3_test' - //sh 'make nxp_fmuk66-v3_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'nxp_fmuk66-v3_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'nxp_fmuk66-v3' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'nxp_fmuk66-v3_test' - //sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "400"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true' - } - } - } // stage test - } - } - - } // parallel - } // stage Hardware Test - } // stages - environment { - CCACHE_DIR = '/tmp/ccache' - CCACHE_NOHASHDIR = 1 - CI = true - } - options { - buildDiscarder(logRotator(numToKeepStr: '30', artifactDaysToKeepStr: '60')) - timeout(time: 180, unit: 'MINUTES') - skipDefaultCheckout() - } -} - -void checkoutSCM() { - retry(3) { - checkout scm - sh 'export' - sh 'make distclean; git clean -ff -x -d .' - sh 'git fetch --tags' - sh 'ccache -z' - } -} - -void quickCalibrate() { - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters before - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"' -} - -void checkStatus() { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - - // status commands - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/meminfo"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/uptime"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander check" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ekf2 status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload; top once; listener cpuload"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /bin"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /dev"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /etc"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status streams" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf latency"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ps"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm_out status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' -} - -void resetParameters() { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param reset_all"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "CBRK_BUZZER" --value "782097"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SDLOG_DIRS_MAX" --value "1"' -} - -void runTests() { - - // test loading a range of airframes - sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001' - - resetParameters() - - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_IMU_AUTOCAL" --value "0" || true' // disable during testing - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_MAG_AUTOCAL" --value "0" || true' // disable during testing - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"' - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ostest"' - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot after ostest - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander_tests" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "controllib_test"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "lightware_laser_test"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink_tests" || true' // TODO - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd readtest"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params" || true' // expected to fail after erase - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"' - - // tests (stop modules first) - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander stop"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink stop-all"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "navigator stop"' - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during microbenchmarks - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "microbench all"' - - //sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "calib_udelay"' -} - -void printTopics() { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true' - - // these are for casually inspecting the system, output failure doesn't matter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_wind" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_optical_flow_vel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_states" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_wind" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener event" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener heater_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener input_rc" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener led_control" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener log_message" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener manual_control_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mavlink_log" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mission" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener multirotor_motor_limits" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener optical_flow" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_controller_landing_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_setpoint_triplet" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener radio_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command_ack" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_control_mode" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_global_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_land_detected" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true' -} - -void resetBoard() { - resetParameters() - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "echo > /fs/microsd/.format" || true' - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply -} From 884dee35a582a6348f21cabce147a5ad9cd32aa9 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Thu, 21 Nov 2024 15:29:22 -0800 Subject: [PATCH 043/211] ci: container tag pattern updates --- .github/workflows/dev_container.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index 22bd291d0e57..a62180d4a54f 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -38,6 +38,14 @@ jobs: with: images: | ghcr.io/PX4/px4-dev + tags: | + type=schedule + type=semver,pattern={{version}} + type=semver,pattern={{major}}.{{minor}} + type=semver,pattern={{major}} + type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}} + type=ref,event=pr + type=sha - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 From 0fca8e3a85d56f1533df0f07d52d7adfc98f391f Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Thu, 21 Nov 2024 16:12:29 -0800 Subject: [PATCH 044/211] ci: build only if push event --- .github/workflows/dev_container.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index a62180d4a54f..d93a1292c3c6 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -82,6 +82,7 @@ jobs: - name: Push container image uses: docker/build-push-action@v6 + if: ${{ github.event_name == 'push' }} with: context: Tools/setup tags: ${{ steps.meta.outputs.tags }} @@ -89,6 +90,6 @@ jobs: platforms: | linux/amd64 provenance: mode=max - push: ${{ github.event_name == 'push' }} + push: true cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max From cf34b9d5743b726c4885b651abba14c03c9063fa Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Thu, 21 Nov 2024 15:11:44 -0800 Subject: [PATCH 045/211] ci: fixes tag uploads and container tagging --- .github/workflows/build_all_targets.yml | 36 ++++++------------------- 1 file changed, 8 insertions(+), 28 deletions(-) diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml index 43f98e4f6f49..e6d0ece2897a 100644 --- a/.github/workflows/build_all_targets.yml +++ b/.github/workflows/build_all_targets.yml @@ -6,11 +6,6 @@ name: Build all targets on: - workflow_dispatch: - inputs: - tag: - required: true - description: release version push: tags: - 'v*' @@ -51,26 +46,15 @@ jobs: - id: set-timestamp run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")" - # This job is also triggered with versioned tags - # Creating a and pushing a tag starting with "v" just as "v1.0.0" - # will trigger this workflow and when all builds are done create a Github Release - # then it will upload all binaries built as assets - # Additionally, we can also trigger this step manually - # From the Github Actions tab for this repository: - # https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml - # You can now click a "Run Workflow" button that will prompt you for a tag name - # This tag name has to match an existing tag otherwise the new release will be detached - # Note: Only developers with "write" permission to the repository can use this feature - - id: set-tag - if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch' - run: echo "::set-output name=tagname::${{ github.event_name == 'workflow_dispatch' && inputs.tag || github.ref_name }}" - - id: set-branch run: echo "::set-output name=branchname::${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}" - name: Debug Matrix Output if: runner.debug == '1' - run: echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)" + run: | + echo "${{ steps.set-timestamp.outputs.timestamp }}" + echo "${{ steps.set-branch.outputs.branchname }}" + echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)" setup: name: Build Group [${{ matrix.group }}] @@ -159,20 +143,16 @@ jobs: # runs-on: ubuntu-latest runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: [setup, group_targets] - if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch' + if: startsWith(github.ref, 'refs/tags/v') steps: - name: Download Artifacts uses: actions/download-artifact@v4 - - - name: Arrange Binaries - run: | - mkdir artifacts - cp **/**/*.px4 artifacts/ + with: + path: artifacts/ + merge-multiple: true - name: Upload Binaries to Release uses: softprops/action-gh-release@v2 with: - name: ${{ needs.group_targets.outputs.tagname }} - tag_name: ${{ needs.group_targets.outputs.tagname }} draft: true files: artifacts/*.px4 From b19a6ee3b52103227b8f4a76eeb14f6a23a0ba6b Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 2 Oct 2024 09:54:08 +0200 Subject: [PATCH 046/211] ekf2: store position state as lat/lon/alt The position error state is still defined in a body-fixed NED frame but the position state itself is latitude-longitude-altitude. --- msg/EstimatorAidSource2d.msg | 2 +- src/modules/ekf2/EKF/CMakeLists.txt | 1 + .../aux_global_position.cpp | 39 ++-- .../barometer/baro_height_control.cpp | 14 +- .../external_vision/ev_height_control.cpp | 14 +- .../external_vision/ev_pos_control.cpp | 16 +- .../EKF/aid_sources/fake_height_control.cpp | 8 +- .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 14 +- .../aid_sources/gnss/gnss_height_control.cpp | 18 +- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 15 +- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 28 +-- .../aid_sources/magnetometer/mag_control.cpp | 9 +- .../optical_flow/optical_flow_control.cpp | 6 +- .../optical_flow/optical_flow_fusion.cpp | 3 +- .../range_finder/range_height_control.cpp | 4 +- src/modules/ekf2/EKF/ekf.cpp | 63 +++--- src/modules/ekf2/EKF/ekf.h | 51 +++-- src/modules/ekf2/EKF/ekf_helper.cpp | 188 ++++++++++-------- src/modules/ekf2/EKF/estimator_interface.h | 34 +++- .../ekf2/EKF/lat_lon_alt/CMakeLists.txt | 1 + .../ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp | 154 ++++++++++++++ .../ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp | 107 ++++++++++ .../EKF/output_predictor/output_predictor.cpp | 49 ++--- .../EKF/output_predictor/output_predictor.h | 18 +- src/modules/ekf2/EKF/position_fusion.cpp | 135 +++++++++---- src/modules/ekf2/EKF/terrain_control.cpp | 4 +- src/modules/ekf2/EKF2.cpp | 10 +- src/modules/ekf2/test/test_EKF_airspeed.cpp | 19 +- src/modules/ekf2/test/test_EKF_basics.cpp | 15 +- src/modules/ekf2/test/test_EKF_flow.cpp | 46 +++++ src/modules/ekf2/test/test_EKF_gps.cpp | 2 +- .../ekf2/test/test_EKF_height_fusion.cpp | 19 +- .../ekf2/test/test_EKF_yaw_estimator.cpp | 13 +- 33 files changed, 777 insertions(+), 342 deletions(-) create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 14e3ac3f846d..8f3a9f0cda11 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -7,7 +7,7 @@ uint32 device_id uint64 time_last_fuse -float32[2] observation +float64[2] observation float32[2] observation_variance float32[2] innovation diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 954123b250f5..27804ebcd88a 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(bias_estimator) add_subdirectory(output_predictor) +add_subdirectory(lat_lon_alt) set(EKF_LIBS) set(EKF_SRCS) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index dc8f318c9778..973a70dbe439 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -74,24 +74,21 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } estimator_aid_source2d_s aid_src{}; - Vector2f position; - - if (ekf.global_origin_valid()) { - position = ekf.global_origin().project(sample.latitude, sample.longitude); - //const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude; - // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate - float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); - const float pos_var = sq(pos_noise); - const Vector2f pos_obs_var(pos_var, pos_var); - - ekf.updateAidSourceStatus(aid_src, - sample.time_us, // sample timestamp - position, // observation - pos_obs_var, // observation variance - Vector2f(ekf.state().pos) - position, // innovation - Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance - math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate - } + const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl); + const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used + + // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate + float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); + const float pos_var = sq(pos_noise); + const Vector2f pos_obs_var(pos_var, pos_var); + + ekf.updateAidSourceStatus(aid_src, + sample.time_us, // sample timestamp + matrix::Vector2d(sample.latitude, sample.longitude), // observation + pos_obs_var, // observation variance + innovation, // innovation + Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance + math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) && ekf.control_status_flags().yaw_align; @@ -113,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } else { // Try to initialize using measurement - if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, - sample.epv)) { + if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, + sample.epv)) { ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; @@ -131,7 +128,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { - ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); ekf.resetAidSourceStatusZeroInnovation(aid_src); diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index f6ab66f73660..0f5bcd610c91 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -73,7 +73,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (_baro_counter <= _obs_buffer_length) { // Initialize the pressure offset (included in the baro bias) - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); } } @@ -106,7 +106,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) 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(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding @@ -131,8 +131,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + resetHeightTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -163,12 +163,12 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::BARO; _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + resetAltitudeTo(measurement, measurement_var); + bias_est.reset(); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); } aid_src.time_last_fuse = imu_sample.time_us; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 440c0fe7acf6..f62be7575fdc 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp 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(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) @@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_height_sensor_ref == HeightSensor::EV) { _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); + resetHeightTo(-measurement, measurement_var); bias_est.reset(); } else { - bias_est.setBias(-_state.pos(2) + measurement); + bias_est.setBias(_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; @@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); - bias_est.setBias(-_state.pos(2) + measurement); + resetHeightTo(-measurement - bias_est.getBias(), measurement_var); + bias_est.setBias(_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; @@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_params.height_sensor_ref == static_cast(HeightSensor::EV)) { ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); + resetHeightTo(-measurement, measurement_var); _height_sensor_ref = HeightSensor::EV; bias_est.reset(); } else { ECL_INFO("starting %s fusion", AID_SRC_NAME); - bias_est.setBias(-_state.pos(2) + measurement); + bias_est.setBias(_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 590a289da394..266b4cf5482b 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample #endif // CONFIG_EKF2_GNSS + const Vector2f position_estimate = getLocalHorizontalPosition(); + const Vector2f measurement{pos(0), pos(1)}; const Vector2f measurement_var{ @@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) { if (quality_sufficient) { // reset the bias estimator - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-position_estimate + measurement); } else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) { // otherwise stop EV position, when quality is good again it will restart with reset bias @@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample ev_sample.time_us, // sample timestamp position, // observation pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation + position_estimate - position, // innovation Vector2f(getStateVariance()) + pos_obs_var, // innovation variance math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate @@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample 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()), + _ev_pos_b_est.fuseBias(measurement - position_estimate, measurement_var + Vector2f(getStateVariance())); } @@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem // TODO: (_params.position_sensor_ref == PositionSensor::EV) if (_control_status.flags.gps) { ECL_INFO("starting %s fusion", EV_AID_SRC_NAME); - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); _ev_pos_b_est.setFusionActive(); } else { @@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure _ev_pos_b_est.reset(); } else { - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } aid_src.time_last_fuse = _time_delayed_us; @@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (_control_status.flags.gps && !pos_xy_fusion_failing) { // reset EV position bias - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement); } else { _information_events.flags.reset_pos_to_vision = true; if (_control_status.flags.gps) { resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar()); - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } else { resetHorizontalPositionTo(measurement, measurement_var); diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index d10fe57d4413..4ff4e67d6cd9 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion() const float obs_var = sq(_params.pos_noaid_noise); const float innov_gate = 3.f; - updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate); + updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate); const bool continuing_conditions_passing = !isVerticalAidingActive(); const bool starting_conditions_passing = continuing_conditions_passing @@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion() void Ekf::resetFakeHgtFusion() { ECL_INFO("reset fake height fusion"); - _last_known_pos(2) = _state.pos(2); + _last_known_gpos.setAltitude(_gpos.altitude()); resetVerticalVelocityToZero(); resetHeightToLastKnown(); @@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion() void Ekf::resetHeightToLastKnown() { _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2)); - resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise)); + ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude()); + resetHeightTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise)); } void Ekf::stopFakeHgtFusion() diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index ecc6cdfacb90..3e244412ab50 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -63,17 +63,17 @@ void Ekf::controlFakePosFusion() obs_var(0) = obs_var(1) = sq(0.5f); } - const Vector2f position(_last_known_pos); + const Vector2f innovation = (_gpos - _last_known_gpos).xy(); const float innov_gate = 3.f; updateAidSourceStatus(aid_src, _time_delayed_us, - position, // observation - obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation - Vector2f(getStateVariance()) + obs_var, // innovation variance - innov_gate); // innovation gate + Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation + obs_var, // observation variance + innovation, // innovation + Vector2f(getStateVariance()) + obs_var, // innovation variance + innov_gate); // innovation gate const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest; const bool enable_fake_pos = !enable_valid_fake_pos @@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion() void Ekf::resetFakePosFusion() { ECL_INFO("reset fake position fusion"); - _last_known_pos.xy() = _state.pos.xy(); + _last_known_gpos.setLatLon(_gpos); resetHorizontalPositionToLastKnown(); resetHorizontalVelocityToZero(); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index cc165d453398..78a6d12f90a3 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2); + const float gnss_alt = gps_sample.alt + pos_offset_earth(2); - const float measurement = gnss_alt - getEkfGlobalOriginAltitude(); + const float measurement = gnss_alt; const float measurement_var = sq(noise); const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); @@ -81,13 +81,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VPOS)) && measurement_valid - && _pos_ref.isInitialized() + && _local_origin_lat_lon.isInitialized() && _gps_checks_passed; const bool starting_conditions_passing = continuing_conditions_passing @@ -105,8 +105,8 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(aid_src.observation, measurement_var); - bias_est.setBias(_state.pos(2) + measurement); + resetHeightTo(measurement, measurement_var); + bias_est.setBias(-_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; @@ -128,13 +128,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) _height_sensor_ref = HeightSensor::GNSS; _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-measurement, measurement_var); - _gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty + + resetAltitudeTo(measurement, measurement_var); bias_est.reset(); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + measurement); + bias_est.setBias(-_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 236fd2dd63e1..c1b47c224fa7 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -57,25 +57,14 @@ void Ekf::collect_gps(const gnssSample &gps) { - if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) { - // If we have good GPS data set the origin's WGS-84 position to the last gps fix - setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc); - - // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(gps.alt, gps.vacc); - } - + if (_filter_initialised && !_local_origin_lat_lon.isInitialized() && _gps_checks_passed) { _information_events.flags.gps_checks_passed = true; - ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f", - _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon()); - } else { // a rough 2D fix is sufficient to lookup earth spin rate const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) { + if (gps_rough_2d_fix && (_gps_checks_passed || !_local_origin_lat_lon.isInitialized())) { _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 9799c8bb71d4..de8dad37b5e8 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -82,10 +82,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } - if (_pos_ref.isInitialized()) { - updateGnssPos(gnss_sample, _aid_src_gnss_pos); - } - + updateGnssPos(gnss_sample, _aid_src_gnss_pos); updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel); } else if (_control_status.flags.gps) { @@ -108,9 +105,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled) && _control_status.flags.tilt_align - && _control_status.flags.yaw_align - && _pos_ref.isInitialized(); + && _control_status.flags.yaw_align; const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; + const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed; if (_control_status.flags.gps) { if (continuing_conditions_passing) { @@ -174,6 +171,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } _control_status.flags.gps = true; + + } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { + resetHorizontalPositionToGnss(_aid_src_gnss_pos); } } } @@ -221,8 +221,10 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s { // correct position and height for offset relative to IMU const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy(); + const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body); + const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt); + const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth); + const Vector2f innovation = (_gpos - measurement_corrected).xy(); // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise); @@ -237,12 +239,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s const float pos_var = math::max(sq(pos_noise), sq(0.01f)); const Vector2f pos_obs_var(pos_var, pos_var); + const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg()); updateAidSourceStatus(aid_src, gnss_sample.time_us, // sample timestamp - position, // observation + observation, // observation pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation + innovation, // innovation Vector2f(getStateVariance()) + pos_obs_var, // innovation variance math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate } @@ -322,8 +325,9 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src) void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) { _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); - _gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty + resetLatLonTo(aid_src.observation[0], aid_src.observation[1], + aid_src.observation_variance[0] + + aid_src.observation_variance[1]); resetAidSourceStatusZeroInnovation(aid_src); } diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index cff897d10649..85979a24472e 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -90,12 +90,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) if (global_origin_valid() && (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) ) { - // position of local NED origin in GPS / WGS84 frame - double latitude_deg; - double longitude_deg; - global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg); - - if (updateWorldMagneticModel(latitude_deg, longitude_deg)) { + if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) { wmm_updated = true; } @@ -368,7 +363,7 @@ bool Ekf::checkHaglYawResetReq() const // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. static constexpr float mag_anomalies_max_hagl = 1.5f; - const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; + const bool above_mag_anomalies = (getTerrainVPos() + _gpos.altitude()) > mag_anomalies_max_hagl; return above_mag_anomalies; } diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index dbbc1a72961b..60238af10572 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -233,7 +233,9 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample) // reset position, estimate is relative to initial position in this mode, so we start with zero error if (!_control_status.flags.in_air) { ECL_INFO("reset position to zero"); - resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); + //TODO: reset origin instead? + resetHorizontalPositionToLastKnown(); + // resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); } resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); @@ -247,7 +249,7 @@ void Ekf::resetTerrainToFlow() ECL_INFO("reset hagl to flow"); // TODO: use the flow data - const float new_terrain = fmaxf(0.0f, _state.pos(2)); + const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance; const float delta_terrain = new_terrain - _state.terrain; _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index ea722569d624..747fac96f1b1 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -67,7 +67,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) // recalculate the innovation using the updated state const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias; - _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1]; + _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast + (_aid_src_optical_flow.observation[1]); } if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) { diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 3a848f7d46cf..2598f3a3318c 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -148,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::RANGE; _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance); + resetHeightTo(aid_src.observation, aid_src.observation_variance); _state.terrain = 0.f; _control_status.flags.rng_hgt = true; stopRngTerrFusion(); @@ -180,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-(aid_src.observation - _state.terrain)); + resetHeightTo(aid_src.observation - _state.terrain); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f453559c0ef7..e1fb96784fd6 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -74,7 +74,7 @@ void Ekf::reset() // #if defined(CONFIG_EKF2_TERRAIN) // assume a ground clearance - _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -97,7 +97,7 @@ void Ekf::reset() resetGpsDriftCheckFilters(); _gps_checks_passed = false; #endif // CONFIG_EKF2_GNSS - _gps_alt_ref = NAN; + _local_origin_alt = NAN; _output_predictor.reset(); @@ -113,7 +113,7 @@ void Ekf::reset() _time_last_heading_fuse = 0; _time_last_terrain_fuse = 0; - _last_known_pos.setZero(); + _last_known_gpos.setZero(); #if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; @@ -168,7 +168,7 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos, _state.gyro_bias, _state.accel_bias); return true; @@ -205,7 +205,7 @@ bool Ekf::initialiseFilter() initialiseCovariance(); // reset the output predictor state history to match the EKF initial values - _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); + _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos); return true; } @@ -258,11 +258,11 @@ void Ekf::predictState(const imuSample &imu_delayed) _state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity - _state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; + _gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; + _state.pos(2) = -_gpos.altitude(); // constrain states _state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit); - _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); // calculate a filtered horizontal acceleration with a 1 sec time constant @@ -283,14 +283,12 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl return false; } - if (!_pos_ref.isInitialized()) { - if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + if (!_local_origin_lat_lon.isInitialized()) { + if (!resetLatLonTo(latitude, longitude, sq(eph))) { return false; } - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(altitude, epv); - } + resetAltitudeTo(altitude, sq(epv)); return true; } @@ -315,12 +313,20 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl pos_correction = _state.vel * dt_s; } - { - const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy(); + LatLonAlt gpos(latitude, longitude, altitude); + bool alt_valid = true; + + if (!checkAltitudeValidity(gpos.altitude())) { + gpos.setAltitude(_gpos.altitude()); + alt_valid = false; + } + + const LatLonAlt gpos_corrected = gpos + pos_correction; + { const float obs_var = math::max(sq(eph), sq(0.01f)); - const Vector2f innov = Vector2f(_state.pos.xy()) - hpos; + const Vector2f innov = (_gpos - gpos_corrected).xy(); const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; const float sq_gate = sq(5.f); // magic hardcoded gate @@ -334,8 +340,8 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl ECL_INFO("reset position to external observation"); _information_events.flags.reset_pos_to_ext_obs = true; - resetHorizontalPositionTo(hpos, obs_var); - _last_known_pos.xy() = _state.pos.xy(); + resetHorizontalPositionTo(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var); + _last_known_gpos.setLatLon(gpos_corrected); } else { ECL_INFO("fuse external observation as position measurement"); @@ -348,24 +354,16 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl _state_reset_status.posNE_change.zero(); _time_last_hor_pos_fuse = _time_delayed_us; - _last_known_pos.xy() = _state.pos.xy(); + _last_known_gpos.setLatLon(gpos_corrected); } } - if (checkAltitudeValidity(altitude)) { - const float altitude_corrected = altitude - pos_correction(2); - - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(altitude_corrected, epv); + if (alt_valid) { + const float obs_var = math::max(sq(epv), sq(0.01f)); - } else { - const float vpos = -(altitude_corrected - _gps_alt_ref); - const float obs_var = math::max(sq(epv), sq(0.01f)); - - ECL_INFO("reset height to external observation"); - resetVerticalPositionTo(vpos, obs_var); - _last_known_pos(2) = _state.pos(2); - } + ECL_INFO("reset height to external observation"); + resetAltitudeTo(gpos_corrected.altitude(), obs_var); + _last_known_gpos.setAltitude(gpos_corrected.altitude()); } return true; @@ -425,9 +423,10 @@ void Ekf::print_status() (double)getStateVariance()(2) ); + const Vector3f position = getPosition(); printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::pos.idx, State::pos.idx + State::pos.dof - 1, - (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2), + (double)position(0), (double)position(1), (double) position(2), (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) ); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ec9a00154ff3..a76c0e351782 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -66,6 +66,8 @@ # include "aid_sources/aux_global_position/aux_global_position.hpp" #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +#include "lat_lon_alt/lat_lon_alt.hpp" + enum class Likelihood { LOW, MEDIUM, HIGH }; class ExternalVisionVel; @@ -102,8 +104,8 @@ class Ekf final : public EstimatorInterface bool isTerrainEstimateValid() const { return _terrain_valid; } // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const { return _state.terrain; }; - float getHagl() const { return _state.terrain - _state.pos(2); } + float getTerrainVertPos() const { return _state.terrain + getEkfGlobalOriginAltitude(); }; + float getHagl() const { return _state.terrain + _gpos.altitude(); } // get the terrain variance float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); } @@ -192,8 +194,8 @@ class Ekf final : public EstimatorInterface bool checkLatLonValidity(double latitude, double longitude); bool checkAltitudeValidity(float altitude); bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN); - bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN, - float epv = NAN); + bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN, + float epv = NAN); // 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; @@ -217,17 +219,14 @@ class Ekf final : public EstimatorInterface void resetAccelBias(); 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 - // and have not started using synthetic position observations to constrain drift bool isGlobalHorizontalPositionValid() const { - return _pos_ref.isInitialized() && isLocalHorizontalPositionValid(); + return _local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid(); } bool isGlobalVerticalPositionValid() const { - return _pos_ref.isInitialized() && isLocalVerticalPositionValid(); + return PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid(); } bool isLocalHorizontalPositionValid() const @@ -473,6 +472,8 @@ class Ekf final : public EstimatorInterface StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + LatLonAlt _gpos{0.0, 0.0, 0.f}; + bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) @@ -486,7 +487,7 @@ class Ekf final : public EstimatorInterface uint64_t _time_last_heading_fuse{0}; uint64_t _time_last_terrain_fuse{0}; - Vector3f _last_known_pos{}; ///< last known local position vector (m) + LatLonAlt _last_known_gpos{}; Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s @@ -645,11 +646,11 @@ class Ekf final : public EstimatorInterface P.slice(S.idx, S.idx) = cov; } - bool setLatLonOrigin(double latitude, double longitude, float eph = NAN); - bool setAltOrigin(float altitude, float epv = NAN); + bool setLatLonOrigin(double latitude, double longitude, float hpos_var = NAN); + bool setAltOrigin(float altitude, float vpos_var = NAN); - bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN); - bool setAltOriginFromCurrentPos(float altitude, float epv = NAN); + bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN); + bool resetAltitudeTo(float altitude, float vpos_var = NAN); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); @@ -711,14 +712,22 @@ class Ekf final : public EstimatorInterface void resetHorizontalPositionToLastKnown(); - void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); - void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } + void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, + const Vector2f &new_horz_pos_var); + void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, const float pos_var = NAN) { resetHorizontalPositionTo(new_latitude, new_longitude, Vector2f(pos_var, pos_var)); } + void resetHorizontalPositionTo(const Vector2f &new_pos, const Vector2f &new_horz_pos_var); + + Vector2f getLocalHorizontalPosition() const; + + Vector2f computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const; + void updateHorizontalPositionResetStatus(const Vector2f &delta); void resetWindTo(const Vector2f &wind, const Vector2f &wind_var); bool isHeightResetRequired() const; - void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); + void resetHeightTo(float new_altitude, float new_vert_pos_var = NAN); + void updateVerticalPositionResetStatus(const float delta_z); void resetVerticalVelocityToZero(); @@ -740,6 +749,7 @@ class Ekf final : public EstimatorInterface void controlTerrainFakeFusion(); void updateTerrainValidity(); + void updateTerrainResetStatus(const 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 @@ -832,6 +842,7 @@ class Ekf final : public EstimatorInterface void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); + void stopEvPosFusion(); void stopEvHgtFusion(); void stopEvVelFusion(); @@ -1049,9 +1060,9 @@ class Ekf final : public EstimatorInterface } // helper used for populating and filtering estimator aid source struct for logging - template + template void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample, - const S &observation, const S &observation_variance, + const D &observation, const S &observation_variance, const S &innovation, const S &innovation_variance, float innovation_gate = 1.f) const { @@ -1106,7 +1117,7 @@ class Ekf final : public EstimatorInterface status.test_ratio[i] = test_ratio; - status.observation[i] = observation(i); + status.observation[i] = static_cast(observation(i)); status.observation_variance[i] = observation_variance(i); status.innovation[i] = innovation(i); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d4b3e6031ebd..33e63a6eb64a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -65,9 +65,9 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { - origin_time = _pos_ref.getProjectionReferenceTimestamp(); - latitude = _pos_ref.getProjectionReferenceLat(); - longitude = _pos_ref.getProjectionReferenceLon(); + origin_time = _local_origin_lat_lon.getProjectionReferenceTimestamp(); + latitude = _local_origin_lat_lon.getProjectionReferenceLat(); + longitude = _local_origin_lat_lon.getProjectionReferenceLon(); origin_alt = getEkfGlobalOriginAltitude(); } @@ -85,156 +85,170 @@ bool Ekf::checkAltitudeValidity(const float altitude) return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f))); } -bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, - const float epv) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float hpos_var, + const float vpos_var) { - if (!setLatLonOrigin(latitude, longitude, eph)) { + if (!setLatLonOrigin(latitude, longitude, hpos_var)) { return false; } // altitude is optional - setAltOrigin(altitude, epv); + setAltOrigin(altitude, vpos_var); return true; } -bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph) +bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } - bool current_pos_available = false; - double current_lat = static_cast(NAN); - double current_lon = static_cast(NAN); + if (!_local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid()) { + // Already navigating in a local frame, use the origin to initialize global position + const Vector2f pos_prev = getLocalHorizontalPosition(); + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + double new_latitude; + double new_longitude; + _local_origin_lat_lon.reproject(pos_prev(0), pos_prev(1), new_latitude, new_longitude); + resetHorizontalPositionTo(new_latitude, new_longitude, hpos_var); - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && isLocalHorizontalPositionValid()) { - _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); - current_pos_available = true; - } - - // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _time_delayed_us); - - if (PX4_ISFINITE(eph) && (eph >= 0.f)) { - _gpos_origin_eph = eph; - } - - if (current_pos_available) { - // reset horizontal position if we already have a global origin - Vector2f position = _pos_ref.project(current_lat, current_lon); - resetHorizontalPositionTo(position); + } else { + // Simply move the origin and compute the change in local position + const Vector2f pos_prev = getLocalHorizontalPosition(); + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + const Vector2f pos_new = getLocalHorizontalPosition(); + const Vector2f delta_pos = pos_new - pos_prev; + updateHorizontalPositionResetStatus(delta_pos); } return true; } -bool Ekf::setAltOrigin(const float altitude, const float epv) +bool Ekf::setAltOrigin(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } - const float gps_alt_ref_prev = _gps_alt_ref; - _gps_alt_ref = altitude; + ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt, + (double)altitude); - if (PX4_ISFINITE(epv) && (epv >= 0.f)) { - _gpos_origin_epv = epv; - } + if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) { + const float local_alt_prev = _gpos.altitude(); + _local_origin_alt = altitude; + resetHeightTo(local_alt_prev + _local_origin_alt); - if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) { - // determine current z - const float z_prev = _state.pos(2); - const float current_alt = -z_prev + 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); - ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, - (double)_state.pos(2)); -#if defined(CONFIG_EKF2_GNSS) - // adjust existing GPS height bias - _gps_hgt_b_est.setBias(gps_hgt_bias); -#endif // CONFIG_EKF2_GNSS + } else { + const float delta_origin_alt = altitude - _local_origin_alt; + _local_origin_alt = altitude; + updateVerticalPositionResetStatus(-delta_origin_alt); + +#if defined(CONFIG_EKF2_TERRAIN) + updateTerrainResetStatus(-delta_origin_alt); +#endif // CONFIG_EKF2_TERRAIN } return true; } -bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude, - const float eph, const float epv) +bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude, + const float hpos_var, const float vpos_var) { - if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + if (!resetLatLonTo(latitude, longitude, hpos_var)) { return false; } // altitude is optional - setAltOriginFromCurrentPos(altitude, epv); + resetAltitudeTo(altitude, vpos_var); return true; } -bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph) +bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } - _pos_ref.initReference(latitude, longitude, _time_delayed_us); + Vector2f pos_prev; + + if (!_local_origin_lat_lon.isInitialized()) { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + pos_prev = zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (isLocalHorizontalPositionValid()) { + double est_lat; + double est_lon; + _local_origin_lat_lon.reproject(-pos_prev(0), -pos_prev(1), est_lat, est_lon); + _local_origin_lat_lon.initReference(est_lat, est_lon, _time_delayed_us); + } + + ECL_INFO("Origin set to lat=%.6f, lon=%.6f", + _local_origin_lat_lon.getProjectionReferenceLat(), _local_origin_lat_lon.getProjectionReferenceLon()); - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (isLocalHorizontalPositionValid()) { - double est_lat; - double est_lon; - _pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon); - _pos_ref.initReference(est_lat, est_lon, _time_delayed_us); + } else { + pos_prev = _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); } - if (PX4_ISFINITE(eph) && (eph >= 0.f)) { - _gpos_origin_eph = eph; + _gpos.setLatLonDeg(latitude, longitude); + _output_predictor.resetLatLonTo(latitude, longitude); + + const Vector2f delta_horz_pos = getLocalHorizontalPosition() - pos_prev; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); +#endif // CONFIG_EKF2_EXTERNAL_VISION + + updateHorizontalPositionResetStatus(delta_horz_pos); + + if (PX4_ISFINITE(hpos_var)) { + P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, math::max(sq(0.01f), hpos_var)); } + // Reset the timout timer + _time_last_hor_pos_fuse = _time_delayed_us; + return true; } -bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv) +bool Ekf::resetAltitudeTo(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } - _gps_alt_ref = altitude + _state.pos(2); + if (!PX4_ISFINITE(_local_origin_alt)) { + const float local_alt_prev = _gpos.altitude(); - if (PX4_ISFINITE(epv) && (epv >= 0.f)) { - _gpos_origin_epv = epv; + if (isLocalVerticalPositionValid()) { + _local_origin_alt = altitude - local_alt_prev; + + } else { + _local_origin_alt = altitude; + } + + ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt); } + resetHeightTo(altitude, vpos_var); + return true; } void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { - float eph = INFINITY; - float epv = INFINITY; - if (global_origin_valid()) { - // report absolute accuracy taking into account the uncertainty in location of the origin - eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph)); - epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv)); - - if (_horizontal_deadreckon_time_exceeded) { - float lpos_eph = 0.f; - float lpos_epv = 0.f; - get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv); + get_ekf_lpos_accuracy(ekf_eph, ekf_epv); - eph = math::max(eph, lpos_eph); - epv = math::max(epv, lpos_epv); - } + } else { + *ekf_eph = INFINITY; + *ekf_epv = INFINITY; } - - *ekf_eph = eph; - *ekf_epv = epv; } void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const @@ -726,7 +740,13 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.vel = matrix::constrain(_state.vel - K.slice(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f); // pos - _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); + const Vector3f pos_correction = K.slice(State::pos.idx, 0) * (-innovation); + + // Accumulate position in global coordinates + _gpos += pos_correction; + _state.pos.zero(); + // Also store altitude in the state vector as this is used for optical flow fusion + _state.pos(2) = -_gpos.altitude(); // gyro_bias _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index a188d2f88e30..5322fe09976c 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -42,6 +42,7 @@ #ifndef EKF_ESTIMATOR_INTERFACE_H #define EKF_ESTIMATOR_INTERFACE_H +#include "lat_lon_alt/lat_lon_alt.hpp" #if defined(MODULE_NAME) #include # define ECL_INFO PX4_DEBUG @@ -241,7 +242,26 @@ class EstimatorInterface 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(); } + Vector3f getPosition() const + { + LatLonAlt lla = _output_predictor.getLatLonAlt(); + float x; + float y; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + } + + const float z = -(lla.altitude() - getEkfGlobalOriginAltitude()); + + return Vector3f(x, y, z); + } + LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -307,9 +327,9 @@ class EstimatorInterface const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } - bool global_origin_valid() const { return _pos_ref.isInitialized(); } - const MapProjection &global_origin() const { return _pos_ref; } - float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } + bool global_origin_valid() const { return _local_origin_lat_lon.isInitialized(); } + const MapProjection &global_origin() const { return _local_origin_lat_lon; } + float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_local_origin_alt) ? _local_origin_alt : 0.f; } OutputPredictor &output_predictor() { return _output_predictor; }; @@ -379,10 +399,8 @@ class EstimatorInterface 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 - 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 + MapProjection _local_origin_lat_lon{}; + float _local_origin_alt{NAN}; #if defined(CONFIG_EKF2_GNSS) RingBuffer *_gps_buffer {nullptr}; diff --git a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt new file mode 100644 index 000000000000..bf2b3ebf536b --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt @@ -0,0 +1 @@ +px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS geo) diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp new file mode 100644 index 000000000000..0fa7115df77a --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "mathlib/math/Limits.hpp" +#include + +class LatLonAlt +{ +public: + LatLonAlt() = default; + LatLonAlt(const LatLonAlt &lla) + { + _latitude_rad = lla.latitude_rad(); + _longitude_rad = lla.longitude_rad(); + _altitude = lla.altitude(); + } + + LatLonAlt(const double latitude_deg, const double longitude_deg, const float altitude_m) + { + _latitude_rad = math::radians(latitude_deg); + _longitude_rad = math::radians(longitude_deg); + _altitude = altitude_m; + } + + void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; } + + double latitude_deg() const { return math::degrees(latitude_rad()); } + double longitude_deg() const { return math::degrees(longitude_rad()); } + + const double &latitude_rad() const { return _latitude_rad; } + const double &longitude_rad() const { return _longitude_rad; } + float altitude() const { return _altitude; } + + void setLatitudeDeg(const double &latitude_deg) { _latitude_rad = math::radians(latitude_deg); } + void setLongitudeDeg(const double &longitude_deg) { _longitude_rad = math::radians(longitude_deg); } + void setAltitude(const float altitude) { _altitude = altitude; } + + void setLatLon(const LatLonAlt &lla) { _latitude_rad = lla.latitude_rad(); _longitude_rad = lla.longitude_rad(); } + void setLatLonDeg(const double latitude, const double longitude) { _latitude_rad = math::radians(latitude); _longitude_rad = math::radians(longitude); } + void setLatLonRad(const double latitude, const double longitude) { _latitude_rad = latitude; _longitude_rad = longitude; } + + void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); } + + void operator+=(const matrix::Vector3f &delta_pos) + { + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + _altitude -= delta_pos(2); + } + + void operator+=(const matrix::Vector2f &delta_pos) + { + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + } + + LatLonAlt operator+(const matrix::Vector3f &delta_pos) const + { + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude()); + const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + const float altitude = _altitude - delta_pos(2); + + LatLonAlt lla_new; + lla_new.setLatLonRad(latitude_rad, longitude_rad); + lla_new.setAltitude(altitude); + return lla_new; + } + + void operator=(const LatLonAlt &lla) + { + _latitude_rad = lla.latitude_rad(); + _longitude_rad = lla.longitude_rad(); + _altitude = lla.altitude(); + } + + matrix::Vector3f operator-(const LatLonAlt &lla) const + { + const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad()); + const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad()); + const float delta_alt = _altitude - lla.altitude(); + + const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + return matrix::Vector3f(static_cast(delta_lat * d_lat_lon_to_d_xy(0)), + static_cast(delta_lon * d_lat_lon_to_d_xy(1)), + -delta_alt); + } + +private: + // Convert between curvilinear and cartesian errors + static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude) + { + double r_n; + double r_e; + computeRadiiOfCurvature(latitude, r_n, r_e); + const double dn_dlat = r_n + static_cast(altitude); + const double de_dlon = (r_e + static_cast(altitude)) * cos(latitude); + + return matrix::Vector2d(dn_dlat, de_dlon); + } + + static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature, + double &transverse_radius_of_curvature) + { + const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2); + const double sqrt_tmp = std::sqrt(tmp); + meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp); + transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp; + } + + struct Wgs84 { + static constexpr double equatorial_radius = 6378137.0; + static constexpr double eccentricity = 0.0818191908425; + static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity); + }; + + double _latitude_rad{0.0}; + double _longitude_rad{0.0}; + float _altitude{0.0}; +}; diff --git a/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp b/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp new file mode 100644 index 000000000000..b7a42ebd48d3 --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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 "lat_lon_alt.hpp" + +using namespace matrix; +using math::radians; +using math::degrees; + +TEST(TestLatLonAlt, init) +{ + LatLonAlt lla(5.7, -2.3, 420); + ASSERT_FLOAT_EQ(lla.latitude_deg(), 5.7); + ASSERT_FLOAT_EQ(lla.longitude_deg(), -2.3); + ASSERT_EQ(lla.altitude(), 420); +} + +TEST(TestLatLonAlt, set) +{ + LatLonAlt lla(0.0, 0.0, 0); + ASSERT_EQ(lla.latitude_rad(), 0.0); + ASSERT_EQ(lla.longitude_rad(), 0.0); + ASSERT_EQ(lla.altitude(), 0); + + lla.setLatLonRad(0.1, -0.5); + lla.setAltitude(420); + ASSERT_EQ(lla.latitude_rad(), 0.1); + ASSERT_EQ(lla.longitude_rad(), -0.5); + ASSERT_EQ(lla.altitude(), 420); +} + +TEST(TestLatLonAlt, copy) +{ + LatLonAlt lla(-0.8, -0.1, 500); + + LatLonAlt lla_copy = lla; + ASSERT_EQ(lla_copy.latitude_deg(), -0.8); + ASSERT_EQ(lla_copy.longitude_deg(), -0.1); + ASSERT_EQ(lla_copy.altitude(), 500); +} + +TEST(TestLatLonAlt, addDeltaPos) +{ + MapProjection pos_ref(60.0, 5.0); + LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 400.f); + + Vector3f delta_pos(5.f, -2.f, 3.f); + lla += delta_pos; + + double lat_new, lon_new; + pos_ref.reproject(delta_pos(0), delta_pos(1), lat_new, lon_new); + + EXPECT_NEAR(lla.latitude_deg(), lat_new, 1e-6); + EXPECT_NEAR(lla.longitude_deg(), lon_new, 1e-6); + EXPECT_EQ(lla.altitude(), 397.f); +} + +TEST(TestLatLonAlt, subLatLonAlt) +{ + MapProjection pos_ref(60.0, 5.0); + LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 0.f); + + const Vector3f delta_pos_true(1.f, -2.f, 3.f); + + double lat_new, lon_new; + pos_ref.reproject(delta_pos_true(0), delta_pos_true(1), lat_new, lon_new); + LatLonAlt lla_new(lat_new, lon_new, -3.f); + Vector3f delta_pos = lla_new - lla; + + EXPECT_NEAR(delta_pos(0), delta_pos_true(0), 1e-2); + EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2); + EXPECT_EQ(delta_pos(2), delta_pos_true(2)); +} diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index d16e20a79095..5657d68a2029 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -69,7 +69,7 @@ void OutputPredictor::print_status() _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); } -void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) +void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state) { const outputSample &output_delayed = _output_buffer.get_oldest(); @@ -77,9 +77,12 @@ void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()}; q_delta.normalize(); - // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon + // calculate the velocity delta between the output and EKF at the EKF fusion time horizon const Vector3f vel_delta = vel_state - output_delayed.vel; - const Vector3f pos_delta = pos_state - output_delayed.pos; + + // zero the position error at delayed time and reset the global reference + const Vector3f pos_delta = -output_delayed.pos; + _global_ref = gpos_state; // loop through the output filter state history and add the deltas for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { @@ -156,29 +159,15 @@ void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel) _output_vert_new.vert_vel += delta_vert_vel; } -void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos) +void OutputPredictor::resetLatLonTo(const double &new_latitude, const double &new_longitude) { - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].pos.xy() += delta_horz_pos; - } - - _output_new.pos.xy() += delta_horz_pos; + _global_ref.setLatitudeDeg(new_latitude); + _global_ref.setLongitudeDeg(new_longitude); } -void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change) +void OutputPredictor::resetAltitudeTo(const float new_altitude, const float vert_pos_change) { - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - _output_new.pos(2) += vert_pos_change; - - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].pos(2) += vert_pos_change; - _output_vert_buffer[i].vert_vel_integ += vert_pos_change; - } - - // add the reset amount to the output observer vertical position state - _output_vert_new.vert_vel_integ = new_vert_pos; + _global_ref.setAltitude(new_altitude); } void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle, @@ -261,7 +250,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, + const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) { // calculate an average filter update time @@ -317,6 +306,8 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); + const Vector3f pos_state = gpos_state - _global_ref; + // calculate down velocity and position tracking errors const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel); const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ); @@ -325,7 +316,7 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, // using a PD feedback tuned to a 5% overshoot const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f; - applyCorrectionToVerticalOutputBuffer(vert_vel_correction); + applyCorrectionToVerticalOutputBuffer(vert_vel_correction, pos_state(2)); // calculate velocity and position tracking errors const Vector3f vel_err(vel_state - output_delayed.vel); @@ -342,10 +333,14 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, _pos_err_integ += pos_err; const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; - applyCorrectionToOutputBuffer(vel_correction, pos_correction); + // as the reference changes, adjust the position correction to keep a constant global position + const Vector3f pos_correction_with_ref_change = pos_correction - pos_state; + applyCorrectionToOutputBuffer(vel_correction, pos_correction_with_ref_change); + + _global_ref = gpos_state; } -void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction) +void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel_correction, const float pos_ref_change) { // loop through the vertical output filter state history starting at the oldest and apply the corrections to the // vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel @@ -367,7 +362,7 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre // position is propagated forward using the corrected velocity and a trapezoidal integrator next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * - next_state.dt; + next_state.dt - pos_ref_change; // advance the index index = (index + 1) % size; diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 07248f3dce72..33b7f127bc05 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -37,6 +37,7 @@ #include #include "../RingBuffer.h" +#include "../lat_lon_alt/lat_lon_alt.hpp" #include @@ -52,7 +53,7 @@ class OutputPredictor // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, - const matrix::Vector3f &pos_state); + const LatLonAlt &gpos_state); /* * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. @@ -67,7 +68,7 @@ class OutputPredictor const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); void correctOutputStates(const uint64_t time_delayed_us, - const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, + const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); void resetQuaternion(const matrix::Quatf &quat_change); @@ -75,8 +76,8 @@ class OutputPredictor void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel); void resetVerticalVelocityTo(float delta_vert_vel); - void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos); - void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change); + void resetLatLonTo(const double &new_latitude, const double &new_longitude); + void resetAltitudeTo(float new_altitude, float vert_pos_change); void print_status(); @@ -106,13 +107,12 @@ class OutputPredictor // get the derivative of the vertical position of the body frame origin in local NED earth frame float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } - // get the position of the body frame origin in local earth frame - matrix::Vector3f getPosition() const + LatLonAlt getLatLonAlt() const { // rotate the position of the IMU relative to the boy origin into earth frame const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body}; // subtract from the EKF position (which is at the IMU) to get position at the body origin - return _output_new.pos - pos_offset_earth; + return _global_ref + (_output_new.pos - pos_offset_earth); } // return an array containing the output predictor angular, velocity and position tracking @@ -133,7 +133,7 @@ class OutputPredictor * This provides an alternative vertical velocity output that is closer to the first derivative * of the position but does degrade tracking relative to the EKF state. */ - void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); + void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction, const float pos_ref_change); /* * Calculate corrections to be applied to vel and pos output state history. @@ -159,6 +159,8 @@ class OutputPredictor float dt{0.f}; ///< delta time (sec) }; + LatLonAlt _global_ref{0.0, 0.0, 0.f}; + RingBuffer _output_buffer{12}; RingBuffer _output_vert_buffer{12}; diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 90080458423b..e4c16b49415b 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -36,7 +36,7 @@ void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate) const { - float innovation = _state.pos(2) - observation; + float innovation = -_gpos.altitude() - observation; float innovation_variance = getStateVariance()(2) + observation_variance; updateAidSourceStatus(aid_src, time_us, @@ -93,10 +93,20 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) return aid_src.fused; } -void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) +void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, + const Vector2f &new_horz_pos_var) { - const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; - _state.pos.xy() = new_horz_pos; + const Vector2f delta_horz_pos = computeDeltaHorizontalPosition(new_latitude, new_longitude); + + updateHorizontalPositionResetStatus(delta_horz_pos); + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); +#endif // CONFIG_EKF2_EXTERNAL_VISION + //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); + + _gpos.setLatLonDeg(new_latitude, new_longitude); + _output_predictor.resetLatLonTo(new_latitude, new_longitude); if (PX4_ISFINITE(new_horz_pos_var(0))) { P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); @@ -106,54 +116,89 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } - _output_predictor.resetHorizontalPositionTo(delta_horz_pos); + // Reset the timout timer + _time_last_hor_pos_fuse = _time_delayed_us; +} + +Vector2f Ekf::computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const +{ + Vector2f pos; + Vector2f pos_new; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1)); + _local_origin_lat_lon.project(new_latitude, new_longitude, pos_new(0), pos_new(1)); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1)); + zero_ref.project(new_latitude, new_longitude, pos_new(0), pos_new(1)); + } + + return pos_new - pos; +} - // record the state change +Vector2f Ekf::getLocalHorizontalPosition() const +{ + if (_local_origin_lat_lon.isInitialized()) { + return _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + return zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + } +} + +void Ekf::updateHorizontalPositionResetStatus(const Vector2f &delta) +{ if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { - _state_reset_status.posNE_change = delta_horz_pos; + _state_reset_status.posNE_change = delta; } else { // there's already a reset this update, accumulate total delta - _state_reset_status.posNE_change += delta_horz_pos; + _state_reset_status.posNE_change += delta; } _state_reset_status.reset_count.posNE++; +} -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); -#endif // CONFIG_EKF2_EXTERNAL_VISION - //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); +void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos, + const Vector2f &new_horz_pos_var) +{ + double new_latitude; + double new_longitude; - // Reset the timout timer - _time_last_hor_pos_fuse = _time_delayed_us; + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude); + } + + resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var); } -void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) +void Ekf::resetHeightTo(const float new_altitude, float new_vert_pos_var) { - const float old_vert_pos = _state.pos(2); - _state.pos(2) = new_vert_pos; + const float old_altitude = _gpos.altitude(); + _gpos.setAltitude(new_altitude); if (PX4_ISFINITE(new_vert_pos_var)) { // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); } - const float delta_z = new_vert_pos - old_vert_pos; + const float delta_z = -(new_altitude - old_altitude); // apply the change in height / height rate to our newest height / height rate estimate // which have already been taken out from the output buffer - _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); - - // record the state change - if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { - _state_reset_status.posD_change = delta_z; + _output_predictor.resetAltitudeTo(new_altitude, delta_z); - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posD_change += delta_z; - } - - _state_reset_status.reset_count.posD++; + updateVerticalPositionResetStatus(delta_z); #if defined(CONFIG_EKF2_BAROMETER) _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); @@ -166,9 +211,29 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_TERRAIN) + updateTerrainResetStatus(delta_z); _state.terrain += delta_z; +#endif // CONFIG_EKF2_TERRAIN + + // Reset the timout timer + _time_last_hgt_fuse = _time_delayed_us; +} - // record the state change +void Ekf::updateVerticalPositionResetStatus(const float delta_z) +{ + if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { + _state_reset_status.posD_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posD_change += delta_z; + } + + _state_reset_status.reset_count.posD++; +} + +void Ekf::updateTerrainResetStatus(const float delta_z) +{ if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { _state_reset_status.hagl_change = delta_z; @@ -178,17 +243,15 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v } _state_reset_status.reset_count.hagl++; -#endif // CONFIG_EKF2_TERRAIN - - // Reset the timout timer - _time_last_hgt_fuse = _time_delayed_us; } void Ekf::resetHorizontalPositionToLastKnown() { - ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); + ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_gpos.latitude_deg(), + (double)_last_known_gpos.longitude_deg()); _information_events.flags.reset_pos_to_last_known = true; // Used when falling back to non-aiding mode of operation - resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); + resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(), + sq(_params.pos_noaid_noise)); } diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 323d5ac55349..afb845a662ec 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -43,7 +43,7 @@ void Ekf::initTerrain() { // assume a ground clearance - _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; // use the ground clearance value as our uncertainty P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); @@ -53,7 +53,7 @@ void Ekf::controlTerrainFakeFusion() { // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { - _last_on_ground_posD = _state.pos(2); + _last_on_ground_posD = -_gpos.altitude(); _control_status.flags.rng_fault = false; } else if (!_control_status_prev.flags.in_air) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a40289f7b340..93c9ed617a0f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1166,17 +1166,17 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) { if (_ekf.global_origin_valid() && _ekf.control_status().flags.yaw_align) { - const Vector3f position{_ekf.getPosition()}; - // generate and publish global position data vehicle_global_position_s global_pos{}; global_pos.timestamp_sample = timestamp; - // Position of local NED origin in GPS / WGS84 frame - _ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon); + // Position GPS / WGS84 frame + const LatLonAlt lla = _ekf.getLatLonAlt(); + global_pos.lat = lla.latitude_deg(); + global_pos.lon = lla.longitude_deg(); global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid(); - global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters + global_pos.alt = lla.altitude(); global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid(); #if defined(CONFIG_EKF2_GNSS) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d1a9e918963a..d905fb85703c 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -180,15 +180,31 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) _ekf->set_vehicle_at_rest(false); _ekf->set_is_fixed_wing(true); + const Vector3f pos_prev = _ekf->getPosition(); + const double latitude_new = -15.0000005; const double longitude_new = -115.0000005; const float altitude_new = 1500.0; const float eph = 50.f; const float epv = 10.f; - _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new); _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0); + const Vector3f pos = _ekf->getPosition(); + + // lat/lon is initialized so the local horizontal position remains constant + EXPECT_NEAR(pos(0), pos_prev(0), 1e-3f); + EXPECT_NEAR(pos(1), pos_prev(1), 1e-3f); + + // alt is updated as the local altitude origin was already set + EXPECT_NEAR(pos(2), pos_prev(2) - (altitude_new - _ekf->getEkfGlobalOriginAltitude()), 1e-3f); + + const LatLonAlt lla = _ekf->getLatLonAlt(); + EXPECT_NEAR(lla.latitude_deg(), latitude_new, 1e-6f); + EXPECT_NEAR(lla.longitude_deg(), longitude_new, 1e-6f); + EXPECT_NEAR(lla.altitude(), altitude_new, 1e-3f); + + // Simulate the fact that the sideslip can start immediately, without // waiting for a measurement sample. _ekf_wrapper.enableBetaFusion(); @@ -227,7 +243,6 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) const float eph = 50.f; const float epv = 1.f; - _ekf->setEkfGlobalOrigin(latitude, longitude, altitude); _ekf->resetGlobalPosToExternalObservation(latitude, longitude, altitude, eph, epv, 0); _ekf_wrapper.enableBetaFusion(); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index afe56992050d..d67cac0e528a 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -219,19 +219,22 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) _altitude_new = 100.0; _sensor_simulator.startGps(); - _ekf->set_min_required_gps_health_time(1e6); - _sensor_simulator.runSeconds(1); + _ekf_wrapper.enableGpsHeightFusion(); _sensor_simulator.setGpsLatitude(_latitude_new); _sensor_simulator.setGpsLongitude(_longitude_new); _sensor_simulator.setGpsAltitude(_altitude_new); + _ekf->set_min_required_gps_health_time(1e6); + _sensor_simulator.runSeconds(1); _sensor_simulator.runSeconds(5); _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); EXPECT_DOUBLE_EQ(_latitude, _latitude_new); EXPECT_DOUBLE_EQ(_longitude, _longitude_new); - EXPECT_NEAR(_altitude, _altitude_new, 0.01f); + + // In baro height ref the origin is set using baro data and not GNSS altitude + EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f); // Note: we cannot reset too far since the local position is limited to 1e6m _latitude_new = 14.0000005; @@ -261,11 +264,13 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) { - _ekf->getEkfGlobalOrigin(_origin_time, _latitude_new, _longitude_new, _altitude_new); + _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); EXPECT_DOUBLE_EQ(_latitude, _latitude_new); EXPECT_DOUBLE_EQ(_longitude, _longitude_new); - EXPECT_FLOAT_EQ(_altitude, _altitude_new); + + // In baro height ref the origin is set using baro data and not GNSS altitude + EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f); EXPECT_FALSE(_ekf->global_origin_valid()); diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 650d168a98b7..bc33c350e792 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -345,3 +345,49 @@ TEST_F(EkfFlowTest, yawMotionNoMagFusion) _ekf->state().vector().print(); _ekf->covariances().print(); } + +TEST_F(EkfFlowTest, deadReckoning) +{ + ResetLoggingChecker reset_logging_checker(_ekf); + + // WHEN: simulate being 5m above ground + const float simulated_distance_to_ground = 5.f; + _sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground); + startRangeFinderFusion(simulated_distance_to_ground); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + + // WHEN: moving a couple of meters while doing flow dead_reckoning + Vector3f simulated_velocity(0.5f, -0.2f, 0.f); + _sensor_simulator._trajectory[0].setCurrentVelocity(simulated_velocity(0)); + _sensor_simulator._trajectory[1].setCurrentVelocity(simulated_velocity(1)); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + + _sensor_simulator.runTrajectorySeconds(5.f); + + simulated_velocity = Vector3f(0.f, 0.f, 0.f); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + + _sensor_simulator.runTrajectorySeconds(_sensor_simulator._trajectory[0].getTotalTime()); + + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + const Vector3f lpos_before_reset = _ekf->getPosition(); + const float altitude_ref_prev = _ekf->getEkfGlobalOriginAltitude(); + + const double latitude_new = -15.0000005; + const double longitude_new = -115.0000005; + const float altitude_new = 1500.0; + const float eph = 50.f; + const float epv = 10.f; + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv); + + const Vector3f lpos_after_reset = _ekf->getPosition(); + + EXPECT_NEAR(lpos_after_reset(0), lpos_before_reset(0), 1e-3); + EXPECT_NEAR(lpos_after_reset(1), lpos_before_reset(1), 1e-3); + EXPECT_NEAR(lpos_after_reset(2), lpos_before_reset(2) + (altitude_new - altitude_ref_prev), 1e-3); +} diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index ac683564a473..f6aae937df0b 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -164,7 +164,7 @@ TEST_F(EkfGpsTest, resetToGpsPosition) // AND: simulate jump in position _sensor_simulator.startGps(); - const Vector3f simulated_position_change(2.0f, -1.0f, 0.f); + const Vector3f simulated_position_change(20.0f, -1.0f, 0.f); _sensor_simulator._gps.stepHorizontalPositionByMeters( Vector2f(simulated_position_change)); _sensor_simulator.runSeconds(6); diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index b322a2818245..17c95cad453f 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -123,7 +123,7 @@ TEST_F(EkfHeightFusionTest, baroRef) /* EXPECT_EQ(status.bias, _sensor_simulator._baro.getData()); */ // This is the real bias, but the estimator isn't running so the status isn't updated EXPECT_EQ(baro_status.bias, 0.f); const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus(); - EXPECT_NEAR(gps_status.bias, -baro_increment, 0.2f); + EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f); const float terrain = _ekf->getTerrainVertPos(); EXPECT_NEAR(terrain, -baro_increment, 1.2f); @@ -170,8 +170,13 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion()); - // AND WHEN: the baro data increases const float baro_initial = _sensor_simulator._baro.getData(); + + const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus(); + const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt; + EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f); + + // AND WHEN: the baro data increases const float baro_increment = 5.f; _sensor_simulator._baro.setData(baro_initial + baro_increment); _sensor_simulator.runSeconds(100); @@ -180,7 +185,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) // the GPS height value and the baro gets its bias estimated EXPECT_NEAR(_ekf->getPosition()(2), 0.f, 1.f); const BiasEstimator::status &baro_status = _ekf->getBaroBiasEstimatorStatus(); - EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f); + EXPECT_NEAR(baro_status.bias, baro_rel_initial + baro_increment, 1.3f); const float terrain = _ekf->getTerrainVertPos(); EXPECT_NEAR(terrain, 0.f, 1.1f); // TODO: why? @@ -196,7 +201,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f); + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_rel_initial + baro_increment - gps_step, 0.2f); // and the innovations are close to zero EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f); @@ -367,6 +372,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) ResetLoggingChecker reset_logging_checker(_ekf); reset_logging_checker.capturePreResetState(); + const float baro_bias_prev = _ekf->getBaroBiasEstimatorStatus().bias; const float alt_increment = 4478.f; _ekf->setEkfGlobalOrigin(lat, lon, alt + alt_increment); @@ -376,9 +382,12 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) EXPECT_NEAR(_ekf->getPosition()(2), alt_increment, 1.f); reset_logging_checker.capturePostResetState(); - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() + alt_increment, 0.2f); + + // An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS) + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f); EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); } diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 1a132e5db509..1d1fdfb6ce8d 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -40,7 +40,6 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" -#include "test_helper/reset_logging_checker.h" class EKFYawEstimatorTest : public ::testing::Test { @@ -91,8 +90,6 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) // WHEN: the vehicle starts accelerating in the horizontal plane _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f)); _ekf->set_in_air_status(true); - ResetLoggingChecker reset_logging_checker(_ekf); - reset_logging_checker.capturePreResetState(); _sensor_simulator.runTrajectorySeconds(3.f); @@ -106,10 +103,12 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_NEAR(yaw_est, yaw, tolerance_rad); EXPECT_LT(yaw_est_var, tolerance_rad); - // 1 reset when starting GNSS aiding - reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); - EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); + EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[0], 0.5f); + EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[1], 0.5f); + + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[0], 0.1f); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[1], 0.1f); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[2], 0.1f); EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); From 13c5bd44127d13084b45d22450113a2b0002e7d1 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 28 Oct 2024 16:54:14 +0100 Subject: [PATCH 047/211] ekf-flow: do not reset position when resetting to flow Flow only provides velocity information --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 60238af10572..906e755a2feb 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -230,14 +230,6 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample) const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample); resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var); - // reset position, estimate is relative to initial position in this mode, so we start with zero error - if (!_control_status.flags.in_air) { - ECL_INFO("reset position to zero"); - //TODO: reset origin instead? - resetHorizontalPositionToLastKnown(); - // resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); - } - resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); _innov_check_fail_status.flags.reject_optflow_X = false; From a49245732283fb95bef9bab5aa6810ea4bbb25b0 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 25 Oct 2024 13:10:24 +0200 Subject: [PATCH 048/211] ekf2-derivation: build state struct based on type --- .../python/ekf_derivation/utils/derivation_utils.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py index faf431da7167..97a11524318c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py @@ -91,20 +91,21 @@ def generate_python_function(function_name, output_names): def build_state_struct(state, T="float"): out = "struct StateSample {\n" - def TypeFromLength(len): - if len == 1: + def get_px4_type(obj): + if isinstance(obj, sf.M11): return f"{T}" - elif len == 2: + elif isinstance(obj, sf.M21): return f"matrix::Vector2<{T}>" - elif len == 3: + elif isinstance(obj, sf.M31): return f"matrix::Vector3<{T}>" - elif len == 4: + elif isinstance(obj, sf.Rot3): return f"matrix::Quaternion<{T}>" else: + print(f"unknown type {type(obj)}") raise NotImplementedError for key, val in state.items(): - out += f"\t{TypeFromLength(val.storage_dim())} {key}{{}};\n" + out += f"\t{get_px4_type(val)} {key}{{}};\n" state_size = state.storage_dim() out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \ From 6becccb2175c7cc1e963080176db64472bceee6f Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 30 Oct 2024 10:24:58 +0100 Subject: [PATCH 049/211] update change indicator --- .../test/change_indication/ekf_gsf_reset.csv | 780 +++++++++--------- .../ekf2/test/change_indication/iris_gps.csv | 700 ++++++++-------- 2 files changed, 740 insertions(+), 740 deletions(-) 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 02d18bc9eea1..fcf346c1e89d 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.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 -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,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 -190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 -290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,0.1,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 -390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,0.1,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 -490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0.00024,0.00049,-0.011,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,0.1,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 -590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.1,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 -690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0.0005,0.00055,-0.013,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,0.1,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 -790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0.0012,0.0012,-0.014,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.018,0.018,0.00077,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 -890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0.00096,0.00072,-0.021,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.019,0.019,0.00051,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 -990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0.0022,0.0014,-0.029,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.021,0.021,0.00062,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 -1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0.0018,0.00086,-0.039,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.023,0.023,0.00043,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 -1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0.0035,0.0014,-0.051,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.025,0.025,0.00051,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 -1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0.0027,0.00089,-0.064,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.026,0.026,0.00038,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 -1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0.0051,0.0013,-0.078,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.028,0.028,0.00043,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 -1490000,1,-0.01,-0.014,0.00016,0.024,0.0029,-0.16,0.0038,0.00083,-0.093,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 -1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0.0065,0.0012,-0.11,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.03,0.03,0.00037,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 -1690000,1,-0.011,-0.014,0.00013,0.028,-0.00014,-0.19,0.0045,0.00062,-0.13,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 -1790000,1,-0.011,-0.014,9.8e-05,0.035,-0.002,-0.2,0.0076,0.00053,-0.15,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 -1890000,1,-0.011,-0.015,7.8e-05,0.043,-0.0033,-0.22,0.011,0.00027,-0.17,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 -1990000,1,-0.011,-0.014,8.9e-05,0.036,-0.0048,-0.23,0.0082,-0.00029,-0.19,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 -2090000,1,-0.011,-0.014,5.1e-05,0.041,-0.0073,-0.24,0.012,-0.00089,-0.22,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 -2190000,1,-0.011,-0.014,6.4e-05,0.033,-0.007,-0.26,0.0079,-0.00099,-0.24,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 -2290000,1,-0.011,-0.014,5.1e-05,0.039,-0.0095,-0.27,0.011,-0.0018,-0.27,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 -2390000,1,-0.011,-0.013,6.9e-05,0.03,-0.0089,-0.29,0.0074,-0.0015,-0.3,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 -2490000,1,-0.011,-0.013,5.1e-05,0.035,-0.011,-0.3,0.011,-0.0025,-0.32,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 -2590000,1,-0.011,-0.013,6.6e-05,0.026,-0.0093,-0.31,0.0068,-0.0018,-0.36,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 -2690000,1,-0.011,-0.013,6.3e-05,0.03,-0.011,-0.33,0.0097,-0.0028,-0.39,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 -2790000,1,-0.011,-0.013,5.8e-05,0.023,-0.0096,-0.34,0.0062,-0.002,-0.42,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 -2890000,1,-0.011,-0.013,8.3e-06,0.027,-0.012,-0.35,0.0087,-0.003,-0.46,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 -2990000,1,-0.011,-0.013,5.6e-05,0.022,-0.0098,-0.36,0.0057,-0.0021,-0.49,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 -3090000,1,-0.011,-0.013,6e-05,0.025,-0.011,-0.38,0.008,-0.0032,-0.53,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 -3190000,1,-0.011,-0.013,2.9e-06,0.02,-0.009,-0.39,0.0053,-0.0022,-0.57,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 -3290000,1,-0.011,-0.013,4.4e-05,0.023,-0.011,-0.4,0.0074,-0.0032,-0.61,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 -3390000,1,-0.011,-0.012,1.5e-05,0.018,-0.0095,-0.42,0.0049,-0.0022,-0.65,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 -3490000,1,-0.011,-0.013,8.2e-06,0.022,-0.012,-0.43,0.0069,-0.0033,-0.69,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 -3590000,1,-0.011,-0.012,3.2e-05,0.017,-0.011,-0.44,0.0047,-0.0023,-0.73,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 -3690000,1,-0.011,-0.012,0.00015,0.019,-0.014,-0.46,0.0065,-0.0036,-0.78,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 -3790000,1,-0.011,-0.012,0.0002,0.016,-0.014,-0.47,0.0044,-0.0027,-0.82,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 -3890000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.48,0.006,-0.0041,-0.87,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -3990000,1,-0.011,-0.012,0.00017,0.02,-0.017,-0.5,0.0079,-0.0057,-0.92,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -4090000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.51,0.0056,-0.0042,-0.97,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4190000,1,-0.011,-0.012,0.00014,0.02,-0.017,-0.53,0.0075,-0.0058,-1,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4290000,1,-0.01,-0.012,9.8e-05,0.017,-0.012,-0.54,0.0054,-0.0042,-1.1,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4390000,1,-0.01,-0.012,0.00012,0.018,-0.013,-0.55,0.0071,-0.0055,-1.1,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4490000,1,-0.01,-0.012,0.00018,0.014,-0.01,-0.57,0.0051,-0.0038,-1.2,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4590000,1,-0.01,-0.012,0.00021,0.017,-0.012,-0.58,0.0067,-0.0049,-1.2,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4690000,1,-0.01,-0.012,0.00021,0.014,-0.01,-0.6,0.0048,-0.0035,-1.3,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4790000,1,-0.01,-0.012,0.0002,0.015,-0.012,-0.61,0.0062,-0.0045,-1.4,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4890000,1,-0.01,-0.011,0.00019,0.012,-0.01,-0.63,0.0044,-0.0033,-1.4,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -4990000,1,-0.01,-0.012,0.00017,0.015,-0.011,-0.64,0.0058,-0.0043,-1.5,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5090000,1,-0.01,-0.011,0.00022,0.011,-0.0085,-0.66,0.0041,-0.0031,-1.6,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5190000,1,-0.01,-0.011,0.00024,0.013,-0.0098,-0.67,0.0053,-0.004,-1.6,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5290000,1,-0.01,-0.011,0.00023,0.0086,-0.0073,-0.68,0.0037,-0.0029,-1.7,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5390000,1,-0.0099,-0.011,0.00029,0.0081,-0.0081,-0.7,0.0045,-0.0036,-1.8,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5490000,1,-0.0098,-0.011,0.0003,0.0055,-0.0062,-0.71,0.0031,-0.0026,-1.8,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5590000,1,-0.0097,-0.011,0.00028,0.0061,-0.0066,-0.73,0.0036,-0.0032,-1.9,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5690000,1,-0.0096,-0.011,0.00036,0.0041,-0.0038,-0.74,0.0025,-0.0022,-2,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5790000,1,-0.0095,-0.011,0.00035,0.0044,-0.0028,-0.75,0.0029,-0.0025,-2,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5890000,1,-0.0095,-0.011,0.00033,0.0038,-0.00092,0.0028,0.002,-0.0016,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-3.6e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5990000,1,-0.0094,-0.011,0.00036,0.0041,0.00056,0.015,0.0023,-0.0016,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-3.6e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -6090000,1,-0.0094,-0.011,0.00034,0.0051,0.0017,-0.011,0.0028,-0.0015,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-3.6e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6190000,1,-0.0094,-0.011,0.00027,0.0038,0.0042,-0.005,0.002,-0.00053,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-3.6e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6290000,1,-0.0094,-0.011,0.00024,0.005,0.0043,-0.012,0.0025,-0.00012,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-3.6e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6390000,1,-0.0093,-0.011,0.00026,0.0043,0.0053,-0.05,0.0019,0.00038,-3.7e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-3.6e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6490000,1,-0.0093,-0.011,0.00026,0.0049,0.0055,-0.052,0.0023,0.00093,-3.7e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-3.6e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6590000,1,-0.0093,-0.011,0.00019,0.0037,0.0055,-0.099,0.0018,0.001,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-3.6e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6690000,1,-0.0093,-0.011,0.00012,0.0046,0.0051,-0.076,0.0022,0.0015,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-3.6e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0051,-0.11,0.0028,0.0015,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-6.9e-05,0.21,8e-05,0.43,3.6e-05,0.00032,-0.0024,0,0,-3.6e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 -6890000,0.71,0.0013,-0.014,0.7,0.00053,0.0064,-0.12,0.0015,0.0026,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-9.7e-05,0.21,1.3e-05,0.43,1.3e-06,0.00089,-0.00086,0,0,-3.6e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 -6990000,0.71,0.0013,-0.014,0.71,-0.00023,0.0046,-0.12,0.0011,0.0017,-3.7e+02,-0.0015,-0.0057,-7.6e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-3.6e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 -7090000,0.71,0.0012,-0.014,0.71,-0.00079,0.0011,-0.13,0.0011,0.00014,-3.7e+02,-0.0014,-0.0057,-7.7e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00037,0.00025,-0.00041,0,0,-3.6e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 -7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0019,-0.15,0.00089,0.0011,-3.7e+02,-0.0015,-0.0057,-7.7e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-3.6e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 -7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.0099,-0.15,0.00034,0.0079,-3.7e+02,-0.0016,-0.0057,-7.3e-05,-0.00021,-0.0002,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00039,0,0,-3.6e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0.00062,0.012,-3.7e+02,-0.0016,-0.0057,-7.1e-05,-0.00028,-3.8e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00032,0,0,-3.6e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.011,-0.16,0.0014,0.0094,-3.7e+02,-0.0016,-0.0057,-7.2e-05,-0.00018,-7.3e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00078,-0.00043,0,0,-3.6e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7590000,0.71,0.0016,-0.014,0.71,-0.0034,0.02,-0.17,0.0033,0.018,-3.7e+02,-0.0017,-0.0057,-6.8e-05,-0.00024,0.00021,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-3.6e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7690000,0.71,0.0016,-0.014,0.71,-0.0046,0.017,-0.16,0.0032,0.016,-3.7e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00032,0.00084,-0.00033,0,0,-3.6e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 -7790000,0.71,0.0017,-0.014,0.71,-0.011,0.016,-0.16,-0.0031,0.016,-3.7e+02,-0.0016,-0.0057,-7.3e-05,-0.00036,6.2e-05,-0.0071,0.21,-1.6e-05,0.43,-0.00034,0.00078,-0.00038,0,0,-3.6e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 -7890000,0.71,0.0017,-0.014,0.71,-0.011,0.021,-0.16,-0.00066,0.02,-3.7e+02,-0.0016,-0.0057,-7.1e-05,-0.00038,0.0002,-0.0096,0.21,-1.5e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-3.6e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 -7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.022,-0.16,0.0022,0.022,-3.7e+02,-0.0016,-0.0056,-6.9e-05,-0.00037,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00085,-0.00038,0,0,-3.6e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 -8090000,0.71,0.0017,-0.014,0.71,-0.005,0.024,-0.17,0.0082,0.024,-3.7e+02,-0.0016,-0.0056,-6.7e-05,-0.0003,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00033,0.0009,-0.00035,0,0,-3.6e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8190000,0.71,0.0017,-0.014,0.71,-0.014,0.028,-0.18,-6.4e-05,0.029,-3.7e+02,-0.0016,-0.0056,-6.9e-05,-0.00043,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-3.6e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8290000,0.71,0.0017,-0.014,0.71,-0.017,0.021,-0.17,-0.0058,0.022,-3.7e+02,-0.0016,-0.0057,-7.2e-05,-0.00056,0.00031,-0.017,0.21,-1e-05,0.43,-0.00031,0.00079,-0.00035,0,0,-3.6e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00041,-0.17,0.0038,0.023,-3.7e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.021,0.21,-9.3e-06,0.43,-0.0003,0.00084,-0.00031,0,0,-3.6e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8490000,0.71,0.0017,-0.014,0.71,-0.003,0.0023,-0.17,0.0036,0.023,-3.7e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.025,0.21,-9e-06,0.43,-0.00031,0.0008,-0.00028,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8590000,0.71,0.0021,-0.014,0.71,-0.00044,0.00093,-0.17,0.0032,0.023,-3.7e+02,-0.0018,-0.0057,-6.8e-05,-0.00051,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00042,0.00076,-0.00028,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8690000,0.71,0.002,-0.014,0.71,-0.0034,0.0028,-0.16,0.003,0.023,-3.7e+02,-0.0017,-0.0056,-6.6e-05,-0.00051,0.00039,-0.035,0.21,-1e-05,0.43,-0.00039,0.00084,-0.00029,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.088,5.8e-05,6.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8790000,0.71,0.0019,-0.014,0.71,-0.0057,0.0049,-0.15,0.0026,0.024,-3.7e+02,-0.0017,-0.0057,-6.9e-05,-0.0005,0.00043,-0.041,0.21,-9.1e-06,0.43,-0.00037,0.0008,-0.00029,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.087,5.6e-05,6.6e-05,2.4e-06,0.04,0.04,0.032,0.0013,6.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8890000,0.71,0.0019,-0.014,0.71,-0.0079,0.0055,-0.15,0.0019,0.024,-3.7e+02,-0.0016,-0.0057,-7.1e-05,-0.00063,0.00042,-0.045,0.21,-7.8e-06,0.43,-0.00034,0.00078,-0.00033,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.086,5.3e-05,6.4e-05,2.4e-06,0.04,0.04,0.03,0.0013,6.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -8990000,0.71,0.0018,-0.014,0.71,-0.01,0.0049,-0.14,0.00085,0.024,-3.7e+02,-0.0015,-0.0058,-7.5e-05,-0.00087,0.00044,-0.051,0.21,-6.6e-06,0.43,-0.0003,0.00071,-0.00034,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.087,5.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.029,0.0013,6.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9090000,0.71,0.002,-0.014,0.71,-0.014,0.006,-0.14,-0.00063,0.025,-3.7e+02,-0.0016,-0.0058,-7.6e-05,-0.00098,0.00054,-0.053,0.21,-6.9e-06,0.43,-0.00033,0.00068,-0.00034,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1.1e+02,1.1e+02,0.086,4.9e-05,6.2e-05,2.4e-06,0.04,0.04,0.028,0.0013,5.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9190000,0.71,0.0019,-0.014,0.71,-0.012,0.0087,-0.14,-0.0008,0.026,-3.7e+02,-0.0015,-0.0056,-7e-05,-0.00086,0.00059,-0.057,0.21,-6.2e-06,0.43,-0.00031,0.00081,-0.00025,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.094,1.1e+02,1.1e+02,0.085,4.6e-05,6e-05,2.4e-06,0.04,0.04,0.027,0.0013,5.6e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9290000,0.71,0.0017,-0.014,0.71,-0.011,0.0086,-0.14,-0.0015,0.026,-3.7e+02,-0.0015,-0.0056,-7.1e-05,-0.00096,0.00059,-0.061,0.21,-5.1e-06,0.43,-0.00026,0.00082,-0.00024,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.093,1.1e+02,1.1e+02,0.085,4.4e-05,5.8e-05,2.4e-06,0.04,0.04,0.025,0.0013,5.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9390000,0.71,0.0015,-0.014,0.71,-0.011,0.0092,-0.14,-0.0023,0.027,-3.7e+02,-0.0014,-0.0056,-7.3e-05,-0.001,0.00058,-0.065,0.21,-4.3e-06,0.43,-0.00022,0.00082,-0.00022,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.093,1.2e+02,1.2e+02,0.086,4.2e-05,5.7e-05,2.4e-06,0.04,0.04,0.024,0.0013,5.2e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9490000,0.71,0.0014,-0.014,0.71,-0.0078,0.011,-0.13,-0.0012,0.028,-3.7e+02,-0.0014,-0.0055,-6.9e-05,-0.001,0.0006,-0.068,0.21,-3.7e-06,0.43,-0.00018,0.0009,-0.00013,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.091,1.2e+02,1.2e+02,0.085,4e-05,5.5e-05,2.4e-06,0.04,0.04,0.023,0.0013,5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9590000,0.71,0.0016,-0.014,0.71,-0.01,0.016,-0.13,-0.0026,0.031,-3.7e+02,-0.0015,-0.0054,-6.6e-05,-0.0011,0.00079,-0.072,0.21,-4.5e-06,0.43,-0.00024,0.00091,-0.00012,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.09,1.3e+02,1.3e+02,0.085,3.8e-05,5.4e-05,2.4e-06,0.04,0.04,0.021,0.0013,4.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9690000,0.71,0.0018,-0.014,0.71,-0.017,0.017,-0.12,-0.0071,0.032,-3.7e+02,-0.0015,-0.0056,-7e-05,-0.0013,0.00092,-0.077,0.21,-4.8e-06,0.43,-0.00027,0.0008,-0.00017,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.089,1.3e+02,1.3e+02,0.086,3.6e-05,5.2e-05,2.4e-06,0.04,0.04,0.02,0.0013,4.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9790000,0.71,0.0017,-0.014,0.71,-0.013,0.02,-0.11,-0.0063,0.034,-3.7e+02,-0.0015,-0.0055,-6.8e-05,-0.0014,0.001,-0.082,0.21,-4.4e-06,0.43,-0.00024,0.00085,-9.7e-05,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.087,1.4e+02,1.4e+02,0.085,3.4e-05,5e-05,2.4e-06,0.04,0.04,0.019,0.0013,4.5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9890000,0.71,0.0018,-0.014,0.71,-0.016,0.023,-0.11,-0.0088,0.037,-3.7e+02,-0.0015,-0.0055,-6.8e-05,-0.0015,0.0011,-0.085,0.21,-4.5e-06,0.43,-0.00026,0.00083,-0.0001,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.084,1.4e+02,1.4e+02,0.085,3.2e-05,4.9e-05,2.4e-06,0.04,0.04,0.018,0.0013,4.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9990000,0.71,0.0019,-0.014,0.71,-0.02,0.021,-0.1,-0.012,0.037,-3.7e+02,-0.0015,-0.0056,-7.1e-05,-0.0016,0.0011,-0.089,0.21,-4.2e-06,0.43,-0.00025,0.00078,-0.00013,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.083,1.5e+02,1.5e+02,0.086,3e-05,4.7e-05,2.4e-06,0.04,0.04,0.017,0.0013,4.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -10090000,0.71,0.002,-0.014,0.71,-0.022,0.025,-0.096,-0.015,0.042,-3.7e+02,-0.0015,-0.0056,-6.9e-05,-0.0017,0.0013,-0.091,0.21,-4.6e-06,0.43,-0.00029,0.00079,-0.00012,0,0,-3.6e+02,0.0013,0.0012,0.039,25,25,0.08,1.6e+02,1.6e+02,0.085,2.9e-05,4.6e-05,2.4e-06,0.04,0.04,0.016,0.0013,4.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10190000,0.71,0.002,-0.013,0.71,-0.033,0.022,-0.096,-0.025,0.04,-3.7e+02,-0.0015,-0.0057,-7.5e-05,-0.0019,0.0013,-0.093,0.21,-4.5e-06,0.43,-0.00027,0.00065,-0.00015,0,0,-3.6e+02,0.0012,0.0012,0.039,25,25,0.078,1.7e+02,1.7e+02,0.084,2.7e-05,4.4e-05,2.4e-06,0.04,0.04,0.014,0.0013,4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10290000,0.71,0.002,-0.013,0.71,-0.04,0.017,-0.084,-0.033,0.038,-3.7e+02,-0.0015,-0.0058,-7.8e-05,-0.0021,0.0014,-0.098,0.21,-4.2e-06,0.43,-0.00026,0.00058,-0.00017,0,0,-3.6e+02,0.0012,0.0012,0.039,25,25,0.076,1.8e+02,1.8e+02,0.085,2.6e-05,4.2e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10390000,0.71,0.0018,-0.013,0.71,0.0091,-0.02,0.0093,0.00085,-0.0018,-3.7e+02,-0.0015,-0.0058,-7.9e-05,-0.0021,0.0013,-0.098,0.21,-3.7e-06,0.43,-0.00023,0.0006,-0.00014,0,0,-3.6e+02,0.0012,0.0012,0.039,0.25,0.25,0.56,0.25,0.25,0.077,2.4e-05,4.1e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10490000,0.71,0.0018,-0.013,0.71,0.0071,-0.019,0.022,0.0016,-0.0038,-3.7e+02,-0.0014,-0.0059,-8.3e-05,-0.0022,0.0013,-0.098,0.21,-3.3e-06,0.43,-0.00021,0.00053,-0.00016,0,0,-3.6e+02,0.0012,0.0012,0.039,0.26,0.26,0.55,0.26,0.26,0.079,2.3e-05,3.9e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10590000,0.71,0.0022,-0.013,0.71,0.006,-0.0081,0.024,0.0018,-0.00082,-3.7e+02,-0.0016,-0.0059,-7.9e-05,-0.0024,0.0018,-0.098,0.21,-4.5e-06,0.43,-0.00029,0.00053,-0.00017,0,0,-3.6e+02,0.0012,0.0011,0.039,0.13,0.13,0.27,0.13,0.13,0.072,2.2e-05,3.8e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10690000,0.71,0.0021,-0.013,0.71,0.0032,-0.009,0.027,0.0023,-0.0017,-3.7e+02,-0.0015,-0.0059,-8.1e-05,-0.0024,0.0018,-0.098,0.21,-4.1e-06,0.43,-0.00027,0.00052,-0.00017,0,0,-3.6e+02,0.0012,0.0011,0.039,0.14,0.14,0.26,0.14,0.14,0.077,2.1e-05,3.6e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10790000,0.71,0.002,-0.013,0.71,0.0033,-0.0063,0.021,0.0026,-0.00085,-3.7e+02,-0.0015,-0.0059,-8e-05,-0.0025,0.0021,-0.098,0.21,-4e-06,0.43,-0.00026,0.00056,-0.00016,0,0,-3.6e+02,0.0012,0.0011,0.038,0.093,0.093,0.17,0.09,0.09,0.071,1.9e-05,3.4e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10890000,0.71,0.0019,-0.013,0.71,0.0021,-0.0065,0.016,0.0029,-0.0015,-3.7e+02,-0.0015,-0.0058,-8.1e-05,-0.0025,0.0021,-0.098,0.21,-3.7e-06,0.43,-0.00025,0.00057,-0.00015,0,0,-3.6e+02,0.0011,0.0011,0.038,0.1,0.1,0.16,0.096,0.096,0.074,1.8e-05,3.3e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -10990000,0.71,0.0017,-0.014,0.71,0.0067,-0.0015,0.011,0.0048,-0.0026,-3.7e+02,-0.0013,-0.0057,-7.9e-05,-0.0025,0.0031,-0.098,0.21,-3.2e-06,0.43,-0.00022,0.00071,-0.00011,0,0,-3.6e+02,0.0011,0.001,0.038,0.079,0.08,0.12,0.072,0.072,0.071,1.7e-05,3.1e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11090000,0.71,0.0017,-0.014,0.71,0.0067,0.0018,0.014,0.0057,-0.0023,-3.7e+02,-0.0014,-0.0056,-7.5e-05,-0.0023,0.0029,-0.098,0.21,-3.3e-06,0.43,-0.00023,0.00078,-6.8e-05,0,0,-3.6e+02,0.0011,0.00099,0.038,0.09,0.091,0.12,0.078,0.078,0.073,1.6e-05,2.9e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11190000,0.71,0.0017,-0.014,0.71,0.01,0.0033,0.0027,0.0067,-0.0032,-3.7e+02,-0.0013,-0.0056,-7.8e-05,-0.0022,0.004,-0.098,0.21,-3.4e-06,0.43,-0.00024,0.00079,-8.6e-05,0,0,-3.6e+02,0.00098,0.00091,0.038,0.073,0.074,0.091,0.062,0.062,0.068,1.5e-05,2.7e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11290000,0.71,0.0016,-0.014,0.71,0.0093,0.0017,0.0014,0.0076,-0.0034,-3.7e+02,-0.0013,-0.0057,-8.3e-05,-0.0026,0.0043,-0.098,0.21,-3e-06,0.43,-0.00022,0.00073,-0.00011,0,0,-3.6e+02,0.00097,0.0009,0.038,0.085,0.087,0.087,0.068,0.068,0.071,1.4e-05,2.6e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11390000,0.71,0.0016,-0.013,0.71,0.0047,0.0008,-0.0044,0.0055,-0.003,-3.7e+02,-0.0013,-0.0058,-8.6e-05,-0.0033,0.0041,-0.098,0.21,-3e-06,0.43,-0.00021,0.00066,-0.00016,0,0,-3.6e+02,0.00086,0.00082,0.038,0.071,0.073,0.072,0.056,0.056,0.067,1.3e-05,2.4e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11490000,0.71,0.0014,-0.013,0.71,0.00093,-0.0021,-0.0044,0.0055,-0.0039,-3.7e+02,-0.0012,-0.0059,-9.3e-05,-0.0039,0.0049,-0.098,0.21,-2.7e-06,0.43,-0.00018,0.00056,-0.00022,0,0,-3.6e+02,0.00086,0.00081,0.038,0.083,0.085,0.069,0.062,0.062,0.068,1.3e-05,2.3e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11590000,0.71,0.0013,-0.013,0.71,-0.0022,-0.0016,-0.01,0.0044,-0.0032,-3.7e+02,-0.0012,-0.0059,-9.4e-05,-0.0045,0.0051,-0.098,0.21,-2.5e-06,0.43,-0.00016,0.00054,-0.00023,0,0,-3.6e+02,0.00075,0.00072,0.038,0.07,0.072,0.059,0.052,0.052,0.066,1.2e-05,2.1e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11690000,0.71,0.0011,-0.013,0.71,-0.0037,-0.0027,-0.016,0.0044,-0.004,-3.7e+02,-0.0011,-0.006,-9.8e-05,-0.0053,0.0052,-0.098,0.21,-2e-06,0.43,-0.0001,0.00053,-0.00026,0,0,-3.6e+02,0.00075,0.00071,0.038,0.081,0.084,0.057,0.059,0.059,0.066,1.1e-05,2e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11790000,0.71,0.0011,-0.013,0.71,-0.0077,-0.00091,-0.018,0.0024,-0.002,-3.7e+02,-0.0011,-0.006,-9.9e-05,-0.0071,0.0054,-0.098,0.21,-2e-06,0.43,-8.7e-05,0.0005,-0.00027,0,0,-3.6e+02,0.00065,0.00063,0.038,0.068,0.07,0.051,0.05,0.05,0.063,1e-05,1.8e-05,2.3e-06,0.028,0.03,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11890000,0.71,0.00095,-0.013,0.71,-0.0087,-0.0038,-0.02,0.0016,-0.0027,-3.7e+02,-0.001,-0.006,-0.0001,-0.0077,0.0059,-0.098,0.21,-2e-06,0.43,-5.6e-05,0.00047,-0.00032,0,0,-3.6e+02,0.00065,0.00062,0.038,0.079,0.082,0.049,0.057,0.057,0.063,9.8e-06,1.8e-05,2.3e-06,0.028,0.029,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11990000,0.71,0.0013,-0.013,0.71,-0.012,0.0011,-0.026,-0.0001,-7.8e-05,-3.7e+02,-0.0011,-0.006,-9.9e-05,-0.0073,0.0065,-0.097,0.21,-2.9e-06,0.43,-0.00013,0.00048,-0.00032,0,0,-3.6e+02,0.00056,0.00055,0.037,0.066,0.068,0.045,0.049,0.049,0.062,9.2e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -12090000,0.71,0.0014,-0.013,0.71,-0.013,0.0043,-0.032,-0.0014,0.00091,-3.7e+02,-0.0012,-0.0059,-9.5e-05,-0.0063,0.0057,-0.097,0.21,-3e-06,0.43,-0.00016,0.00053,-0.00028,0,0,-3.6e+02,0.00056,0.00055,0.037,0.077,0.079,0.045,0.056,0.056,0.062,8.8e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12190000,0.71,0.0012,-0.013,0.71,-0.0069,0.0039,-0.026,0.0015,0.00037,-3.7e+02,-0.0011,-0.0059,-9.6e-05,-0.0061,0.006,-0.099,0.21,-2.4e-06,0.43,-0.00011,0.0006,-0.00026,0,0,-3.6e+02,0.00048,0.00048,0.037,0.064,0.066,0.041,0.048,0.048,0.059,8.2e-06,1.4e-05,2.3e-06,0.023,0.026,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12290000,0.71,0.001,-0.013,0.71,-0.0081,0.0044,-0.026,0.0011,0.00072,-3.7e+02,-0.0011,-0.0059,-9.6e-05,-0.0067,0.0056,-0.099,0.21,-2e-06,0.43,-8.3e-05,0.0006,-0.00026,0,0,-3.6e+02,0.00048,0.00048,0.037,0.073,0.076,0.042,0.055,0.056,0.06,7.9e-06,1.4e-05,2.3e-06,0.023,0.025,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12390000,0.71,0.001,-0.013,0.71,-0.0054,0.0032,-0.023,0.0025,0.00024,-3.7e+02,-0.0011,-0.0059,-9.9e-05,-0.0053,0.0073,-0.1,0.21,-2.5e-06,0.43,-0.00012,0.00061,-0.00025,0,0,-3.6e+02,0.00042,0.00043,0.037,0.061,0.063,0.039,0.048,0.048,0.058,7.4e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12490000,0.71,0.00094,-0.013,0.71,-0.0071,0.0027,-0.027,0.0017,3.4e-05,-3.7e+02,-0.0011,-0.0059,-0.0001,-0.0058,0.0085,-0.1,0.21,-2.7e-06,0.43,-0.00015,0.00055,-0.00025,0,0,-3.6e+02,0.00042,0.00042,0.037,0.069,0.072,0.04,0.055,0.055,0.058,7.1e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12590000,0.71,0.0011,-0.013,0.71,-0.014,0.0037,-0.033,-0.0033,0.00021,-3.7e+02,-0.0011,-0.006,-0.0001,-0.0051,0.0079,-0.1,0.21,-3e-06,0.43,-0.00019,0.00051,-0.00024,0,0,-3.6e+02,0.00037,0.00038,0.037,0.058,0.059,0.038,0.047,0.047,0.057,6.8e-06,1.2e-05,2.3e-06,0.019,0.022,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12690000,0.71,0.0014,-0.013,0.71,-0.017,0.0046,-0.037,-0.0058,0.00079,-3.7e+02,-0.0012,-0.006,-0.0001,-0.0033,0.0092,-0.1,0.21,-3.9e-06,0.43,-0.00026,0.0005,-0.00025,0,0,-3.6e+02,0.00037,0.00038,0.037,0.065,0.067,0.039,0.055,0.055,0.057,6.5e-06,1.1e-05,2.3e-06,0.019,0.022,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12790000,0.71,0.0012,-0.013,0.71,-0.02,0.0027,-0.041,-0.0081,0.00025,-3.7e+02,-0.0012,-0.006,-0.0001,-0.0049,0.008,-0.1,0.21,-3.3e-06,0.43,-0.0002,0.00048,-0.00027,0,0,-3.6e+02,0.00033,0.00034,0.037,0.054,0.056,0.037,0.047,0.047,0.056,6.2e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12890000,0.71,0.0011,-0.013,0.71,-0.02,0.0014,-0.039,-0.0096,0.00019,-3.7e+02,-0.0011,-0.006,-0.0001,-0.0062,0.0077,-0.1,0.21,-2.9e-06,0.43,-0.00017,0.00047,-0.00026,0,0,-3.6e+02,0.00033,0.00034,0.037,0.061,0.063,0.038,0.054,0.055,0.057,6e-06,1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -12990000,0.71,0.001,-0.013,0.71,-0.009,0.0019,-0.039,-0.0018,0.0005,-3.7e+02,-0.0011,-0.0059,-9.9e-05,-0.0038,0.0081,-0.1,0.21,-2.7e-06,0.43,-0.00018,0.00058,-0.0002,0,0,-3.6e+02,0.0003,0.00031,0.037,0.051,0.053,0.037,0.047,0.047,0.055,5.7e-06,9.9e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13090000,0.71,0.00095,-0.013,0.71,-0.0097,-0.00015,-0.039,-0.0029,-7.2e-05,-3.7e+02,-0.0011,-0.006,-0.0001,-0.005,0.0097,-0.1,0.21,-3.1e-06,0.43,-0.0002,0.00052,-0.00023,0,0,-3.6e+02,0.0003,0.00031,0.037,0.057,0.059,0.038,0.054,0.054,0.056,5.5e-06,9.6e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13190000,0.71,0.00094,-0.013,0.71,-0.0023,0.00081,-0.034,0.0028,0.00014,-3.7e+02,-0.0011,-0.006,-0.00011,-0.0032,0.011,-0.11,0.21,-3.4e-06,0.43,-0.00023,0.00056,-0.00021,0,0,-3.6e+02,0.00027,0.00028,0.037,0.048,0.05,0.037,0.047,0.047,0.054,5.2e-06,9.1e-06,2.3e-06,0.015,0.019,0.0098,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13290000,0.71,0.00079,-0.013,0.71,-0.00079,0.0015,-0.029,0.0034,0.0003,-3.7e+02,-0.001,-0.0059,-0.0001,-0.0047,0.0096,-0.11,0.21,-2.5e-06,0.43,-0.00018,0.00058,-0.0002,0,0,-3.6e+02,0.00027,0.00028,0.037,0.054,0.055,0.038,0.054,0.054,0.055,5.1e-06,8.9e-06,2.3e-06,0.015,0.018,0.0097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13390000,0.71,0.0007,-0.013,0.71,0.00023,0.0023,-0.024,0.0025,0.00042,-3.7e+02,-0.001,-0.0059,-0.0001,-0.0041,0.0089,-0.11,0.21,-2.3e-06,0.43,-0.00016,0.00063,-0.00019,0,0,-3.6e+02,0.00025,0.00026,0.037,0.045,0.047,0.037,0.047,0.047,0.054,4.8e-06,8.5e-06,2.3e-06,0.014,0.018,0.0091,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13490000,0.71,0.00071,-0.013,0.71,0.00021,0.0024,-0.022,0.0027,0.0008,-3.7e+02,-0.001,-0.0059,-0.0001,-0.0042,0.0081,-0.11,0.21,-2e-06,0.43,-0.00015,0.00063,-0.00017,0,0,-3.6e+02,0.00025,0.00026,0.037,0.05,0.052,0.038,0.054,0.054,0.055,4.7e-06,8.2e-06,2.3e-06,0.014,0.017,0.009,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13590000,0.71,0.00072,-0.013,0.71,-2.7e-05,0.0028,-0.024,0.0015,0.00038,-3.7e+02,-0.001,-0.0059,-0.0001,-0.004,0.0094,-0.11,0.21,-2.4e-06,0.43,-0.00017,0.00062,-0.00019,0,0,-3.6e+02,0.00023,0.00025,0.037,0.042,0.044,0.037,0.046,0.047,0.054,4.5e-06,7.9e-06,2.3e-06,0.013,0.017,0.0085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13690000,0.71,0.00071,-0.013,0.71,0.00091,0.0053,-0.029,0.0017,0.0011,-3.7e+02,-0.001,-0.0059,-9.9e-05,-0.0034,0.0082,-0.11,0.21,-2.1e-06,0.43,-0.00016,0.00064,-0.00016,0,0,-3.6e+02,0.00023,0.00024,0.037,0.047,0.049,0.038,0.053,0.054,0.055,4.3e-06,7.7e-06,2.3e-06,0.013,0.017,0.0083,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13790000,0.71,0.00075,-0.013,0.71,0.0006,0.0021,-0.03,0.0024,-0.00073,-3.7e+02,-0.0011,-0.0059,-9.9e-05,-0.002,0.0089,-0.11,0.21,-2.5e-06,0.43,-0.00019,0.00065,-0.00015,0,0,-3.6e+02,0.00022,0.00023,0.037,0.04,0.041,0.036,0.046,0.046,0.053,4.2e-06,7.3e-06,2.3e-06,0.012,0.016,0.0078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13890000,0.71,0.00059,-0.013,0.71,0.0024,0.0022,-0.035,0.003,-0.00056,-3.7e+02,-0.001,-0.0059,-9.8e-05,-0.0034,0.0078,-0.11,0.21,-1.8e-06,0.43,-0.00015,0.00065,-0.00015,0,0,-3.6e+02,0.00022,0.00023,0.037,0.044,0.045,0.037,0.053,0.053,0.055,4e-06,7.1e-06,2.3e-06,0.012,0.016,0.0076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13990000,0.71,0.00066,-0.013,0.71,0.0019,0.00054,-0.034,0.0036,-0.0019,-3.7e+02,-0.001,-0.0059,-9.8e-05,-0.0021,0.0083,-0.11,0.21,-2e-06,0.43,-0.00018,0.00065,-0.00013,0,0,-3.6e+02,0.0002,0.00022,0.037,0.037,0.039,0.036,0.046,0.046,0.054,3.9e-06,6.8e-06,2.3e-06,0.012,0.015,0.0071,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -14090000,0.71,0.00072,-0.013,0.71,0.0014,0.0019,-0.035,0.0036,-0.0013,-3.7e+02,-0.0011,-0.0059,-9.4e-05,-0.00063,0.0073,-0.11,0.21,-1.8e-06,0.43,-0.00018,0.00069,-9.7e-05,0,0,-3.6e+02,0.00021,0.00022,0.037,0.041,0.043,0.036,0.053,0.053,0.054,3.8e-06,6.7e-06,2.3e-06,0.012,0.015,0.007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14190000,0.71,0.00068,-0.013,0.71,0.0044,0.0019,-0.037,0.0057,-0.001,-3.7e+02,-0.0011,-0.0059,-9.3e-05,-0.00017,0.007,-0.11,0.21,-1.6e-06,0.43,-0.00018,0.00071,-8e-05,0,0,-3.6e+02,0.0002,0.00021,0.037,0.035,0.037,0.035,0.046,0.046,0.054,3.6e-06,6.4e-06,2.3e-06,0.011,0.015,0.0065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14290000,0.71,0.00075,-0.013,0.71,0.0046,0.0032,-0.035,0.0059,-0.00065,-3.7e+02,-0.0011,-0.0058,-9.2e-05,0.00068,0.007,-0.11,0.21,-1.8e-06,0.43,-0.00019,0.00071,-6.2e-05,0,0,-3.6e+02,0.0002,0.0002,0.037,0.038,0.04,0.036,0.052,0.052,0.055,3.5e-06,6.2e-06,2.3e-06,0.011,0.014,0.0063,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14390000,0.71,0.00064,-0.014,0.71,0.0071,0.005,-0.037,0.0077,-0.00036,-3.7e+02,-0.0011,-0.0058,-8.9e-05,0.00035,0.0055,-0.11,0.21,-9.2e-07,0.43,-0.00015,0.00074,-4.6e-05,0,0,-3.6e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14490000,0.71,0.00053,-0.013,0.71,0.008,0.0065,-0.041,0.0088,0.00011,-3.7e+02,-0.001,-0.0058,-8.9e-05,-0.00096,0.005,-0.11,0.21,-4.4e-07,0.43,-0.00013,0.0007,-4.6e-05,0,0,-3.6e+02,0.00019,0.00019,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14590000,0.71,0.00042,-0.013,0.71,0.0061,0.005,-0.041,0.006,-0.0015,-3.7e+02,-0.001,-0.0058,-8.9e-05,-0.0018,0.0047,-0.11,0.21,-3.4e-07,0.43,-0.00012,0.00067,-5.9e-05,0,0,-3.6e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14690000,0.71,0.00037,-0.013,0.71,0.0079,0.0031,-0.038,0.0068,-0.00088,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.0018,0.0038,-0.11,0.21,3.8e-08,0.43,-0.0001,0.00068,-4.5e-05,0,0,-3.6e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14790000,0.71,0.00035,-0.013,0.71,0.0055,0.0016,-0.033,0.0042,-0.0021,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.002,0.0037,-0.12,0.21,-2e-08,0.43,-0.00011,0.00066,-4.6e-05,0,0,-3.6e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0097,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0034,-0.037,0.005,-0.0018,-3.7e+02,-0.001,-0.0058,-8.6e-05,-0.0022,0.0032,-0.12,0.21,2.4e-07,0.43,-9.1e-05,0.00067,-4.3e-05,0,0,-3.6e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0096,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -14990000,0.71,0.00027,-0.013,0.71,0.0068,0.0024,-0.032,0.004,-0.0017,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.0028,0.0037,-0.12,0.21,4.6e-08,0.43,-0.0001,0.00064,-5.6e-05,0,0,-3.6e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.012,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15090000,0.71,0.0002,-0.013,0.71,0.0075,0.0025,-0.035,0.0047,-0.0016,-3.7e+02,-0.001,-0.0058,-8.8e-05,-0.0028,0.0041,-0.12,0.21,-5.6e-08,0.43,-0.00011,0.00063,-5.7e-05,0,0,-3.6e+02,0.00017,0.00017,0.037,0.03,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0092,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15190000,0.71,0.00017,-0.013,0.7,0.0073,0.003,-0.033,0.0039,-0.0015,-3.7e+02,-0.001,-0.0058,-9e-05,-0.0035,0.0044,-0.12,0.21,-5.2e-08,0.43,-0.00012,0.00059,-6.4e-05,0,0,-3.6e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.009,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15290000,0.71,0.00019,-0.013,0.7,0.0077,0.0041,-0.03,0.0044,-0.00097,-3.7e+02,-0.001,-0.0058,-8.8e-05,-0.0026,0.0042,-0.12,0.21,-8.2e-09,0.43,-0.00014,0.00059,-3.9e-05,0,0,-3.6e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15390000,0.71,0.0002,-0.013,0.7,0.007,0.0054,-0.028,0.0017,-0.00041,-3.7e+02,-0.001,-0.0058,-8.3e-05,-0.0018,0.0027,-0.12,0.21,3.4e-07,0.43,-0.00012,0.00062,-1.6e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15490000,0.71,0.00022,-0.013,0.7,0.0086,0.0045,-0.028,0.0024,-0.00036,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.0022,0.004,-0.12,0.21,-1.1e-07,0.43,-0.00014,0.00059,-3.5e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15590000,0.71,0.00018,-0.013,0.71,0.0072,0.0036,-0.027,8.4e-05,-0.00068,-3.7e+02,-0.001,-0.0059,-8.9e-05,-0.0028,0.0047,-0.12,0.21,-4.6e-07,0.43,-0.00014,0.00058,-6.1e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0037,-0.028,0.00059,-0.00037,-3.7e+02,-0.001,-0.0059,-8.9e-05,-0.0023,0.0052,-0.12,0.21,-7.6e-07,0.43,-0.00015,0.00059,-6.4e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15790000,0.71,0.00019,-0.013,0.7,0.0081,0.0021,-0.03,0.00049,-0.0016,-3.7e+02,-0.001,-0.0059,-8.9e-05,-0.0026,0.0054,-0.12,0.21,-8.8e-07,0.43,-0.00016,0.00057,-6.8e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15890000,0.71,0.0002,-0.013,0.71,0.0088,0.0021,-0.028,0.0011,-0.0014,-3.7e+02,-0.0011,-0.0059,-8.8e-05,-0.0018,0.0057,-0.12,0.21,-1.1e-06,0.43,-0.00018,0.00057,-6e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.1e-06,3.8e-06,2.3e-06,0.008,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15990000,0.71,0.00018,-0.013,0.71,0.0086,0.0022,-0.024,0.00076,-0.0019,-3.7e+02,-0.0011,-0.0059,-8.4e-05,-0.0012,0.0049,-0.12,0.21,-9.8e-07,0.43,-0.00018,0.00058,-4.2e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -16090000,0.71,0.00021,-0.013,0.71,0.011,0.0039,-0.02,0.0016,-0.0011,-3.7e+02,-0.0011,-0.0058,-8e-05,-0.00043,0.0037,-0.12,0.21,-7.1e-07,0.43,-0.00016,0.00063,-2.6e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.048,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16190000,0.71,0.00026,-0.013,0.71,0.01,0.0041,-0.019,0.001,-0.00095,-3.7e+02,-0.0011,-0.0058,-7.9e-05,0.00031,0.0039,-0.12,0.21,-1e-06,0.43,-0.00017,0.00063,-2.1e-05,0,0,-3.6e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16290000,0.71,0.00028,-0.014,0.71,0.012,0.0052,-0.02,0.0023,5.7e-05,-3.7e+02,-0.0011,-0.0058,-7.5e-05,0.00063,0.0026,-0.12,0.21,-5.6e-07,0.43,-0.00015,0.00065,-5.9e-06,0,0,-3.6e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.047,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16390000,0.71,0.00033,-0.013,0.71,0.01,0.0037,-0.019,0.0013,-0.00022,-3.7e+02,-0.0011,-0.0058,-7.6e-05,0.0018,0.0038,-0.12,0.21,-1.3e-06,0.43,-0.00018,0.00064,-2e-06,0,0,-3.6e+02,0.00014,0.00014,0.037,0.02,0.022,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16490000,0.71,0.00042,-0.013,0.71,0.0088,0.005,-0.022,0.0018,0.00032,-3.7e+02,-0.0011,-0.0058,-7.5e-05,0.0026,0.0039,-0.12,0.21,-1.6e-06,0.43,-0.00019,0.00064,1.2e-05,0,0,-3.6e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16590000,0.71,0.00053,-0.013,0.71,0.0068,0.0059,-0.023,-0.00051,0.0023,-3.7e+02,-0.0012,-0.0058,-7.6e-05,0.0028,0.0036,-0.12,0.21,-1.6e-06,0.43,-0.00017,0.00063,7.8e-06,0,0,-3.6e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0095,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16690000,0.71,0.00049,-0.013,0.71,0.0076,0.0062,-0.019,0.00033,0.0026,-3.7e+02,-0.0012,-0.0058,-7.9e-05,0.0022,0.0043,-0.12,0.21,-1.8e-06,0.43,-0.00018,0.00062,-3.3e-06,0,0,-3.6e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16790000,0.71,0.00049,-0.013,0.71,0.0058,0.0069,-0.018,-0.0016,0.0042,-3.7e+02,-0.0012,-0.0058,-8e-05,0.002,0.004,-0.12,0.21,-1.7e-06,0.43,-0.00016,0.00063,-1.6e-05,0,0,-3.6e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16890000,0.71,0.00054,-0.013,0.71,0.0055,0.0078,-0.016,-0.0013,0.0047,-3.7e+02,-0.0012,-0.0058,-8.2e-05,0.0023,0.0046,-0.13,0.21,-2e-06,0.43,-0.00017,0.00062,-1.2e-05,0,0,-3.6e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0091,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -16990000,0.71,0.00051,-0.013,0.71,0.0055,0.0055,-0.015,-0.0021,0.0025,-3.7e+02,-0.0012,-0.0059,-8.2e-05,0.0022,0.0056,-0.13,0.21,-2.5e-06,0.43,-0.00019,0.00061,-2.2e-05,0,0,-3.6e+02,0.00013,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17090000,0.71,0.00055,-0.013,0.71,0.0058,0.0071,-0.015,-0.002,0.0032,-3.7e+02,-0.0012,-0.0059,-8.1e-05,0.0032,0.0059,-0.13,0.21,-2.8e-06,0.43,-0.0002,0.00061,-1e-05,0,0,-3.6e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17190000,0.71,0.00061,-0.013,0.71,0.0059,0.008,-0.016,-0.0028,0.0021,-3.7e+02,-0.0012,-0.0059,-7.7e-05,0.004,0.0059,-0.13,0.21,-3.1e-06,0.43,-0.0002,0.00062,-7.9e-06,0,0,-3.6e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17290000,0.71,0.00063,-0.013,0.71,0.0077,0.0088,-0.011,-0.0026,0.0025,-3.7e+02,-0.0012,-0.0059,-7.9e-05,0.0043,0.0068,-0.13,0.21,-3.5e-06,0.43,-0.00022,0.00061,-5.3e-06,0,0,-3.6e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.5e-06,2.3e-06,0.0067,0.0086,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17390000,0.71,0.00068,-0.013,0.71,0.0075,0.0092,-0.0095,-0.0024,0.0016,-3.7e+02,-0.0012,-0.0059,-7.3e-05,0.0051,0.0066,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.4e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17490000,0.71,0.00063,-0.013,0.71,0.0091,0.0093,-0.0078,-0.0012,0.0025,-3.7e+02,-0.0012,-0.0059,-7.3e-05,0.0045,0.0064,-0.13,0.21,-3.4e-06,0.43,-0.00023,0.00061,7.6e-06,0,0,-3.6e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17590000,0.71,0.0006,-0.013,0.71,0.0099,0.0085,-0.0024,-0.001,0.0013,-3.7e+02,-0.0012,-0.0059,-7e-05,0.0048,0.0066,-0.13,0.21,-3.7e-06,0.43,-0.00024,0.00061,4.9e-06,0,0,-3.6e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17690000,0.71,0.00058,-0.013,0.71,0.011,0.01,-0.003,1.7e-05,0.0023,-3.7e+02,-0.0012,-0.0059,-6.9e-05,0.0049,0.0064,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.045,0.046,0.048,1.3e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17790000,0.71,0.00056,-0.013,0.71,0.012,0.011,-0.0042,1.3e-05,0.0029,-3.7e+02,-0.0012,-0.0058,-6e-05,0.0059,0.0053,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00064,2.2e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17890000,0.71,0.00055,-0.013,0.71,0.015,0.012,-0.0041,0.0016,0.0045,-3.7e+02,-0.0012,-0.0058,-5.7e-05,0.0057,0.0046,-0.13,0.21,-3.3e-06,0.43,-0.00024,0.00064,2.7e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.018,0.02,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0062,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17990000,0.71,0.00049,-0.013,0.71,0.016,0.0091,-0.0027,0.0018,0.0036,-3.7e+02,-0.0012,-0.0058,-5.6e-05,0.0054,0.0049,-0.13,0.21,-3.4e-06,0.43,-0.00024,0.00064,2.4e-05,0,0,-3.6e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0078,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -18090000,0.71,0.00048,-0.013,0.71,0.017,0.0083,-0.00045,0.0035,0.0037,-3.7e+02,-0.0012,-0.0059,-6.2e-05,0.0049,0.0059,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00061,1.9e-05,0,0,-3.6e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18190000,0.71,0.00044,-0.013,0.71,0.018,0.0093,0.001,0.0041,0.0036,-3.7e+02,-0.0012,-0.0059,-5.7e-05,0.0051,0.0056,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00062,2e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.04,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0076,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18290000,0.71,0.00035,-0.013,0.71,0.018,0.0088,0.0022,0.0061,0.0041,-3.7e+02,-0.0012,-0.0059,-6e-05,0.0047,0.0059,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00061,1.4e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18390000,0.71,0.00032,-0.013,0.71,0.02,0.011,0.0035,0.0068,0.0042,-3.7e+02,-0.0012,-0.0059,-5.4e-05,0.0045,0.0051,-0.13,0.21,-3.4e-06,0.43,-0.00024,0.00062,1.6e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18490000,0.71,0.00038,-0.013,0.71,0.021,0.012,0.0031,0.0085,0.0055,-3.7e+02,-0.0012,-0.0059,-5.3e-05,0.005,0.0052,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00063,1.9e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18590000,0.71,0.00039,-0.013,0.71,0.02,0.013,0.0014,0.0074,0.0054,-3.7e+02,-0.0012,-0.0058,-4.5e-05,0.0058,0.0047,-0.13,0.21,-3.7e-06,0.43,-0.00025,0.00065,2.5e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18690000,0.71,0.00031,-0.013,0.71,0.022,0.013,-0.00039,0.01,0.0065,-3.7e+02,-0.0012,-0.0059,-4.7e-05,0.0051,0.0047,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00064,1.8e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18790000,0.71,0.00032,-0.013,0.71,0.021,0.012,-0.00061,0.0087,0.0054,-3.7e+02,-0.0012,-0.0059,-4.6e-05,0.0053,0.0053,-0.13,0.21,-3.8e-06,0.43,-0.00025,0.00063,1.5e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00067,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18890000,0.71,0.00041,-0.013,0.71,0.021,0.014,1.9e-05,0.01,0.0075,-3.7e+02,-0.0012,-0.0058,-4e-05,0.0062,0.0047,-0.13,0.21,-3.9e-06,0.43,-0.00026,0.00065,2.9e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.007,0.00065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -18990000,0.71,0.00046,-0.013,0.71,0.021,0.015,-0.0012,0.01,0.0068,-3.7e+02,-0.0013,-0.0058,-3.4e-05,0.0069,0.0049,-0.13,0.21,-4.2e-06,0.43,-0.00027,0.00066,3e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.015,0.017,0.012,0.039,0.04,0.044,9.7e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.00062,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19090000,0.71,0.00051,-0.013,0.71,0.021,0.016,0.0018,0.012,0.0084,-3.7e+02,-0.0013,-0.0058,-3.4e-05,0.0075,0.0052,-0.13,0.21,-4.4e-06,0.43,-0.00028,0.00066,3.4e-05,0,0,-3.6e+02,0.00011,0.0001,0.037,0.016,0.019,0.012,0.042,0.044,0.044,9.6e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.0006,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19190000,0.71,0.00056,-0.013,0.71,0.02,0.016,0.0019,0.012,0.0076,-3.7e+02,-0.0013,-0.0059,-2.7e-05,0.008,0.0054,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00067,3.7e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.014,0.017,0.012,0.038,0.04,0.043,9.3e-07,1.5e-06,2.1e-06,0.0055,0.0068,0.00058,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19290000,0.71,0.00058,-0.013,0.71,0.02,0.015,0.0046,0.013,0.0088,-3.7e+02,-0.0013,-0.0059,-3e-05,0.0078,0.0058,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00066,4e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.015,0.019,0.012,0.042,0.044,0.043,9.2e-07,1.5e-06,2.1e-06,0.0055,0.0067,0.00056,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19390000,0.71,0.00054,-0.013,0.71,0.019,0.014,0.0084,0.012,0.0079,-3.7e+02,-0.0013,-0.0059,-2.4e-05,0.0078,0.0056,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00065,3.5e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.014,0.017,0.011,0.038,0.04,0.043,8.9e-07,1.5e-06,2.1e-06,0.0054,0.0066,0.00054,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19490000,0.71,0.00056,-0.013,0.71,0.019,0.015,0.0049,0.014,0.01,-3.7e+02,-0.0013,-0.0058,-1.8e-05,0.0078,0.0049,-0.13,0.21,-4.6e-06,0.43,-0.00029,0.00066,3.8e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.015,0.018,0.011,0.042,0.044,0.043,8.8e-07,1.4e-06,2.1e-06,0.0054,0.0066,0.00052,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19590000,0.71,0.00063,-0.013,0.71,0.017,0.015,0.0043,0.012,0.0095,-3.7e+02,-0.0013,-0.0058,-6.7e-06,0.0086,0.0047,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00068,4.7e-05,0,0,-3.6e+02,0.00011,9.8e-05,0.036,0.014,0.017,0.011,0.038,0.039,0.042,8.5e-07,1.4e-06,2.1e-06,0.0053,0.0065,0.0005,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19690000,0.71,0.00067,-0.013,0.71,0.017,0.013,0.0059,0.013,0.01,-3.7e+02,-0.0013,-0.0059,-1e-05,0.0089,0.0053,-0.13,0.21,-5e-06,0.43,-0.00031,0.00068,3.9e-05,0,0,-3.6e+02,0.00011,9.8e-05,0.036,0.015,0.018,0.011,0.042,0.043,0.042,8.4e-07,1.4e-06,2.1e-06,0.0053,0.0064,0.00049,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19790000,0.71,0.00075,-0.013,0.71,0.014,0.011,0.0064,0.011,0.009,-3.7e+02,-0.0013,-0.0059,-5.6e-06,0.0094,0.0057,-0.13,0.21,-5.3e-06,0.43,-0.00031,0.00068,3.7e-05,0,0,-3.6e+02,0.00011,9.6e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.042,8.2e-07,1.3e-06,2.1e-06,0.0053,0.0063,0.00047,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19890000,0.71,0.00066,-0.013,0.71,0.015,0.013,0.0076,0.014,0.011,-3.7e+02,-0.0013,-0.0058,2.5e-06,0.0092,0.0046,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00069,4.5e-05,0,0,-3.6e+02,0.00011,9.6e-05,0.036,0.015,0.018,0.01,0.041,0.043,0.042,8.1e-07,1.3e-06,2.1e-06,0.0052,0.0063,0.00046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19990000,0.71,0.00063,-0.013,0.7,0.013,0.013,0.01,0.012,0.011,-3.7e+02,-0.0013,-0.0058,1.8e-05,0.0095,0.0038,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.0007,5e-05,0,0,-3.6e+02,0.0001,9.4e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00044,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -20090000,0.71,0.00066,-0.013,0.7,0.013,0.013,0.011,0.013,0.013,-3.7e+02,-0.0013,-0.0058,2.8e-05,0.01,0.0031,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00072,6.1e-05,0,0,-3.6e+02,0.00011,9.4e-05,0.036,0.014,0.018,0.01,0.041,0.043,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20190000,0.71,0.0007,-0.013,0.7,0.012,0.012,0.013,0.011,0.012,-3.7e+02,-0.0013,-0.0058,3.7e-05,0.0099,0.0028,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00072,6.1e-05,0,0,-3.6e+02,0.0001,9.2e-05,0.036,0.013,0.016,0.0098,0.037,0.039,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00042,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20290000,0.71,0.0007,-0.013,0.7,0.01,0.012,0.011,0.012,0.013,-3.7e+02,-0.0013,-0.0058,4.1e-05,0.01,0.0027,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00074,6.2e-05,0,0,-3.6e+02,0.0001,9.2e-05,0.036,0.014,0.018,0.0097,0.041,0.043,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.006,0.0004,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20390000,0.71,0.00066,-0.013,0.7,0.0086,0.0097,0.013,0.01,0.012,-3.7e+02,-0.0013,-0.0058,4.5e-05,0.01,0.0028,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00073,5.2e-05,0,0,-3.6e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.005,0.0059,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20490000,0.71,0.00071,-0.013,0.7,0.0087,0.0097,0.013,0.011,0.012,-3.7e+02,-0.0013,-0.0058,4.2e-05,0.01,0.003,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00072,5.1e-05,0,0,-3.6e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.0059,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20590000,0.71,0.00074,-0.013,0.7,0.0078,0.0076,0.0099,0.0091,0.01,-3.7e+02,-0.0013,-0.0058,4.3e-05,0.01,0.0036,-0.13,0.21,-5e-06,0.43,-0.00031,0.00072,5e-05,0,0,-3.6e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0058,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20690000,0.71,0.00077,-0.013,0.7,0.0084,0.0077,0.011,0.01,0.011,-3.7e+02,-0.0013,-0.0058,4.6e-05,0.01,0.0034,-0.13,0.21,-5e-06,0.43,-0.00031,0.00072,5.2e-05,0,0,-3.6e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.0049,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20790000,0.71,0.00081,-0.013,0.7,0.0063,0.0072,0.012,0.0084,0.01,-3.7e+02,-0.0013,-0.0058,5.1e-05,0.011,0.0035,-0.13,0.21,-5e-06,0.43,-0.00032,0.00073,4.6e-05,0,0,-3.6e+02,9.8e-05,8.7e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20890000,0.71,0.00082,-0.013,0.7,0.0062,0.0068,0.011,0.009,0.011,-3.7e+02,-0.0013,-0.0058,5.9e-05,0.011,0.0031,-0.13,0.21,-5e-06,0.43,-0.00033,0.00074,5.2e-05,0,0,-3.6e+02,9.8e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.6e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -20990000,0.71,0.00084,-0.013,0.7,0.0044,0.0045,0.011,0.009,0.01,-3.7e+02,-0.0013,-0.0058,6.3e-05,0.011,0.0033,-0.13,0.21,-5e-06,0.43,-0.00034,0.00075,4.9e-05,0,0,-3.6e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21090000,0.71,0.00081,-0.013,0.7,0.0054,0.0037,0.012,0.01,0.011,-3.7e+02,-0.0013,-0.0058,6.8e-05,0.011,0.0029,-0.13,0.21,-4.9e-06,0.43,-0.00033,0.00075,4.6e-05,0,0,-3.6e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.8e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21190000,0.71,0.00082,-0.013,0.7,0.0057,0.0028,0.011,0.011,0.0091,-3.7e+02,-0.0013,-0.0058,6.8e-05,0.011,0.0031,-0.13,0.21,-4.9e-06,0.43,-0.00033,0.00075,4.3e-05,0,0,-3.6e+02,9.4e-05,8.4e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.2e-07,9.4e-07,1.9e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21290000,0.71,0.0009,-0.013,0.7,0.005,0.0029,0.013,0.011,0.01,-3.7e+02,-0.0013,-0.0058,7.9e-05,0.011,0.0026,-0.13,0.21,-4.9e-06,0.43,-0.00034,0.00077,4.7e-05,0,0,-3.6e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.4e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21390000,0.71,0.00088,-0.013,0.7,0.004,0.00086,0.013,0.0095,0.0093,-3.7e+02,-0.0013,-0.0058,7.4e-05,0.011,0.0028,-0.13,0.21,-5e-06,0.43,-0.00033,0.00076,4.7e-05,0,0,-3.6e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21490000,0.71,0.00088,-0.013,0.7,0.0045,0.0013,0.013,0.01,0.0098,-3.7e+02,-0.0013,-0.0058,7.9e-05,0.011,0.0024,-0.13,0.21,-5e-06,0.43,-0.00032,0.00077,5.3e-05,0,0,-3.6e+02,9.4e-05,8.3e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21590000,0.71,0.00087,-0.013,0.7,0.0033,0.0017,0.012,0.009,0.0091,-3.7e+02,-0.0013,-0.0058,7.7e-05,0.011,0.0025,-0.13,0.21,-5.1e-06,0.43,-0.00032,0.00076,5e-05,0,0,-3.6e+02,9.1e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21690000,0.71,0.00084,-0.013,0.7,0.0049,0.0021,0.014,0.01,0.0097,-3.7e+02,-0.0013,-0.0058,8.1e-05,0.011,0.0021,-0.13,0.21,-5e-06,0.43,-0.00031,0.00077,5.1e-05,0,0,-3.6e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.6e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21790000,0.71,0.00084,-0.013,0.7,0.003,0.0042,0.013,0.0074,0.01,-3.7e+02,-0.0013,-0.0058,7.2e-05,0.011,0.0023,-0.13,0.21,-5.6e-06,0.43,-0.00033,0.00076,5.3e-05,0,0,-3.6e+02,9e-05,8e-05,0.036,0.012,0.015,0.0079,0.036,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0052,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0047,0.013,0.0079,0.011,-3.7e+02,-0.0013,-0.0058,7.3e-05,0.011,0.0022,-0.13,0.21,-5.5e-06,0.43,-0.00033,0.00076,5e-05,0,0,-3.6e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0052,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21990000,0.71,0.00085,-0.013,0.7,0.0026,0.0054,0.014,0.006,0.012,-3.7e+02,-0.0013,-0.0058,7e-05,0.012,0.002,-0.13,0.21,-5.9e-06,0.43,-0.00034,0.00076,5.2e-05,0,0,-3.7e+02,8.8e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22090000,0.71,0.00088,-0.013,0.7,0.0025,0.007,0.012,0.006,0.013,-3.7e+02,-0.0013,-0.0058,7e-05,0.012,0.0021,-0.13,0.21,-5.9e-06,0.43,-0.00034,0.00076,5.2e-05,0,0,-3.7e+02,8.9e-05,7.9e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,7.9e-07,1.8e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22190000,0.71,0.00085,-0.013,0.7,0.0019,0.007,0.013,0.0053,0.011,-3.7e+02,-0.0013,-0.0058,7.6e-05,0.012,0.0021,-0.13,0.21,-5.7e-06,0.43,-0.00035,0.00077,4.1e-05,0,0,-3.7e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.6e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22290000,0.71,0.00088,-0.013,0.7,0.0013,0.0067,0.013,0.0057,0.012,-3.7e+02,-0.0013,-0.0058,7.4e-05,0.012,0.0022,-0.13,0.21,-5.8e-06,0.43,-0.00035,0.00076,4.2e-05,0,0,-3.7e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.6e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22390000,0.71,0.00089,-0.013,0.7,-0.001,0.0064,0.015,0.0041,0.01,-3.7e+02,-0.0013,-0.0058,8.1e-05,0.012,0.0024,-0.13,0.21,-5.7e-06,0.43,-0.00035,0.00077,4.3e-05,0,0,-3.7e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.3e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22490000,0.71,0.00093,-0.013,0.7,-0.0021,0.0072,0.016,0.0033,0.011,-3.7e+02,-0.0013,-0.0058,8.2e-05,0.012,0.0026,-0.13,0.21,-5.7e-06,0.43,-0.00037,0.00078,4.1e-05,0,0,-3.7e+02,8.6e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.3e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22590000,0.71,0.00096,-0.013,0.7,-0.0038,0.0065,0.015,0.00096,0.01,-3.7e+02,-0.0014,-0.0058,8.5e-05,0.013,0.0027,-0.13,0.21,-5.6e-06,0.43,-0.00038,0.00079,3.8e-05,0,0,-3.7e+02,8.4e-05,7.5e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.0049,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22690000,0.71,0.001,-0.013,0.7,-0.0052,0.008,0.016,0.00022,0.012,-3.7e+02,-0.0014,-0.0058,9.1e-05,0.013,0.0026,-0.13,0.21,-5.5e-06,0.43,-0.00039,0.0008,3.7e-05,0,0,-3.7e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7e-07,1.8e-06,0.0044,0.0049,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22790000,0.71,0.001,-0.013,0.7,-0.0071,0.0069,0.017,-0.0024,0.0091,-3.7e+02,-0.0014,-0.0058,8.1e-05,0.013,0.0034,-0.13,0.21,-5.7e-06,0.43,-0.00039,0.00078,4.3e-05,0,0,-3.7e+02,8.3e-05,7.4e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22890000,0.71,0.00099,-0.013,0.7,-0.0076,0.0078,0.019,-0.0024,0.0098,-3.7e+02,-0.0014,-0.0058,8e-05,0.013,0.0033,-0.13,0.21,-5.6e-06,0.43,-0.00038,0.00077,3.9e-05,0,0,-3.7e+02,8.4e-05,7.5e-05,0.036,0.013,0.016,0.0071,0.039,0.042,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22990000,0.71,0.00097,-0.013,0.7,-0.0077,0.0068,0.02,-0.0036,0.009,-3.7e+02,-0.0014,-0.0058,9e-05,0.013,0.0032,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.00078,3.6e-05,0,0,-3.7e+02,8.2e-05,7.3e-05,0.036,0.012,0.015,0.007,0.036,0.038,0.036,4.7e-07,6.5e-07,1.7e-06,0.0043,0.0048,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23090000,0.71,0.00092,-0.013,0.7,-0.0081,0.0067,0.02,-0.0043,0.009,-3.7e+02,-0.0014,-0.0058,8.1e-05,0.012,0.0034,-0.13,0.21,-5.5e-06,0.43,-0.00038,0.00077,3.3e-05,0,0,-3.7e+02,8.3e-05,7.4e-05,0.036,0.013,0.016,0.007,0.039,0.042,0.036,4.6e-07,6.5e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23190000,0.71,0.00097,-0.013,0.7,-0.0094,0.0047,0.022,-0.0076,0.0078,-3.7e+02,-0.0014,-0.0058,8.4e-05,0.013,0.0036,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00076,2.5e-05,0,0,-3.7e+02,8.1e-05,7.2e-05,0.036,0.012,0.014,0.0069,0.036,0.038,0.035,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23290000,0.71,0.00089,-0.013,0.7,-0.0093,0.0042,0.022,-0.0081,0.0084,-3.7e+02,-0.0014,-0.0058,8.6e-05,0.012,0.0034,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00076,2.4e-05,0,0,-3.7e+02,8.1e-05,7.3e-05,0.036,0.013,0.016,0.0069,0.039,0.042,0.036,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0047,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23390000,0.71,0.00095,-0.013,0.7,-0.0095,0.003,0.02,-0.01,0.0072,-3.7e+02,-0.0014,-0.0058,8.8e-05,0.012,0.0035,-0.13,0.21,-5.6e-06,0.43,-0.00035,0.00075,2.6e-05,0,0,-3.7e+02,8e-05,7.1e-05,0.036,0.012,0.014,0.0068,0.036,0.038,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23490000,0.71,0.0033,-0.01,0.7,-0.016,0.0032,-0.013,-0.011,0.008,-3.7e+02,-0.0014,-0.0058,9.5e-05,0.012,0.0032,-0.13,0.21,-5.5e-06,0.43,-0.00034,0.00079,5.1e-05,0,0,-3.7e+02,8e-05,7.2e-05,0.036,0.013,0.015,0.0068,0.039,0.042,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 -23590000,0.71,0.0086,-0.0026,0.7,-0.027,0.0032,-0.045,-0.011,0.0075,-3.7e+02,-0.0014,-0.0058,9.1e-05,0.012,0.0032,-0.13,0.21,-5.5e-06,0.43,-0.00034,0.00084,0.00012,0,0,-3.7e+02,7.8e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0046,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23690000,0.71,0.0082,0.0032,0.71,-0.058,-0.0046,-0.095,-0.015,0.0075,-3.7e+02,-0.0014,-0.0058,9.1e-05,0.012,0.0032,-0.13,0.21,-5.5e-06,0.43,-0.00034,0.00078,9e-05,0,0,-3.7e+02,7.9e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23790000,0.71,0.0053,-0.00011,0.71,-0.083,-0.016,-0.15,-0.016,0.0069,-3.7e+02,-0.0013,-0.0058,9.3e-05,0.011,0.0028,-0.13,0.21,-4.7e-06,0.43,-0.00039,0.00078,0.00044,0,0,-3.7e+02,7.7e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23890000,0.71,0.0026,-0.0062,0.71,-0.1,-0.025,-0.2,-0.026,0.0047,-3.7e+02,-0.0013,-0.0058,9.2e-05,0.012,0.0029,-0.13,0.21,-4.5e-06,0.43,-0.00042,0.00083,0.00035,0,0,-3.7e+02,7.8e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23990000,0.71,0.0013,-0.011,0.71,-0.1,-0.029,-0.26,-0.031,0.00042,-3.7e+02,-0.0013,-0.0058,9.8e-05,0.012,0.0029,-0.13,0.21,-4.2e-06,0.43,-0.00039,0.00083,0.00032,0,0,-3.7e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24090000,0.71,0.0025,-0.0095,0.71,-0.1,-0.028,-0.3,-0.041,-0.002,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0026,-0.13,0.21,-3.8e-06,0.43,-0.00042,0.0008,0.00036,0,0,-3.7e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24190000,0.71,0.0036,-0.0072,0.71,-0.11,-0.03,-0.35,-0.044,-0.0052,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0025,-0.13,0.21,-3e-06,0.43,-0.00042,0.00083,0.00036,0,0,-3.7e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.3e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24290000,0.71,0.0041,-0.0064,0.71,-0.12,-0.033,-0.41,-0.055,-0.0085,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0025,-0.13,0.21,-2.7e-06,0.43,-0.00046,0.00088,0.00042,0,0,-3.7e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.3e-07,1.6e-06,0.0041,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24390000,0.71,0.0042,-0.0065,0.71,-0.13,-0.041,-0.46,-0.062,-0.021,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0031,-0.13,0.21,1.4e-07,0.43,-0.00034,0.00092,0.00042,0,0,-3.7e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24490000,0.71,0.005,-0.0024,0.71,-0.14,-0.046,-0.51,-0.076,-0.025,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0031,-0.13,0.21,1.4e-07,0.43,-0.00034,0.00093,0.00041,0,0,-3.7e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24590000,0.71,0.0055,0.0013,0.71,-0.16,-0.057,-0.56,-0.081,-0.035,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.0031,-0.13,0.21,1.2e-06,0.43,1.8e-05,0.00058,0.00036,0,0,-3.7e+02,7.4e-05,6.6e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24690000,0.71,0.0056,0.0022,0.71,-0.18,-0.07,-0.64,-0.097,-0.041,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.0029,-0.13,0.21,2.2e-06,0.43,-2.7e-05,0.00062,0.00054,0,0,-3.7e+02,7.4e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24790000,0.71,0.0053,0.00098,0.71,-0.2,-0.083,-0.73,-0.11,-0.054,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.003,-0.13,0.21,1.6e-06,0.43,5.7e-06,0.00059,0.0003,0,0,-3.7e+02,7.3e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24890000,0.71,0.0071,0.0027,0.71,-0.22,-0.094,-0.75,-0.13,-0.063,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.0031,-0.13,0.21,2.4e-06,0.43,-0.0001,0.00075,0.00032,0,0,-3.7e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24990000,0.71,0.0089,0.0044,0.71,-0.24,-0.1,-0.81,-0.13,-0.073,-3.7e+02,-0.0013,-0.0058,0.00011,0.0098,0.0027,-0.13,0.21,2e-06,0.43,-0.00018,0.00085,-2e-05,0,0,-3.7e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.7e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25090000,0.71,0.0092,0.0038,0.71,-0.27,-0.11,-0.86,-0.16,-0.084,-3.7e+02,-0.0013,-0.0058,0.00011,0.0098,0.0028,-0.13,0.21,1.5e-06,0.43,-0.0002,0.00086,-5.6e-05,0,0,-3.7e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.7e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25190000,0.71,0.0087,0.0024,0.71,-0.3,-0.13,-0.91,-0.18,-0.11,-3.7e+02,-0.0013,-0.0058,0.00013,0.0092,0.0031,-0.13,0.21,6.9e-06,0.43,5.9e-05,0.00083,8e-05,0,0,-3.7e+02,7.1e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25290000,0.71,0.011,0.0093,0.71,-0.33,-0.14,-0.96,-0.21,-0.12,-3.7e+02,-0.0013,-0.0058,0.00013,0.0092,0.0031,-0.13,0.21,6.8e-06,0.43,8.4e-05,0.00077,8.7e-05,0,0,-3.7e+02,7.2e-05,6.4e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,-0.22,-0.14,-3.7e+02,-0.0013,-0.0058,0.00014,0.0084,0.0029,-0.13,0.21,1.1e-05,0.43,0.00049,0.00046,0.00012,0,0,-3.7e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,-0.26,-0.16,-3.7e+02,-0.0013,-0.0058,0.00014,0.0084,0.0027,-0.13,0.21,9.5e-06,0.43,0.00066,0.00013,0.00031,0,0,-3.7e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,-0.29,-0.2,-3.7e+02,-0.0012,-0.0058,0.00016,0.0077,0.0031,-0.13,0.21,1.6e-05,0.43,0.00096,0.00014,0.00032,0,0,-3.7e+02,7e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0042,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25690000,0.71,0.015,0.022,0.71,-0.5,-0.23,-1.2,-0.34,-0.22,-3.7e+02,-0.0012,-0.0058,0.00016,0.0077,0.0031,-0.13,0.21,1.6e-05,0.43,0.00095,0.00016,0.0004,0,0,-3.7e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0042,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 -25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,-0.36,-0.25,-3.7e+02,-0.0012,-0.0058,0.00016,0.007,0.0024,-0.13,0.21,1.9e-05,0.43,0.0013,-8.9e-05,-4.9e-05,0,0,-3.7e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25890000,0.71,0.018,0.029,0.71,-0.62,-0.29,-1.3,-0.41,-0.28,-3.7e+02,-0.0012,-0.0058,0.00017,0.0072,0.0024,-0.13,0.21,2.1e-05,0.43,0.0014,-2.1e-05,-0.00011,0,0,-3.7e+02,7.1e-05,6.2e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25990000,0.7,0.017,0.026,0.71,-0.67,-0.32,-1.3,-0.45,-0.33,-3.7e+02,-0.0012,-0.0058,0.00019,0.0063,0.0027,-0.13,0.21,2.9e-05,0.43,0.0023,-0.00058,-0.00057,0,0,-3.7e+02,7e-05,6.1e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 -26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,-0.53,-0.37,-3.7e+02,-0.0012,-0.0059,0.00018,0.0064,0.0029,-0.13,0.21,2.5e-05,0.43,0.0024,-0.0005,-0.0012,0,0,-3.7e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 -26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,-0.55,-0.41,-3.7e+02,-0.0012,-0.0058,0.00018,0.0053,0.0017,-0.13,0.21,3.9e-05,0.43,0.0023,0.00039,-0.0014,0,0,-3.7e+02,7e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 -26290000,0.7,0.025,0.047,0.71,-0.89,-0.43,-1.3,-0.64,-0.45,-3.7e+02,-0.0012,-0.0058,0.00018,0.0052,0.0018,-0.13,0.21,3.8e-05,0.43,0.0023,0.00027,-0.0014,0,0,-3.7e+02,7.1e-05,6.1e-05,0.03,0.015,0.028,0.0059,0.039,0.043,0.033,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.00099,3.9e-05,0.00099,0.0013,0.001,0.00099,1,1,0.01 -26390000,0.7,0.024,0.044,0.71,-0.96,-0.49,-1.3,-0.7,-0.54,-3.7e+02,-0.0011,-0.0059,0.00021,0.0044,0.0025,-0.13,0.21,4.6e-05,0.44,0.0036,-0.00019,-0.0024,0,0,-3.7e+02,7.1e-05,6e-05,0.028,0.014,0.027,0.0058,0.036,0.039,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00014,0.00096,3.9e-05,0.00095,0.0012,0.00096,0.00095,1,1,0.01 -26490000,0.7,0.031,0.06,0.71,-1.1,-0.53,-1.3,-0.8,-0.59,-3.7e+02,-0.0011,-0.0059,0.00021,0.0044,0.0025,-0.13,0.21,3.9e-05,0.44,0.004,-0.001,-0.0026,0,0,-3.7e+02,7.1e-05,6.1e-05,0.028,0.016,0.031,0.0058,0.039,0.044,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00014,0.00092,3.9e-05,0.00092,0.0012,0.00092,0.00091,1,1,0.01 -26590000,0.7,0.037,0.076,0.71,-1.2,-0.59,-1.3,-0.85,-0.66,-3.7e+02,-0.0011,-0.0059,0.00019,0.0028,0.0015,-0.13,0.21,3.9e-05,0.44,0.0041,-0.00065,-0.0048,0,0,-3.7e+02,7.1e-05,6e-05,0.025,0.015,0.031,0.0058,0.036,0.04,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00087,3.9e-05,0.00086,0.001,0.00087,0.00086,1,1,0.01 -26690000,0.7,0.039,0.079,0.71,-1.3,-0.65,-1.3,-0.97,-0.72,-3.7e+02,-0.0011,-0.0059,0.00019,0.0028,0.0014,-0.13,0.21,4.5e-05,0.44,0.004,-0.00014,-0.004,0,0,-3.7e+02,7.2e-05,6.1e-05,0.025,0.017,0.038,0.0058,0.04,0.045,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00081,3.9e-05,0.0008,0.001,0.00081,0.00079,1,1,0.01 -26790000,0.7,0.036,0.074,0.71,-1.4,-0.74,-1.3,-1.1,-0.85,-3.7e+02,-0.0011,-0.0059,0.00022,0.0012,0.0023,-0.13,0.21,8.4e-05,0.44,0.0055,0.00061,-0.0037,0,0,-3.7e+02,7.2e-05,6e-05,0.022,0.016,0.036,0.0057,0.036,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00076,3.9e-05,0.00075,0.00092,0.00076,0.00074,1,1,0.01 -26890000,0.7,0.045,0.096,0.71,-1.6,-0.8,-1.3,-1.2,-0.93,-3.7e+02,-0.0011,-0.0059,0.00023,0.0013,0.0023,-0.13,0.21,8.9e-05,0.44,0.0054,0.0012,-0.0041,0,0,-3.7e+02,7.3e-05,6e-05,0.022,0.018,0.043,0.0058,0.04,0.046,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00072,3.9e-05,0.0007,0.00092,0.00072,0.0007,1,1,0.01 -26990000,0.7,0.051,0.12,0.71,-1.7,-0.89,-1.3,-1.3,-1,-3.7e+02,-0.00099,-0.0059,0.00022,-0.00068,0.00038,-0.13,0.21,0.00013,0.44,0.0061,0.0034,-0.0056,0,0,-3.7e+02,7.3e-05,5.9e-05,0.019,0.017,0.042,0.0057,0.037,0.041,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00065,3.9e-05,0.00063,0.00079,0.00065,0.00062,1,1,0.01 -27090000,0.7,0.052,0.12,0.71,-1.9,-0.98,-1.3,-1.5,-1.1,-3.7e+02,-0.00099,-0.0059,0.00022,-0.00072,0.00039,-0.13,0.21,0.00013,0.44,0.006,0.0035,-0.0052,0,0,-3.7e+02,7.4e-05,6e-05,0.019,0.02,0.052,0.0057,0.04,0.048,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00059,3.9e-05,0.00056,0.00078,0.0006,0.00056,1,1,0.01 -27190000,0.7,0.05,0.11,0.7,-2.1,-1,-1.2,-1.7,-1.2,-3.7e+02,-0.00097,-0.0059,0.00022,-0.0018,0.0002,-0.13,0.21,4.8e-05,0.44,0.0021,0.0027,-0.0049,0,0,-3.7e+02,7.5e-05,6e-05,0.016,0.02,0.051,0.0057,0.043,0.05,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00055,3.9e-05,0.00051,0.00066,0.00055,0.00051,1,1,0.01 -27290000,0.71,0.044,0.095,0.7,-2.3,-1.1,-1.2,-1.9,-1.3,-3.7e+02,-0.00097,-0.0059,0.00023,-0.0018,0.00019,-0.13,0.21,5.6e-05,0.44,0.0019,0.0034,-0.0049,0,0,-3.7e+02,7.5e-05,6e-05,0.016,0.022,0.059,0.0057,0.047,0.057,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00052,3.9e-05,0.00048,0.00066,0.00052,0.00048,1,1,0.01 -27390000,0.71,0.038,0.079,0.7,-2.4,-1.1,-1.2,-2.1,-1.4,-3.7e+02,-0.00092,-0.0058,0.00022,-0.003,-0.0016,-0.13,0.21,1.3e-05,0.44,-0.0005,0.0031,-0.0063,0,0,-3.7e+02,7.6e-05,6e-05,0.013,0.021,0.052,0.0057,0.049,0.059,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00049,3.9e-05,0.00046,0.00055,0.00049,0.00046,1,1,0.01 -27490000,0.71,0.032,0.064,0.7,-2.5,-1.1,-1.2,-2.3,-1.5,-3.7e+02,-0.00092,-0.0058,0.00022,-0.0029,-0.0015,-0.13,0.21,1.9e-05,0.44,-0.00058,0.0032,-0.0067,0,0,-3.7e+02,7.7e-05,6.1e-05,0.013,0.023,0.056,0.0057,0.054,0.067,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00048,3.9e-05,0.00045,0.00055,0.00048,0.00044,1,1,0.01 -27590000,0.72,0.028,0.051,0.69,-2.6,-1.1,-1.2,-2.6,-1.6,-3.7e+02,-0.00092,-0.0059,0.00023,-0.0036,-0.00088,-0.12,0.21,-3.7e-05,0.44,-0.0032,0.0027,-0.0065,0,0,-3.7e+02,7.7e-05,6e-05,0.011,0.021,0.047,0.0057,0.056,0.068,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00044,1,1,0.01 -27690000,0.72,0.027,0.05,0.69,-2.6,-1.2,-1.2,-2.9,-1.7,-3.7e+02,-0.00092,-0.0059,0.00023,-0.0036,-0.00075,-0.12,0.21,-3.2e-05,0.44,-0.0033,0.0026,-0.0067,0,0,-3.7e+02,7.8e-05,6.1e-05,0.011,0.022,0.049,0.0057,0.062,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00043,1,1,0.01 -27790000,0.72,0.027,0.051,0.69,-2.6,-1.2,-1.2,-3.1,-1.8,-3.7e+02,-0.00092,-0.0059,0.00022,-0.0042,-0.00056,-0.12,0.21,-6e-05,0.44,-0.005,0.0021,-0.0072,0,0,-3.7e+02,7.8e-05,6e-05,0.0097,0.02,0.042,0.0056,0.064,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00045,3.9e-05,0.00043,0.00041,0.00045,0.00043,1,1,0.01 -27890000,0.72,0.027,0.049,0.69,-2.7,-1.2,-1.2,-3.4,-1.9,-3.7e+02,-0.00091,-0.0059,0.00023,-0.0042,-0.00056,-0.12,0.21,-6.1e-05,0.44,-0.005,0.002,-0.0072,0,0,-3.7e+02,7.9e-05,6.1e-05,0.0097,0.021,0.043,0.0057,0.07,0.087,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00044,3.9e-05,0.00043,0.00041,0.00044,0.00042,1,1,0.01 -27990000,0.72,0.026,0.046,0.69,-2.7,-1.2,-1.2,-3.7,-2,-3.7e+02,-0.00093,-0.0059,0.00024,-0.004,0.00035,-0.12,0.21,-7.7e-05,0.44,-0.0063,0.0015,-0.0071,0,0,-3.7e+02,8e-05,6e-05,0.0086,0.02,0.038,0.0056,0.072,0.087,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00043,3.9e-05,0.00042,0.00037,0.00043,0.00042,1,1,0.01 -28090000,0.72,0.032,0.059,0.69,-2.8,-1.2,-1.2,-4,-2.1,-3.7e+02,-0.00093,-0.0059,0.00023,-0.0041,0.00056,-0.12,0.21,-7.6e-05,0.44,-0.0065,0.0013,-0.0072,0,0,-3.7e+02,8e-05,6.1e-05,0.0086,0.021,0.039,0.0057,0.078,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00042,3.9e-05,0.00042,0.00036,0.00042,0.00041,1,1,0.01 -28190000,0.72,0.037,0.072,0.69,-2.8,-1.2,-0.94,-4.3,-2.2,-3.7e+02,-0.00094,-0.0059,0.00024,-0.0042,0.001,-0.12,0.21,-9.4e-05,0.44,-0.0073,0.00091,-0.0071,0,0,-3.7e+02,8.1e-05,6.1e-05,0.0079,0.02,0.034,0.0056,0.08,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00041,3.9e-05,0.00041,0.00033,0.00041,0.00041,1,1,0.01 -28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,-4.5,-2.4,-3.7e+02,-0.00094,-0.0059,0.00024,-0.0043,0.0011,-0.12,0.21,-9.7e-05,0.44,-0.0073,0.0013,-0.0069,0,0,-3.7e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,-4.8,-2.5,-3.7e+02,-0.00094,-0.0059,0.00023,-0.0042,0.0014,-0.12,0.21,-8.5e-05,0.44,-0.0075,0.0014,-0.007,0,0,-3.7e+02,8.3e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28490000,0.73,0.0028,0.0061,0.69,-2.8,-1.2,1.1,-5.1,-2.6,-3.7e+02,-0.00095,-0.0059,0.00023,-0.0038,0.0017,-0.12,0.21,-6.6e-05,0.44,-0.0076,0.0014,-0.0069,0,0,-3.7e+02,8.3e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28590000,0.73,0.00089,0.0025,0.69,-2.7,-1.2,0.97,-5.4,-2.7,-3.7e+02,-0.00095,-0.0059,0.00024,-0.0039,0.0017,-0.12,0.21,-6.9e-05,0.44,-0.0075,0.0015,-0.0069,0,0,-3.7e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28690000,0.73,0.00019,0.0016,0.69,-2.6,-1.2,0.98,-5.7,-2.8,-3.7e+02,-0.00095,-0.0059,0.00024,-0.0036,0.0021,-0.12,0.21,-5.2e-05,0.44,-0.0076,0.0014,-0.0068,0,0,-3.7e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 -28790000,0.73,-8.8e-06,0.0015,0.69,-2.6,-1.2,0.98,-6,-2.9,-3.7e+02,-0.00099,-0.0059,0.00024,-0.0031,0.0023,-0.12,0.21,-9.2e-05,0.44,-0.009,0.00063,-0.006,0,0,-3.7e+02,8.5e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.004,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 -28890000,0.73,1.1e-06,0.0018,0.69,-2.5,-1.2,0.97,-6.2,-3,-3.7e+02,-0.00099,-0.0059,0.00024,-0.0029,0.0026,-0.12,0.21,-7.6e-05,0.44,-0.0091,0.00059,-0.0059,0,0,-3.7e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 -28990000,0.73,0.00035,0.0024,0.68,-2.5,-1.1,0.97,-6.6,-3.1,-3.7e+02,-0.001,-0.0059,0.00025,-0.0016,0.003,-0.12,0.21,-0.00011,0.44,-0.01,-0.00033,-0.0047,0,0,-3.7e+02,8.7e-05,6.1e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00039,0.00039,1,1,0.01 -29090000,0.73,0.00052,0.0028,0.68,-2.4,-1.1,0.96,-6.8,-3.3,-3.7e+02,-0.001,-0.0059,0.00025,-0.0014,0.0034,-0.12,0.21,-9.3e-05,0.44,-0.011,-0.00039,-0.0046,0,0,-3.7e+02,8.7e-05,6.2e-05,0.0073,0.021,0.025,0.0057,0.14,0.17,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29190000,0.73,0.00077,0.0032,0.68,-2.4,-1.1,0.95,-7.1,-3.4,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00099,0.0034,-0.12,0.21,-0.00013,0.44,-0.011,-0.00062,-0.0041,0,0,-3.7e+02,8.8e-05,6.1e-05,0.0072,0.02,0.023,0.0056,0.14,0.17,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29290000,0.73,0.0011,0.004,0.68,-2.3,-1.1,0.98,-7.3,-3.5,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00096,0.0039,-0.12,0.21,-0.00012,0.44,-0.011,-0.00069,-0.0041,0,0,-3.7e+02,8.8e-05,6.1e-05,0.0072,0.021,0.024,0.0056,0.14,0.18,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29390000,0.73,0.0017,0.0055,0.68,-2.3,-1.1,0.99,-7.6,-3.6,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00023,0.0044,-0.12,0.21,-0.00015,0.44,-0.012,-0.0014,-0.0033,0,0,-3.7e+02,8.8e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.18,0.031,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 -29490000,0.73,0.0023,0.0066,0.68,-2.3,-1.1,0.99,-7.9,-3.7,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00022,0.0046,-0.12,0.21,-0.00015,0.44,-0.012,-0.0013,-0.0033,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.15,0.19,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 -29590000,0.73,0.0027,0.0076,0.68,-2.2,-1.1,0.98,-8.1,-3.8,-3.7e+02,-0.0011,-0.0059,0.00026,0.0004,0.0046,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.19,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00038,1,1,0.01 -29690000,0.73,0.003,0.0082,0.68,-2.2,-1.1,0.98,-8.3,-3.9,-3.7e+02,-0.0011,-0.0059,0.00026,0.00033,0.005,-0.12,0.21,-0.00015,0.44,-0.012,-0.0016,-0.0029,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29790000,0.73,0.0034,0.0087,0.68,-2.2,-1.1,0.96,-8.6,-4,-3.7e+02,-0.0012,-0.0059,0.00025,0.0014,0.0048,-0.12,0.21,-0.00017,0.44,-0.012,-0.0019,-0.0023,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29890000,0.73,0.0034,0.0088,0.68,-2.1,-1.1,0.95,-8.8,-4.1,-3.7e+02,-0.0012,-0.0059,0.00025,0.0011,0.0053,-0.12,0.21,-0.00018,0.44,-0.013,-0.0019,-0.0023,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.17,0.21,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29990000,0.73,0.0036,0.0089,0.68,-2.1,-1.1,0.94,-9.1,-4.2,-3.7e+02,-0.0012,-0.0059,0.00022,0.0016,0.005,-0.12,0.21,-0.00019,0.44,-0.013,-0.0021,-0.002,0,0,-3.7e+02,8.8e-05,6e-05,0.0071,0.02,0.024,0.0056,0.17,0.21,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -30090000,0.73,0.0035,0.0088,0.68,-2.1,-1.1,0.92,-9.3,-4.3,-3.7e+02,-0.0012,-0.0059,0.00022,0.0012,0.0053,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.18,0.22,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -30190000,0.73,0.0036,0.0085,0.68,-2.1,-1.1,0.91,-9.5,-4.4,-3.7e+02,-0.0012,-0.0059,0.00021,0.0021,0.0047,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-3.7e+02,8.8e-05,6e-05,0.007,0.02,0.025,0.0055,0.18,0.22,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00038,0.00028,0.00037,0.00038,1,1,0.01 -30290000,0.73,0.0035,0.0083,0.68,-2,-1.1,0.9,-9.7,-4.5,-3.7e+02,-0.0012,-0.0059,0.00021,0.0019,0.005,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-3.7e+02,8.8e-05,6e-05,0.007,0.021,0.026,0.0056,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30390000,0.73,0.0035,0.008,0.68,-2,-1.1,0.89,-10,-4.6,-3.7e+02,-0.0012,-0.0059,0.00019,0.0026,0.0048,-0.12,0.21,-0.00022,0.43,-0.012,-0.0025,-0.0014,0,0,-3.7e+02,8.7e-05,6e-05,0.007,0.021,0.025,0.0055,0.19,0.23,0.031,2.8e-07,3.5e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30490000,0.73,0.0034,0.0077,0.68,-2,-1.1,0.87,-10,-4.7,-3.7e+02,-0.0012,-0.0059,0.00019,0.0026,0.005,-0.12,0.21,-0.00022,0.43,-0.012,-0.0024,-0.0014,0,0,-3.7e+02,8.7e-05,6e-05,0.007,0.022,0.027,0.0056,0.2,0.24,0.031,2.8e-07,3.5e-07,1.2e-06,0.0038,0.0039,9.8e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30590000,0.73,0.0034,0.0073,0.68,-1.9,-1,0.83,-10,-4.8,-3.7e+02,-0.0012,-0.0059,0.00018,0.0034,0.0046,-0.12,0.21,-0.00024,0.43,-0.012,-0.0025,-0.001,0,0,-3.7e+02,8.6e-05,6e-05,0.0069,0.021,0.026,0.0055,0.2,0.24,0.031,2.8e-07,3.5e-07,1.1e-06,0.0037,0.0038,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30690000,0.73,0.0031,0.0069,0.68,-1.9,-1,0.83,-11,-4.9,-3.7e+02,-0.0012,-0.0059,0.00018,0.0032,0.0049,-0.12,0.21,-0.00024,0.43,-0.012,-0.0024,-0.001,0,0,-3.7e+02,8.6e-05,6e-05,0.0069,0.022,0.028,0.0055,0.21,0.25,0.031,2.8e-07,3.5e-07,1.1e-06,0.0037,0.0038,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30790000,0.73,0.0031,0.0065,0.68,-1.9,-1,0.82,-11,-5,-3.7e+02,-0.0012,-0.0059,0.00015,0.0039,0.0043,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00078,0,0,-3.7e+02,8.4e-05,6e-05,0.0068,0.021,0.027,0.0055,0.21,0.25,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30890000,0.73,0.0029,0.006,0.68,-1.9,-1,0.81,-11,-5.1,-3.7e+02,-0.0013,-0.0059,0.00015,0.0039,0.0047,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00076,0,0,-3.7e+02,8.5e-05,6e-05,0.0068,0.022,0.029,0.0055,0.22,0.26,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30990000,0.73,0.003,0.0054,0.68,-1.8,-1,0.8,-11,-5.2,-3.7e+02,-0.0013,-0.0058,0.00012,0.0048,0.0042,-0.12,0.21,-0.00025,0.43,-0.011,-0.0027,-0.00042,0,0,-3.7e+02,8.3e-05,5.9e-05,0.0067,0.021,0.028,0.0055,0.22,0.26,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.5e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31090000,0.73,0.0027,0.0049,0.68,-1.8,-1,0.79,-11,-5.3,-3.7e+02,-0.0013,-0.0058,0.00012,0.0046,0.0046,-0.12,0.21,-0.00026,0.43,-0.011,-0.0027,-0.00042,0,0,-3.7e+02,8.3e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31190000,0.73,0.0026,0.0044,0.68,-1.8,-1,0.78,-12,-5.3,-3.7e+02,-0.0013,-0.0058,9.7e-05,0.0049,0.0045,-0.12,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00025,0,0,-3.7e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31290000,0.73,0.0023,0.0039,0.68,-1.8,-1,0.78,-12,-5.4,-3.7e+02,-0.0013,-0.0058,0.0001,0.0046,0.0049,-0.11,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00028,0,0,-3.7e+02,8.2e-05,5.9e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31390000,0.73,0.0022,0.0032,0.68,-1.7,-0.99,0.78,-12,-5.5,-3.7e+02,-0.0013,-0.0058,7.8e-05,0.0051,0.0047,-0.11,0.21,-0.0003,0.43,-0.011,-0.0028,-2.8e-05,0,0,-3.7e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 -31490000,0.73,0.0019,0.0026,0.68,-1.7,-0.99,0.78,-12,-5.6,-3.7e+02,-0.0013,-0.0058,7.4e-05,0.005,0.0052,-0.11,0.21,-0.0003,0.43,-0.011,-0.0029,9.1e-06,0,0,-3.7e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 -31590000,0.73,0.002,0.0021,0.68,-1.7,-0.97,0.77,-12,-5.7,-3.7e+02,-0.0013,-0.0058,4.7e-05,0.0059,0.0048,-0.11,0.21,-0.00029,0.43,-0.01,-0.0029,0.00025,0,0,-3.7e+02,7.7e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31690000,0.73,0.0016,0.0014,0.68,-1.6,-0.97,0.78,-12,-5.8,-3.7e+02,-0.0013,-0.0058,4.9e-05,0.0056,0.0052,-0.11,0.21,-0.00029,0.43,-0.01,-0.0029,0.00021,0,0,-3.7e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31790000,0.73,0.0015,0.00057,0.69,-1.6,-0.95,0.78,-13,-5.9,-3.7e+02,-0.0013,-0.0058,2.4e-05,0.0066,0.0051,-0.11,0.21,-0.0003,0.43,-0.0097,-0.0029,0.00055,0,0,-3.7e+02,7.6e-05,5.9e-05,0.0062,0.021,0.029,0.0054,0.26,0.3,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31890000,0.73,0.0013,-0.00016,0.69,-1.6,-0.95,0.77,-13,-6,-3.7e+02,-0.0013,-0.0058,2.5e-05,0.0065,0.0056,-0.11,0.21,-0.00029,0.43,-0.0097,-0.0029,0.00058,0,0,-3.7e+02,7.6e-05,5.9e-05,0.0062,0.022,0.031,0.0054,0.27,0.31,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31990000,0.73,0.0012,-0.00077,0.69,-1.6,-0.93,0.77,-13,-6.1,-3.7e+02,-0.0013,-0.0058,-6.6e-06,0.007,0.0054,-0.11,0.21,-0.00029,0.43,-0.0092,-0.003,0.00075,0,0,-3.7e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0036,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32090000,0.73,0.00083,-0.0015,0.69,-1.5,-0.93,0.78,-13,-6.2,-3.7e+02,-0.0013,-0.0058,-7.5e-06,0.0068,0.0059,-0.11,0.21,-0.00029,0.43,-0.0093,-0.003,0.00076,0,0,-3.7e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0036,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32190000,0.73,0.00062,-0.0025,0.69,-1.5,-0.91,0.78,-13,-6.2,-3.7e+02,-0.0014,-0.0058,-4.2e-05,0.0073,0.006,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-3.7e+02,7.2e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32290000,0.73,0.00035,-0.0032,0.69,-1.5,-0.91,0.77,-14,-6.3,-3.7e+02,-0.0014,-0.0058,-4.1e-05,0.0071,0.0065,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-3.7e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32390000,0.73,0.00025,-0.004,0.69,-1.5,-0.89,0.77,-14,-6.4,-3.7e+02,-0.0014,-0.0058,-5.9e-05,0.0076,0.0064,-0.11,0.2,-0.00029,0.43,-0.0084,-0.0031,0.0011,0,0,-3.7e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00036,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32490000,0.72,0.0001,-0.0042,0.69,-1.4,-0.88,0.78,-14,-6.5,-3.7e+02,-0.0014,-0.0058,-5.7e-05,0.0074,0.0069,-0.11,0.21,-0.00029,0.43,-0.0084,-0.0031,0.0011,0,0,-3.7e+02,7.1e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32590000,0.72,0.00015,-0.0046,0.69,-1.4,-0.87,0.78,-14,-6.6,-3.7e+02,-0.0014,-0.0057,-7.8e-05,0.0078,0.0068,-0.11,0.21,-0.0003,0.43,-0.008,-0.0031,0.0013,0,0,-3.7e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32690000,0.72,0.00011,-0.0046,0.69,-1.4,-0.86,0.77,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-7.9e-05,0.0078,0.0072,-0.11,0.2,-0.0003,0.43,-0.008,-0.0031,0.0014,0,0,-3.7e+02,7e-05,5.8e-05,0.0056,0.022,0.032,0.0053,0.31,0.35,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00036,1,1,0.01 -32790000,0.72,0.00023,-0.0047,0.69,-1.3,-0.84,0.77,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-9.9e-05,0.0081,0.0072,-0.11,0.21,-0.0003,0.43,-0.0076,-0.0031,0.0015,0,0,-3.7e+02,6.8e-05,5.7e-05,0.0054,0.021,0.03,0.0053,0.3,0.35,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -32890000,0.72,0.00031,-0.0046,0.69,-1.3,-0.84,0.77,-14,-6.8,-3.7e+02,-0.0014,-0.0057,-0.00011,0.008,0.0078,-0.11,0.2,-0.0003,0.43,-0.0076,-0.0032,0.0016,0,0,-3.7e+02,6.9e-05,5.8e-05,0.0054,0.022,0.031,0.0053,0.32,0.36,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -32990000,0.72,0.00052,-0.0047,0.69,-1.3,-0.82,0.77,-15,-6.9,-3.7e+02,-0.0014,-0.0057,-0.00011,0.0085,0.008,-0.11,0.2,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-3.7e+02,6.7e-05,5.7e-05,0.0052,0.021,0.029,0.0053,0.31,0.36,0.03,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -33090000,0.72,0.00049,-0.0047,0.69,-1.3,-0.82,0.76,-15,-7,-3.7e+02,-0.0014,-0.0057,-0.00011,0.0084,0.0083,-0.11,0.21,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-3.7e+02,6.7e-05,5.7e-05,0.0053,0.022,0.031,0.0053,0.33,0.37,0.031,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -33190000,0.72,0.0039,-0.0039,0.7,-1.2,-0.8,0.7,-15,-7,-3.7e+02,-0.0014,-0.0057,-0.00012,0.0085,0.0082,-0.11,0.21,-0.0003,0.43,-0.0068,-0.0031,0.0017,0,0,-3.7e+02,6.6e-05,5.7e-05,0.0051,0.022,0.029,0.0053,0.32,0.37,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0035,8.4e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 -33290000,0.67,0.016,-0.0033,0.74,-1.2,-0.78,0.68,-15,-7.1,-3.7e+02,-0.0014,-0.0057,-0.00011,0.0083,0.0084,-0.11,0.2,-0.00028,0.43,-0.007,-0.0032,0.0016,0,0,-3.7e+02,6.6e-05,5.7e-05,0.0051,0.022,0.031,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0035,8.3e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 -33390000,0.56,0.014,-0.0037,0.83,-1.2,-0.77,0.88,-15,-7.2,-3.7e+02,-0.0014,-0.0057,-0.00013,0.0085,0.0087,-0.11,0.21,-0.00034,0.43,-0.0063,-0.0032,0.0018,0,0,-3.7e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 -33490000,0.43,0.007,-0.0011,0.9,-1.2,-0.76,0.89,-15,-7.3,-3.7e+02,-0.0014,-0.0057,-0.00014,0.0085,0.0088,-0.11,0.21,-0.00042,0.43,-0.0058,-0.002,0.0018,0,0,-3.7e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 -33590000,0.27,0.00094,-0.0036,0.96,-1.2,-0.75,0.86,-15,-7.4,-3.7e+02,-0.0014,-0.0057,-0.00018,0.0085,0.0088,-0.11,0.21,-0.00067,0.43,-0.0038,-0.0013,0.002,0,0,-3.7e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 -33690000,0.098,-0.0027,-0.0066,1,-1.1,-0.74,0.87,-15,-7.4,-3.7e+02,-0.0014,-0.0057,-0.00019,0.0085,0.0088,-0.11,0.21,-0.00072,0.43,-0.0035,-0.001,0.002,0,0,-3.7e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 -33790000,-0.074,-0.0045,-0.0084,1,-1.1,-0.72,0.85,-16,-7.5,-3.7e+02,-0.0014,-0.0057,-0.00021,0.0085,0.0088,-0.11,0.21,-0.00088,0.43,-0.0021,-0.001,0.0023,0,0,-3.7e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 -33890000,-0.24,-0.0059,-0.009,0.97,-1,-0.69,0.83,-16,-7.6,-3.7e+02,-0.0014,-0.0057,-0.00021,0.0085,0.0088,-0.11,0.21,-0.001,0.43,-0.0012,-0.0011,0.0024,0,0,-3.7e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 -33990000,-0.39,-0.0046,-0.012,0.92,-0.94,-0.64,0.8,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00022,0.0085,0.0089,-0.11,0.21,-0.00097,0.43,-0.0011,-0.00064,0.0025,0,0,-3.7e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 -34090000,-0.5,-0.0037,-0.014,0.87,-0.88,-0.59,0.81,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00022,0.0085,0.0092,-0.11,0.21,-0.00095,0.43,-0.0012,-0.00052,0.0025,0,0,-3.7e+02,6e-05,5.3e-05,0.0014,0.023,0.03,0.0052,0.37,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,3e-05,3.4e-05,0.00036,2.2e-05,2.3e-05,0.00036,1,1,0.01 -34190000,-0.57,-0.0036,-0.012,0.82,-0.86,-0.54,0.81,-16,-7.8,-3.7e+02,-0.0015,-0.0057,-0.00021,0.0063,0.012,-0.11,0.21,-0.00094,0.43,-0.001,-0.00031,0.0027,0,0,-3.7e+02,5.7e-05,5.1e-05,0.0013,0.023,0.029,0.0052,0.37,0.38,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.5e-05,3.4e-05,0.00036,1.8e-05,1.8e-05,0.00036,1,1,0.01 -34290000,-0.61,-0.0046,-0.0091,0.79,-0.8,-0.48,0.81,-16,-7.8,-3.7e+02,-0.0015,-0.0057,-0.0002,0.0061,0.012,-0.11,0.21,-0.00096,0.43,-0.00092,-0.00018,0.0027,0,0,-3.7e+02,5.7e-05,5.1e-05,0.0012,0.025,0.032,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.2e-05,3.4e-05,0.00036,1.5e-05,1.5e-05,0.00036,1,1,0.01 -34390000,-0.63,-0.0052,-0.0062,0.77,-0.77,-0.44,0.81,-16,-7.9,-3.7e+02,-0.0015,-0.0057,-0.00018,0.0033,0.016,-0.11,0.21,-0.00092,0.43,-0.00091,1.6e-05,0.0029,0,0,-3.7e+02,5.3e-05,4.9e-05,0.0012,0.025,0.031,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.2e-05,2e-05,3.3e-05,0.00036,1.3e-05,1.3e-05,0.00036,1,1,0.01 -34490000,-0.65,-0.0062,-0.004,0.76,-0.72,-0.39,0.81,-16,-7.9,-3.7e+02,-0.0015,-0.0057,-0.00018,0.0031,0.017,-0.11,0.21,-0.00093,0.43,-0.00085,-2.5e-05,0.0029,0,0,-3.7e+02,5.3e-05,4.9e-05,0.0011,0.027,0.035,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.1e-05,1.8e-05,3.3e-05,0.00036,1.2e-05,1.2e-05,0.00036,1,1,0.01 -34590000,-0.66,-0.0062,-0.0026,0.75,-0.7,-0.37,0.8,-16,-8,-3.7e+02,-0.0015,-0.0058,-0.00015,-0.0018,0.023,-0.11,0.21,-0.00088,0.43,-0.00092,3e-05,0.0032,0,0,-3.7e+02,5e-05,4.7e-05,0.0011,0.027,0.034,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,1.1e-05,1e-05,0.00036,1,1,0.01 -34690000,-0.67,-0.0066,-0.0018,0.75,-0.64,-0.32,0.8,-17,-8.1,-3.7e+02,-0.0015,-0.0058,-0.00015,-0.0021,0.023,-0.11,0.21,-0.0009,0.43,-0.00078,0.00023,0.0031,0,0,-3.7e+02,5e-05,4.7e-05,0.0011,0.03,0.037,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,9.9e-06,9.4e-06,0.00036,1,1,0.01 -34790000,-0.67,-0.006,-0.0013,0.74,-0.63,-0.3,0.79,-17,-8.1,-3.7e+02,-0.0015,-0.0058,-0.00011,-0.008,0.03,-0.11,0.21,-0.00089,0.43,-0.00062,0.00032,0.0032,0,0,-3.7e+02,4.6e-05,4.4e-05,0.001,0.029,0.036,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,9.1e-06,8.6e-06,0.00036,1,1,0.01 -34890000,-0.67,-0.006,-0.0012,0.74,-0.57,-0.26,0.79,-17,-8.2,-3.7e+02,-0.0015,-0.0058,-0.00011,-0.0081,0.03,-0.11,0.21,-0.00089,0.43,-0.00063,0.00028,0.0033,0,0,-3.7e+02,4.6e-05,4.4e-05,0.001,0.032,0.04,0.0052,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,8.4e-06,7.9e-06,0.00036,1,1,0.01 -34990000,-0.67,-0.013,-0.0037,0.74,0.47,0.35,-0.043,-17,-8.2,-3.7e+02,-0.0016,-0.0058,-6.7e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00051,0.00034,0.0035,0,0,-3.7e+02,4.2e-05,4.1e-05,0.001,0.034,0.046,0.0054,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.3e-05,3.3e-05,0.00036,8e-06,7.4e-06,0.00036,1,1,0.01 -35090000,-0.67,-0.013,-0.0037,0.74,0.6,0.39,-0.1,-17,-8.2,-3.7e+02,-0.0016,-0.0058,-6.9e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00047,0.00028,0.0035,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00099,0.037,0.051,0.0055,0.42,0.43,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.2e-05,3.3e-05,0.00036,7.5e-06,6.9e-06,0.00036,1,1,0.01 -35190000,-0.67,-0.013,-0.0038,0.74,0.63,0.43,-0.1,-17,-8.1,-3.7e+02,-0.0016,-0.0058,-6.9e-05,-0.016,0.039,-0.11,0.21,-0.00088,0.43,-0.00041,0.00032,0.0035,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00099,0.041,0.055,0.0055,0.43,0.44,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.2e-05,3.3e-05,0.00036,7.1e-06,6.4e-06,0.00036,1,1,0.01 -35290000,-0.67,-0.013,-0.0038,0.74,0.66,0.47,-0.099,-17,-8.1,-3.7e+02,-0.0016,-0.0058,-7.1e-05,-0.016,0.039,-0.11,0.21,-0.00089,0.43,-0.00033,0.00032,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00098,0.044,0.06,0.0055,0.44,0.45,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6.8e-06,6.1e-06,0.00036,1,1,0.01 -35390000,-0.67,-0.013,-0.0038,0.74,0.69,0.51,-0.096,-17,-8,-3.7e+02,-0.0016,-0.0058,-7.3e-05,-0.016,0.039,-0.11,0.21,-0.00091,0.43,-0.00023,0.00029,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00098,0.048,0.064,0.0055,0.46,0.47,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6.5e-06,5.8e-06,0.00036,1,1,0.01 -35490000,-0.67,-0.013,-0.0038,0.74,0.73,0.56,-0.095,-17,-8,-3.7e+02,-0.0016,-0.0058,-7.6e-05,-0.016,0.039,-0.11,0.21,-0.00092,0.43,-0.00014,0.00024,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00097,0.053,0.069,0.0055,0.47,0.48,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6.2e-06,5.5e-06,0.00036,1,1,0.01 -35590000,-0.67,-0.013,-0.0038,0.74,0.76,0.6,-0.094,-16,-7.9,-3.7e+02,-0.0016,-0.0058,-7.5e-05,-0.016,0.039,-0.11,0.21,-0.00091,0.43,-0.00017,0.00025,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00097,0.057,0.075,0.0055,0.49,0.5,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6e-06,5.2e-06,0.00036,1,1,0.01 -35690000,-0.67,-0.013,-0.0038,0.74,0.79,0.64,-0.092,-16,-7.9,-3.7e+02,-0.0016,-0.0058,-7.6e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-9.2e-05,0.00024,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00097,0.062,0.08,0.0056,0.51,0.52,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.8e-06,5e-06,0.00036,1,1,0.01 -35790000,-0.67,-0.013,-0.0038,0.74,0.82,0.68,-0.089,-16,-7.8,-3.7e+02,-0.0016,-0.0058,-7.7e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-6.7e-05,0.00023,0.0036,0,0,-3.7e+02,4.2e-05,4.2e-05,0.00097,0.067,0.086,0.0056,0.53,0.54,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.6e-06,4.8e-06,0.00036,1,1,0.023 -35890000,-0.67,-0.013,-0.0038,0.74,0.85,0.72,-0.086,-16,-7.7,-3.7e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-5.2e-05,0.00022,0.0037,0,0,-3.7e+02,4.2e-05,4.2e-05,0.00097,0.072,0.092,0.0056,0.55,0.56,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.4e-06,4.6e-06,0.00036,1,1,0.048 -35990000,-0.67,-0.013,-0.0038,0.74,0.88,0.76,-0.083,-16,-7.7,-3.7e+02,-0.0016,-0.0058,-7.7e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-7.7e-05,0.00021,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.078,0.098,0.0056,0.57,0.59,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,9.8e-06,3.3e-05,0.00036,5.3e-06,4.5e-06,0.00036,1,1,0.073 -36090000,-0.68,-0.013,-0.0038,0.74,0.91,0.81,-0.079,-16,-7.6,-3.7e+02,-0.0016,-0.0058,-7.9e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-4.2e-05,0.00019,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.083,0.11,0.0056,0.6,0.62,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.7e-06,3.3e-05,0.00036,5.2e-06,4.3e-06,0.00036,1,1,0.099 -36190000,-0.68,-0.013,-0.0038,0.74,0.94,0.85,-0.075,-16,-7.5,-3.7e+02,-0.0016,-0.0058,-8.3e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,5.5e-05,0.00018,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.089,0.11,0.0056,0.63,0.65,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.5e-06,3.3e-05,0.00036,5e-06,4.2e-06,0.00036,1,1,0.12 -36290000,-0.68,-0.013,-0.0038,0.74,0.97,0.89,-0.07,-16,-7.4,-3.7e+02,-0.0016,-0.0058,-8.4e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,7.5e-05,0.00018,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.095,0.12,0.0056,0.66,0.68,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.4e-06,3.3e-05,0.00036,4.9e-06,4.1e-06,0.00036,1,1,0.15 -36390000,-0.68,-0.013,-0.0038,0.74,1,0.93,-0.067,-16,-7.3,-3.7e+02,-0.0016,-0.0058,-8.4e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,7.3e-05,0.00022,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.1,0.13,0.0056,0.69,0.72,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.3e-06,3.3e-05,0.00036,4.8e-06,3.9e-06,0.00036,1,1,0.17 -36490000,-0.68,-0.013,-0.0038,0.74,1,0.97,-0.063,-16,-7.2,-3.7e+02,-0.0016,-0.0058,-8.3e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,4.3e-05,0.00023,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.11,0.13,0.0056,0.72,0.76,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.2e-06,3.3e-05,0.00036,4.7e-06,3.8e-06,0.00036,1,1,0.2 -36590000,-0.68,-0.013,-0.0038,0.74,1.1,1,-0.057,-16,-7.1,-3.7e+02,-0.0016,-0.0058,-8.5e-05,-0.015,0.039,-0.11,0.21,-0.00096,0.43,9.1e-05,0.00025,0.0037,0,0,-3.7e+02,4.3e-05,4.3e-05,0.00096,0.12,0.14,0.0056,0.76,0.8,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.1e-06,3.3e-05,0.00036,4.6e-06,3.7e-06,0.00036,1,1,0.23 -36690000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.053,-15,-7,-3.7e+02,-0.0016,-0.0058,-8.7e-05,-0.015,0.039,-0.11,0.21,-0.00096,0.43,0.00012,0.00026,0.0037,0,0,-3.7e+02,4.3e-05,4.3e-05,0.00096,0.12,0.15,0.0057,0.8,0.84,0.032,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9e-06,3.3e-05,0.00036,4.6e-06,3.6e-06,0.00036,1,1,0.25 -36790000,-0.68,-0.013,-0.0037,0.74,1.1,1.1,-0.047,-15,-6.9,-3.7e+02,-0.0016,-0.0058,-9.1e-05,-0.015,0.038,-0.11,0.21,-0.00097,0.43,0.00017,0.00022,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.13,0.16,0.0057,0.84,0.9,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.9e-06,3.3e-05,0.00036,4.5e-06,3.5e-06,0.00036,1,1,0.28 -36890000,-0.68,-0.013,-0.0037,0.74,1.2,1.1,-0.042,-15,-6.8,-3.7e+02,-0.0016,-0.0058,-9.4e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00021,0.00022,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0056,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00035,1,1,0.3 -36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,-15,-6.7,-3.7e+02,-0.0016,-0.0058,-9.5e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00023,0.00023,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00035,1,1,0.33 -37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,-15,-6.6,-3.7e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00023,0.00026,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00035,1,1,0.35 -37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,-15,-6.4,-3.7e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00023,0.00026,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00035,1,1,0.38 -37290000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.02,-15,-6.3,-3.7e+02,-0.0016,-0.0058,-9.7e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00024,0.00026,0.0037,0,0,-3.7e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00035,1,1,0.41 -37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,-15,-6.2,-3.7e+02,-0.0016,-0.0058,-9.9e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00027,0.00027,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00035,1,1,0.43 -37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0093,-14,-6,-3.7e+02,-0.0016,-0.0058,-0.0001,-0.015,0.038,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.3e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00035,1,1,0.46 -37590000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0027,-14,-5.9,-3.7e+02,-0.0016,-0.0058,-0.0001,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00032,0.0003,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.2,0.23,0.0057,1.3,1.5,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.3e-06,3.3e-05,0.00036,4e-06,3e-06,0.00035,1,1,0.48 -37690000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,0.0046,-14,-5.7,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00033,0.00029,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.21,0.24,0.0057,1.4,1.6,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.51 -37790000,-0.68,-0.013,-0.0036,0.74,1.5,1.5,0.012,-14,-5.6,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00034,0.0003,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.22,0.25,0.0056,1.5,1.7,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.54 -37890000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.017,-14,-5.4,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0038,0,0,-3.7e+02,4.5e-05,4.5e-05,0.00096,0.23,0.27,0.0056,1.6,1.8,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.56 -37990000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.025,-14,-5.3,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0037,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.24,0.28,0.0056,1.7,1.9,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.59 -38090000,-0.68,-0.013,-0.0036,0.74,1.5,1.7,0.034,-14,-5.1,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00037,0.00029,0.0037,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.25,0.29,0.0056,1.8,2,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.61 -38190000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.04,-13,-4.9,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00029,0.0038,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.26,0.3,0.0056,1.9,2.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.64 -38290000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.046,-13,-4.7,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00027,0.0038,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.27,0.31,0.0056,2.1,2.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.67 -38390000,-0.68,-0.013,-0.0035,0.74,1.6,1.8,0.052,-13,-4.6,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00029,0.0037,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.28,0.32,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.69 -38490000,-0.68,-0.013,-0.0035,0.74,1.7,1.8,0.058,-13,-4.4,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00031,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00096,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.72 -38590000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.063,-13,-4.2,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00035,1,1,0.75 -38690000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.069,-13,-4,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.77 -38790000,-0.68,-0.013,-0.0034,0.74,1.8,1.9,0.075,-12,-3.8,-3.7e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.1,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 -38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,-12,-3.6,-3.7e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.0003,0.0037,0,0,-3.7e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,0,0,-1.2e+02,-3e-11,-2.6e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 +290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,0,0,-1.2e+02,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 +390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0,0,-1.2e+02,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 +490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0,0,-1.2e+02,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 +590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0,0,-1.2e+02,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 +690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0,0,-1.2e+02,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,-1.2e+02,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 +790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0,0,-1.2e+02,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.018,0.018,0.00077,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 +890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0,0,-1.2e+02,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.019,0.019,0.00051,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 +990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0,0,-1.2e+02,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.021,0.021,0.00062,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 +1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0,0,-1.2e+02,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.023,0.023,0.00043,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 +1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0,0,-1.2e+02,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00051,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 +1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0,0,-1.2e+02,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.00038,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 +1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0,0,-1.2e+02,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00043,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 +1490000,1,-0.01,-0.014,0.00016,0.024,0.0029,-0.16,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.03,0.03,0.00037,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.011,-0.014,0.00013,0.028,-0.00014,-0.19,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.011,-0.014,9.8e-05,0.035,-0.002,-0.2,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.011,-0.015,7.8e-05,0.043,-0.0033,-0.22,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,8.9e-05,0.036,-0.0048,-0.23,0,0,-1.2e+02,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,5.1e-05,0.041,-0.0073,-0.24,0,0,-1.2e+02,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,6.4e-05,0.033,-0.007,-0.26,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,5.1e-05,0.039,-0.0095,-0.27,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,6.9e-05,0.03,-0.0089,-0.29,0,0,-1.2e+02,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.013,5.1e-05,0.035,-0.011,-0.3,0,0,-1.2e+02,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.011,-0.013,6.6e-05,0.026,-0.0093,-0.31,0,0,-1.2e+02,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.011,-0.013,6.3e-05,0.03,-0.011,-0.33,0,0,-1.2e+02,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.011,-0.013,5.8e-05,0.023,-0.0096,-0.34,0,0,-1.2e+02,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.011,-0.013,8.3e-06,0.027,-0.012,-0.35,0,0,-1.2e+02,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.011,-0.013,5.6e-05,0.022,-0.0098,-0.36,0,0,-1.2e+02,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.011,-0.013,6e-05,0.025,-0.011,-0.38,0,0,-1.2e+02,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.011,-0.013,2.9e-06,0.02,-0.009,-0.39,0,0,-1.2e+02,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.011,-0.013,4.4e-05,0.023,-0.011,-0.4,0,0,-1.2e+02,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.011,-0.012,1.5e-05,0.018,-0.0095,-0.42,0,0,-1.2e+02,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.011,-0.013,8.2e-06,0.022,-0.012,-0.43,0,0,-1.2e+02,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.011,-0.012,3.2e-05,0.017,-0.011,-0.44,0,0,-1.2e+02,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.011,-0.012,0.00015,0.019,-0.014,-0.46,0,0,-1.2e+02,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.011,-0.012,0.0002,0.016,-0.014,-0.47,0,0,-1.2e+02,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.48,0,0,-1.2e+02,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.011,-0.012,0.00017,0.02,-0.017,-0.5,0,0,-1.2e+02,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.51,0,0,-1.2e+02,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.011,-0.012,0.00014,0.02,-0.017,-0.53,0,0,-1.2e+02,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.01,-0.012,9.8e-05,0.017,-0.012,-0.54,0,0,-1.2e+02,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.01,-0.012,0.00012,0.018,-0.013,-0.55,0,0,-1.2e+02,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.01,-0.012,0.00018,0.014,-0.01,-0.57,0,0,-1.2e+02,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.01,-0.012,0.00021,0.017,-0.012,-0.58,0,0,-1.2e+02,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.01,-0.012,0.00021,0.014,-0.01,-0.6,0,0,-1.2e+02,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.01,-0.012,0.0002,0.015,-0.012,-0.61,0,0,-1.2e+02,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.01,-0.011,0.00019,0.012,-0.01,-0.63,0,0,-1.2e+02,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.01,-0.012,0.00017,0.015,-0.011,-0.64,0,0,-1.2e+02,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.01,-0.011,0.00022,0.011,-0.0085,-0.66,0,0,-1.2e+02,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.01,-0.011,0.00024,0.013,-0.0098,-0.67,0,0,-1.2e+02,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.01,-0.011,0.00023,0.0086,-0.0073,-0.68,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0099,-0.011,0.00029,0.0081,-0.0081,-0.7,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0098,-0.011,0.0003,0.0055,-0.0062,-0.71,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0097,-0.011,0.00028,0.0061,-0.0066,-0.73,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0096,-0.011,0.00036,0.0041,-0.0038,-0.74,0,0,-1.2e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0095,-0.011,0.00035,0.0044,-0.0028,-0.75,0,0,-1.2e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0095,-0.011,0.00033,0.0038,-0.00092,0.0028,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0094,-0.011,0.00036,0.0041,0.00056,0.015,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0094,-0.011,0.00034,0.0051,0.0017,-0.011,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0094,-0.011,0.00027,0.0038,0.0042,-0.005,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0094,-0.011,0.00024,0.005,0.0043,-0.012,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0093,-0.011,0.00026,0.0043,0.0053,-0.05,0,0,-4.9e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0093,-0.011,0.00026,0.0049,0.0055,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0093,-0.011,0.00019,0.0037,0.0055,-0.099,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0093,-0.011,0.00012,0.0046,0.0051,-0.076,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0051,-0.11,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,-6.9e-05,0.21,8e-05,0.43,3.6e-05,0.00032,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 +6890000,0.71,0.0013,-0.014,0.7,0.00053,0.0064,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,-9.7e-05,0.21,1.3e-05,0.43,1.3e-06,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 +6990000,0.71,0.0013,-0.014,0.71,-0.00023,0.0046,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 +7090000,0.71,0.0012,-0.014,0.71,-0.00079,0.0011,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00037,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 +7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0019,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.7e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 +7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.0099,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00021,-0.0002,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00039,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00028,-3.8e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00032,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.011,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00018,-7.3e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00078,-0.00043,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7590000,0.71,0.0016,-0.014,0.71,-0.0034,0.02,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-6.8e-05,-0.00024,0.00021,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7690000,0.71,0.0016,-0.014,0.71,-0.0046,0.017,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00032,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 +7790000,0.71,0.0017,-0.014,0.71,-0.011,0.016,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00036,6.2e-05,-0.0071,0.21,-1.6e-05,0.43,-0.00034,0.00078,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 +7890000,0.71,0.0017,-0.014,0.71,-0.011,0.021,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00038,0.0002,-0.0096,0.21,-1.5e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 +7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00037,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 +8090000,0.71,0.0017,-0.014,0.71,-0.005,0.024,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.7e-05,-0.0003,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00033,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8190000,0.71,0.0017,-0.014,0.71,-0.014,0.028,-0.18,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00043,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8290000,0.71,0.0017,-0.014,0.71,-0.017,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00056,0.00031,-0.017,0.21,-1e-05,0.43,-0.00031,0.00079,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00041,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.021,0.21,-9.3e-06,0.43,-0.0003,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8490000,0.71,0.0017,-0.014,0.71,-0.003,0.0023,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.025,0.21,-9e-06,0.43,-0.00031,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8590000,0.71,0.0021,-0.014,0.71,-0.00044,0.00093,-0.17,0,0,-4.9e+02,-0.0018,-0.0057,-6.8e-05,-0.00051,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00042,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8690000,0.71,0.002,-0.014,0.71,-0.0034,0.0028,-0.16,0,0,-4.9e+02,-0.0017,-0.0056,-6.6e-05,-0.00051,0.00039,-0.035,0.21,-1e-05,0.43,-0.00039,0.00084,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.088,5.8e-05,6.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8790000,0.71,0.0019,-0.014,0.71,-0.0057,0.0049,-0.15,0,0,-4.9e+02,-0.0017,-0.0057,-6.9e-05,-0.0005,0.00043,-0.041,0.21,-9.1e-06,0.43,-0.00037,0.0008,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.087,5.6e-05,6.6e-05,2.4e-06,0.04,0.04,0.032,0.0013,6.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8890000,0.71,0.0019,-0.014,0.71,-0.0079,0.0055,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00063,0.00042,-0.045,0.21,-7.8e-06,0.43,-0.00034,0.00078,-0.00033,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.086,5.3e-05,6.4e-05,2.4e-06,0.04,0.04,0.03,0.0013,6.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +8990000,0.71,0.0018,-0.014,0.71,-0.01,0.0049,-0.14,0,0,-4.9e+02,-0.0015,-0.0058,-7.5e-05,-0.00087,0.00044,-0.051,0.21,-6.6e-06,0.43,-0.0003,0.00071,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.087,5.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.029,0.0013,6.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9090000,0.71,0.002,-0.014,0.71,-0.014,0.006,-0.14,0,0,-4.9e+02,-0.0016,-0.0058,-7.6e-05,-0.00098,0.00054,-0.053,0.21,-6.9e-06,0.43,-0.00033,0.00068,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1.1e+02,1.1e+02,0.086,4.9e-05,6.2e-05,2.4e-06,0.04,0.04,0.028,0.0013,5.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9190000,0.71,0.0019,-0.014,0.71,-0.012,0.0087,-0.14,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.00086,0.00059,-0.057,0.21,-6.2e-06,0.43,-0.00031,0.00081,-0.00025,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.094,1.1e+02,1.1e+02,0.085,4.6e-05,6e-05,2.4e-06,0.04,0.04,0.027,0.0013,5.6e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9290000,0.71,0.0017,-0.014,0.71,-0.011,0.0086,-0.14,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00096,0.00059,-0.061,0.21,-5.1e-06,0.43,-0.00026,0.00082,-0.00024,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.1e+02,1.1e+02,0.085,4.4e-05,5.8e-05,2.4e-06,0.04,0.04,0.025,0.0013,5.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9390000,0.71,0.0015,-0.014,0.71,-0.011,0.0092,-0.14,0,0,-4.9e+02,-0.0014,-0.0056,-7.3e-05,-0.001,0.00058,-0.065,0.21,-4.3e-06,0.43,-0.00022,0.00082,-0.00022,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.2e+02,1.2e+02,0.086,4.2e-05,5.7e-05,2.4e-06,0.04,0.04,0.024,0.0013,5.2e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9490000,0.71,0.0014,-0.014,0.71,-0.0078,0.011,-0.13,0,0,-4.9e+02,-0.0014,-0.0055,-6.9e-05,-0.001,0.0006,-0.068,0.21,-3.7e-06,0.43,-0.00018,0.0009,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.091,1.2e+02,1.2e+02,0.085,4e-05,5.5e-05,2.4e-06,0.04,0.04,0.023,0.0013,5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9590000,0.71,0.0016,-0.014,0.71,-0.01,0.016,-0.13,0,0,-4.9e+02,-0.0015,-0.0054,-6.6e-05,-0.0011,0.00079,-0.072,0.21,-4.5e-06,0.43,-0.00024,0.00091,-0.00012,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.09,1.3e+02,1.3e+02,0.085,3.8e-05,5.4e-05,2.4e-06,0.04,0.04,0.021,0.0013,4.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9690000,0.71,0.0018,-0.014,0.71,-0.017,0.017,-0.12,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.0013,0.00092,-0.077,0.21,-4.8e-06,0.43,-0.00027,0.0008,-0.00017,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.089,1.3e+02,1.3e+02,0.086,3.6e-05,5.2e-05,2.4e-06,0.04,0.04,0.02,0.0013,4.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9790000,0.71,0.0017,-0.014,0.71,-0.013,0.02,-0.11,0,0,-4.9e+02,-0.0015,-0.0055,-6.8e-05,-0.0014,0.001,-0.082,0.21,-4.4e-06,0.43,-0.00024,0.00085,-9.7e-05,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.087,1.4e+02,1.4e+02,0.085,3.4e-05,5e-05,2.4e-06,0.04,0.04,0.019,0.0013,4.5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9890000,0.71,0.0018,-0.014,0.71,-0.016,0.023,-0.11,0,0,-4.9e+02,-0.0015,-0.0055,-6.8e-05,-0.0015,0.0011,-0.085,0.21,-4.6e-06,0.43,-0.00026,0.00083,-0.0001,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.084,1.4e+02,1.4e+02,0.085,3.2e-05,4.9e-05,2.4e-06,0.04,0.04,0.018,0.0013,4.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9990000,0.71,0.0019,-0.014,0.71,-0.02,0.021,-0.1,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.0016,0.0011,-0.089,0.21,-4.2e-06,0.43,-0.00025,0.00078,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.083,1.5e+02,1.5e+02,0.086,3e-05,4.7e-05,2.4e-06,0.04,0.04,0.017,0.0013,4.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +10090000,0.71,0.002,-0.014,0.71,-0.022,0.025,-0.096,0,0,-4.9e+02,-0.0015,-0.0056,-6.9e-05,-0.0017,0.0013,-0.091,0.21,-4.6e-06,0.43,-0.00029,0.00079,-0.00012,0,0,-4.9e+02,0.0013,0.0012,0.039,25,25,0.08,1.6e+02,1.6e+02,0.085,2.9e-05,4.6e-05,2.4e-06,0.04,0.04,0.016,0.0013,4.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10190000,0.71,0.002,-0.013,0.71,-0.033,0.022,-0.096,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.0019,0.0013,-0.093,0.21,-4.5e-06,0.43,-0.00027,0.00065,-0.00015,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.078,1.7e+02,1.7e+02,0.084,2.7e-05,4.4e-05,2.4e-06,0.04,0.04,0.014,0.0013,4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10290000,0.71,0.002,-0.013,0.71,-0.04,0.017,-0.084,0,0,-4.9e+02,-0.0015,-0.0058,-7.8e-05,-0.0021,0.0014,-0.098,0.21,-4.2e-06,0.43,-0.00026,0.00058,-0.00017,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.076,1.8e+02,1.8e+02,0.085,2.6e-05,4.2e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10390000,0.71,0.0018,-0.013,0.71,0.0091,-0.02,0.0093,0,0,-4.9e+02,-0.0015,-0.0058,-7.9e-05,-0.0021,0.0013,-0.098,0.21,-3.7e-06,0.43,-0.00023,0.0006,-0.00014,0,0,-4.9e+02,0.0012,0.0012,0.039,0.25,0.25,0.56,0.5,0.5,0.077,2.4e-05,4.1e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10490000,0.71,0.0018,-0.013,0.71,0.0071,-0.019,0.022,0,0,-4.9e+02,-0.0014,-0.0059,-8.3e-05,-0.0022,0.0013,-0.098,0.21,-3.3e-06,0.43,-0.00021,0.00053,-0.00016,0,0,-4.9e+02,0.0012,0.0012,0.039,0.26,0.26,0.55,0.51,0.51,0.079,2.3e-05,3.9e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10590000,0.71,0.0022,-0.013,0.71,0.006,-0.0082,0.024,0,0,-4.9e+02,-0.0016,-0.0059,-7.9e-05,-0.0024,0.0018,-0.098,0.21,-4.5e-06,0.43,-0.00029,0.00053,-0.00017,0,0,-4.9e+02,0.0012,0.0011,0.039,0.13,0.13,0.27,0.17,0.17,0.072,2.2e-05,3.8e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10690000,0.71,0.0021,-0.013,0.71,0.0033,-0.009,0.027,0,0,-4.9e+02,-0.0015,-0.0059,-8.1e-05,-0.0024,0.0018,-0.098,0.21,-4.1e-06,0.43,-0.00027,0.00052,-0.00017,0,0,-4.9e+02,0.0012,0.0011,0.039,0.14,0.14,0.26,0.18,0.18,0.077,2.1e-05,3.6e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10790000,0.71,0.002,-0.013,0.71,0.0034,-0.0064,0.021,0,0,-4.9e+02,-0.0015,-0.0059,-8e-05,-0.0025,0.0021,-0.098,0.21,-4e-06,0.43,-0.00026,0.00056,-0.00016,0,0,-4.9e+02,0.0012,0.0011,0.038,0.093,0.094,0.17,0.11,0.11,0.071,1.9e-05,3.4e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10890000,0.71,0.0019,-0.013,0.71,0.0021,-0.0066,0.016,0,0,-4.9e+02,-0.0015,-0.0058,-8.1e-05,-0.0025,0.0021,-0.098,0.21,-3.7e-06,0.43,-0.00025,0.00057,-0.00015,0,0,-4.9e+02,0.0011,0.0011,0.038,0.1,0.1,0.16,0.11,0.11,0.074,1.8e-05,3.3e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +10990000,0.71,0.0017,-0.014,0.71,0.0068,-0.0015,0.011,0,0,-4.9e+02,-0.0013,-0.0057,-7.9e-05,-0.0025,0.0031,-0.098,0.21,-3.2e-06,0.43,-0.00022,0.00071,-0.00011,0,0,-4.9e+02,0.0011,0.001,0.038,0.079,0.08,0.12,0.079,0.079,0.071,1.7e-05,3.1e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11090000,0.71,0.0017,-0.014,0.71,0.0068,0.0018,0.014,0,0,-4.9e+02,-0.0014,-0.0056,-7.5e-05,-0.0023,0.0029,-0.098,0.21,-3.3e-06,0.43,-0.00023,0.00078,-6.8e-05,0,0,-4.9e+02,0.0011,0.00099,0.038,0.09,0.091,0.12,0.085,0.085,0.073,1.6e-05,2.9e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11190000,0.71,0.0017,-0.014,0.71,0.01,0.0034,0.0027,0,0,-4.9e+02,-0.0013,-0.0056,-7.8e-05,-0.0023,0.004,-0.098,0.21,-3.4e-06,0.43,-0.00024,0.00079,-8.6e-05,0,0,-4.9e+02,0.00098,0.00092,0.038,0.074,0.075,0.091,0.066,0.066,0.068,1.5e-05,2.7e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11290000,0.71,0.0016,-0.014,0.71,0.0094,0.0018,0.0014,0,0,-4.9e+02,-0.0013,-0.0057,-8.3e-05,-0.0026,0.0043,-0.098,0.21,-3e-06,0.43,-0.00022,0.00073,-0.00011,0,0,-4.9e+02,0.00098,0.00091,0.038,0.086,0.087,0.087,0.072,0.072,0.071,1.4e-05,2.6e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11390000,0.71,0.0016,-0.013,0.71,0.0049,0.00087,-0.0044,0,0,-4.9e+02,-0.0013,-0.0058,-8.6e-05,-0.0033,0.0041,-0.098,0.21,-3e-06,0.43,-0.00021,0.00066,-0.00016,0,0,-4.9e+02,0.00087,0.00082,0.038,0.072,0.073,0.072,0.058,0.058,0.067,1.3e-05,2.4e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11490000,0.71,0.0014,-0.013,0.71,0.0011,-0.002,-0.0044,0,0,-4.9e+02,-0.0012,-0.0059,-9.3e-05,-0.0039,0.0049,-0.098,0.21,-2.7e-06,0.43,-0.00018,0.00056,-0.00022,0,0,-4.9e+02,0.00086,0.00081,0.038,0.083,0.086,0.069,0.064,0.064,0.068,1.3e-05,2.3e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11590000,0.71,0.0013,-0.013,0.71,-0.002,-0.0015,-0.01,0,0,-4.9e+02,-0.0012,-0.0059,-9.4e-05,-0.0045,0.005,-0.098,0.21,-2.5e-06,0.43,-0.00016,0.00054,-0.00023,0,0,-4.9e+02,0.00075,0.00072,0.038,0.07,0.072,0.059,0.054,0.054,0.066,1.2e-05,2.1e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11690000,0.71,0.0011,-0.013,0.71,-0.0035,-0.0027,-0.016,0,0,-4.9e+02,-0.0011,-0.006,-9.8e-05,-0.0053,0.0052,-0.098,0.21,-2e-06,0.43,-0.0001,0.00053,-0.00026,0,0,-4.9e+02,0.00075,0.00071,0.038,0.082,0.084,0.057,0.06,0.06,0.066,1.1e-05,2e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11790000,0.71,0.0011,-0.013,0.71,-0.0075,-0.00084,-0.018,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.0071,0.0053,-0.098,0.21,-2e-06,0.43,-8.6e-05,0.0005,-0.00027,0,0,-4.9e+02,0.00065,0.00063,0.038,0.068,0.07,0.051,0.051,0.051,0.063,1e-05,1.8e-05,2.3e-06,0.028,0.03,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11890000,0.71,0.00095,-0.013,0.71,-0.0085,-0.0037,-0.02,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0078,0.0059,-0.098,0.21,-1.9e-06,0.43,-5.4e-05,0.00047,-0.00032,0,0,-4.9e+02,0.00065,0.00062,0.038,0.08,0.082,0.049,0.058,0.058,0.063,9.8e-06,1.8e-05,2.3e-06,0.028,0.029,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11990000,0.71,0.0013,-0.013,0.71,-0.012,0.0012,-0.026,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.0074,0.0064,-0.097,0.21,-2.9e-06,0.43,-0.00013,0.00048,-0.00032,0,0,-4.9e+02,0.00056,0.00055,0.037,0.066,0.068,0.045,0.049,0.049,0.062,9.2e-06,1.6e-05,2.3e-06,0.026,0.028,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +12090000,0.71,0.0014,-0.013,0.71,-0.013,0.0044,-0.032,0,0,-4.9e+02,-0.0012,-0.0059,-9.5e-05,-0.0063,0.0056,-0.097,0.21,-3e-06,0.43,-0.00016,0.00053,-0.00028,0,0,-4.9e+02,0.00056,0.00055,0.037,0.077,0.079,0.045,0.056,0.057,0.062,8.8e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12190000,0.71,0.0012,-0.013,0.71,-0.0068,0.004,-0.026,0,0,-4.9e+02,-0.0011,-0.0059,-9.6e-05,-0.0062,0.006,-0.099,0.21,-2.3e-06,0.43,-0.00011,0.0006,-0.00026,0,0,-4.9e+02,0.00048,0.00048,0.037,0.064,0.066,0.041,0.048,0.048,0.059,8.2e-06,1.4e-05,2.3e-06,0.023,0.026,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12290000,0.71,0.001,-0.013,0.71,-0.0081,0.0044,-0.026,0,0,-4.9e+02,-0.0011,-0.0059,-9.6e-05,-0.0067,0.0056,-0.099,0.21,-2e-06,0.43,-8.1e-05,0.0006,-0.00026,0,0,-4.9e+02,0.00048,0.00048,0.037,0.073,0.076,0.042,0.056,0.056,0.06,7.9e-06,1.4e-05,2.3e-06,0.023,0.025,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12390000,0.71,0.001,-0.013,0.71,-0.0053,0.0032,-0.023,0,0,-4.9e+02,-0.0011,-0.0059,-9.9e-05,-0.0053,0.0073,-0.1,0.21,-2.5e-06,0.43,-0.00012,0.00061,-0.00025,0,0,-4.9e+02,0.00042,0.00043,0.037,0.061,0.063,0.039,0.048,0.048,0.058,7.4e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12490000,0.71,0.00093,-0.013,0.71,-0.007,0.0028,-0.027,0,0,-4.9e+02,-0.0011,-0.0059,-0.0001,-0.0058,0.0085,-0.1,0.21,-2.7e-06,0.43,-0.00015,0.00054,-0.00025,0,0,-4.9e+02,0.00042,0.00042,0.037,0.069,0.072,0.04,0.055,0.055,0.058,7.1e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12590000,0.71,0.0011,-0.013,0.71,-0.014,0.0037,-0.033,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0051,0.0078,-0.1,0.21,-3e-06,0.43,-0.00019,0.00051,-0.00024,0,0,-4.9e+02,0.00037,0.00038,0.037,0.058,0.059,0.038,0.047,0.048,0.057,6.8e-06,1.2e-05,2.3e-06,0.019,0.022,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12690000,0.71,0.0014,-0.013,0.71,-0.017,0.0046,-0.037,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0034,0.0091,-0.1,0.21,-3.9e-06,0.43,-0.00026,0.0005,-0.00025,0,0,-4.9e+02,0.00037,0.00038,0.037,0.065,0.067,0.039,0.055,0.055,0.057,6.5e-06,1.1e-05,2.3e-06,0.019,0.022,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12790000,0.71,0.0012,-0.013,0.71,-0.02,0.0027,-0.041,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0049,0.008,-0.1,0.21,-3.3e-06,0.43,-0.0002,0.00048,-0.00027,0,0,-4.9e+02,0.00033,0.00034,0.037,0.054,0.056,0.037,0.047,0.047,0.056,6.2e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12890000,0.71,0.0011,-0.013,0.71,-0.019,0.0014,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0063,0.0076,-0.1,0.21,-2.8e-06,0.43,-0.00017,0.00047,-0.00026,0,0,-4.9e+02,0.00033,0.00034,0.037,0.061,0.063,0.038,0.054,0.055,0.057,6e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +12990000,0.71,0.001,-0.013,0.71,-0.009,0.0019,-0.039,0,0,-4.9e+02,-0.0011,-0.0059,-9.9e-05,-0.0039,0.008,-0.1,0.21,-2.7e-06,0.43,-0.00018,0.00058,-0.0002,0,0,-4.9e+02,0.0003,0.00031,0.037,0.051,0.053,0.037,0.047,0.047,0.055,5.7e-06,9.9e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13090000,0.71,0.00094,-0.013,0.71,-0.0097,-0.00016,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.005,0.0096,-0.1,0.21,-3e-06,0.43,-0.00019,0.00052,-0.00023,0,0,-4.9e+02,0.0003,0.00031,0.037,0.057,0.059,0.038,0.054,0.054,0.056,5.5e-06,9.6e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13190000,0.71,0.00094,-0.013,0.71,-0.0023,0.0008,-0.034,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0032,0.011,-0.11,0.21,-3.4e-06,0.43,-0.00023,0.00056,-0.00021,0,0,-4.9e+02,0.00027,0.00029,0.037,0.048,0.05,0.037,0.047,0.047,0.054,5.2e-06,9.1e-06,2.3e-06,0.015,0.019,0.0098,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13290000,0.71,0.00079,-0.013,0.71,-0.00082,0.0015,-0.029,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0047,0.0096,-0.11,0.21,-2.5e-06,0.43,-0.00017,0.00058,-0.0002,0,0,-4.9e+02,0.00027,0.00028,0.037,0.054,0.055,0.038,0.054,0.054,0.055,5.1e-06,8.9e-06,2.3e-06,0.015,0.019,0.0097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13390000,0.71,0.0007,-0.013,0.71,0.00021,0.0023,-0.024,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0042,0.0088,-0.11,0.21,-2.3e-06,0.43,-0.00016,0.00063,-0.00019,0,0,-4.9e+02,0.00025,0.00026,0.037,0.045,0.047,0.037,0.047,0.047,0.054,4.8e-06,8.5e-06,2.3e-06,0.014,0.018,0.0091,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13490000,0.71,0.0007,-0.013,0.71,0.00018,0.0024,-0.022,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0042,0.0081,-0.11,0.21,-1.9e-06,0.43,-0.00014,0.00063,-0.00017,0,0,-4.9e+02,0.00025,0.00026,0.037,0.05,0.052,0.038,0.054,0.054,0.055,4.7e-06,8.2e-06,2.3e-06,0.014,0.018,0.009,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13590000,0.71,0.00072,-0.013,0.71,-5.1e-05,0.0028,-0.024,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.004,0.0093,-0.11,0.21,-2.4e-06,0.43,-0.00017,0.00062,-0.00019,0,0,-4.9e+02,0.00023,0.00025,0.037,0.042,0.044,0.037,0.046,0.047,0.054,4.5e-06,7.9e-06,2.3e-06,0.013,0.017,0.0085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13690000,0.71,0.0007,-0.013,0.71,0.00087,0.0053,-0.029,0,0,-4.9e+02,-0.001,-0.0059,-9.9e-05,-0.0035,0.0081,-0.11,0.21,-2e-06,0.43,-0.00016,0.00064,-0.00016,0,0,-4.9e+02,0.00023,0.00024,0.037,0.047,0.049,0.038,0.053,0.054,0.055,4.3e-06,7.7e-06,2.3e-06,0.013,0.017,0.0083,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13790000,0.71,0.00074,-0.013,0.71,0.00057,0.0021,-0.03,0,0,-4.9e+02,-0.0011,-0.0059,-9.9e-05,-0.002,0.0088,-0.11,0.21,-2.4e-06,0.43,-0.00019,0.00065,-0.00015,0,0,-4.9e+02,0.00022,0.00023,0.037,0.04,0.041,0.036,0.046,0.046,0.053,4.2e-06,7.3e-06,2.3e-06,0.013,0.016,0.0078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13890000,0.71,0.00059,-0.013,0.71,0.0024,0.0021,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.0034,0.0077,-0.11,0.21,-1.7e-06,0.43,-0.00015,0.00065,-0.00015,0,0,-4.9e+02,0.00022,0.00023,0.037,0.044,0.046,0.037,0.053,0.053,0.055,4e-06,7.1e-06,2.3e-06,0.012,0.016,0.0076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13990000,0.71,0.00065,-0.013,0.71,0.0018,0.0005,-0.034,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.0021,0.0083,-0.11,0.21,-2e-06,0.43,-0.00018,0.00065,-0.00013,0,0,-4.9e+02,0.00021,0.00022,0.037,0.037,0.039,0.036,0.046,0.046,0.054,3.9e-06,6.8e-06,2.3e-06,0.012,0.015,0.0071,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +14090000,0.71,0.00072,-0.013,0.71,0.0014,0.0019,-0.035,0,0,-4.9e+02,-0.0011,-0.0059,-9.4e-05,-0.00064,0.0072,-0.11,0.21,-1.8e-06,0.43,-0.00018,0.00068,-9.7e-05,0,0,-4.9e+02,0.00021,0.00022,0.037,0.041,0.043,0.036,0.053,0.053,0.054,3.8e-06,6.7e-06,2.3e-06,0.012,0.015,0.007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14190000,0.71,0.00068,-0.013,0.71,0.0044,0.0018,-0.037,0,0,-4.9e+02,-0.0011,-0.0059,-9.3e-05,-0.00018,0.007,-0.11,0.21,-1.6e-06,0.43,-0.00017,0.0007,-8e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.035,0.037,0.035,0.046,0.046,0.054,3.6e-06,6.4e-06,2.3e-06,0.011,0.015,0.0065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14290000,0.71,0.00075,-0.013,0.71,0.0046,0.0032,-0.035,0,0,-4.9e+02,-0.0011,-0.0058,-9.2e-05,0.00066,0.0069,-0.11,0.21,-1.7e-06,0.43,-0.00019,0.00071,-6.2e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.039,0.04,0.036,0.052,0.052,0.055,3.5e-06,6.2e-06,2.3e-06,0.011,0.014,0.0063,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14390000,0.71,0.00063,-0.014,0.71,0.007,0.0049,-0.037,0,0,-4.9e+02,-0.0011,-0.0058,-8.9e-05,0.00034,0.0054,-0.11,0.21,-9e-07,0.43,-0.00015,0.00074,-4.6e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14490000,0.71,0.00052,-0.013,0.71,0.008,0.0064,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,-0.00096,0.0049,-0.11,0.21,-4.1e-07,0.43,-0.00013,0.0007,-4.6e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14590000,0.71,0.00042,-0.013,0.71,0.006,0.0049,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,-0.0019,0.0047,-0.11,0.21,-3.2e-07,0.43,-0.00012,0.00067,-5.9e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14690000,0.71,0.00037,-0.013,0.71,0.0078,0.003,-0.038,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.0018,0.0038,-0.11,0.21,6.3e-08,0.43,-0.0001,0.00068,-4.6e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14790000,0.71,0.00035,-0.013,0.71,0.0055,0.0016,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.002,0.0036,-0.12,0.21,7.8e-09,0.43,-0.0001,0.00066,-4.7e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0098,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0033,-0.037,0,0,-4.9e+02,-0.001,-0.0058,-8.6e-05,-0.0022,0.0031,-0.12,0.21,2.7e-07,0.43,-9e-05,0.00067,-4.4e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0097,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +14990000,0.71,0.00027,-0.013,0.71,0.0067,0.0023,-0.032,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.0028,0.0036,-0.12,0.21,7.5e-08,0.43,-9.9e-05,0.00064,-5.6e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.013,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15090000,0.71,0.0002,-0.013,0.71,0.0075,0.0024,-0.035,0,0,-4.9e+02,-0.001,-0.0058,-8.8e-05,-0.0029,0.004,-0.12,0.21,-2.8e-08,0.43,-0.00011,0.00063,-5.7e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.031,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0093,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15190000,0.71,0.00016,-0.013,0.7,0.0072,0.003,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-9e-05,-0.0035,0.0043,-0.12,0.21,-2.3e-08,0.43,-0.00012,0.00059,-6.4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.0091,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15290000,0.71,0.00019,-0.013,0.7,0.0077,0.0041,-0.03,0,0,-4.9e+02,-0.001,-0.0058,-8.8e-05,-0.0026,0.0041,-0.12,0.21,2.1e-08,0.43,-0.00014,0.00059,-4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15390000,0.71,0.0002,-0.013,0.7,0.007,0.0053,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.3e-05,-0.0018,0.0027,-0.12,0.21,3.7e-07,0.43,-0.00012,0.00062,-1.7e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15490000,0.71,0.00021,-0.013,0.7,0.0085,0.0045,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.0022,0.004,-0.12,0.21,-8e-08,0.43,-0.00014,0.00059,-3.5e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15590000,0.71,0.00018,-0.013,0.71,0.0071,0.0035,-0.027,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.0028,0.0047,-0.12,0.21,-4.3e-07,0.43,-0.00014,0.00058,-6.1e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0036,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.0023,0.0051,-0.12,0.21,-7.3e-07,0.43,-0.00015,0.00058,-6.4e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15790000,0.71,0.00019,-0.013,0.7,0.008,0.002,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.0026,0.0053,-0.12,0.21,-8.4e-07,0.43,-0.00016,0.00057,-6.8e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0082,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15890000,0.71,0.0002,-0.013,0.71,0.0087,0.0021,-0.028,0,0,-4.9e+02,-0.0011,-0.0059,-8.8e-05,-0.0018,0.0056,-0.12,0.21,-1e-06,0.43,-0.00018,0.00057,-6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.1e-06,3.8e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15990000,0.71,0.00017,-0.013,0.71,0.0085,0.0022,-0.024,0,0,-4.9e+02,-0.0011,-0.0059,-8.4e-05,-0.0012,0.0049,-0.12,0.21,-9.4e-07,0.43,-0.00018,0.00058,-4.2e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +16090000,0.71,0.00021,-0.013,0.71,0.01,0.0038,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-8e-05,-0.00044,0.0036,-0.12,0.21,-6.7e-07,0.43,-0.00016,0.00063,-2.6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.049,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16190000,0.71,0.00026,-0.013,0.71,0.01,0.004,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.9e-05,0.0003,0.0038,-0.12,0.21,-9.9e-07,0.43,-0.00017,0.00063,-2.1e-05,0,0,-4.9e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16290000,0.71,0.00028,-0.014,0.71,0.012,0.0051,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.00062,0.0025,-0.12,0.21,-5.3e-07,0.43,-0.00015,0.00064,-5.9e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.048,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16390000,0.71,0.00033,-0.013,0.71,0.01,0.0036,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.6e-05,0.0017,0.0037,-0.12,0.21,-1.3e-06,0.43,-0.00018,0.00064,-2e-06,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.023,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16490000,0.71,0.00042,-0.013,0.71,0.0087,0.0049,-0.022,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.0026,0.0039,-0.12,0.21,-1.5e-06,0.43,-0.00019,0.00064,1.2e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16590000,0.71,0.00052,-0.013,0.71,0.0066,0.0058,-0.023,0,0,-4.9e+02,-0.0012,-0.0058,-7.6e-05,0.0027,0.0036,-0.12,0.21,-1.6e-06,0.43,-0.00017,0.00063,7.8e-06,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0096,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16690000,0.71,0.00049,-0.013,0.71,0.0075,0.0062,-0.019,0,0,-4.9e+02,-0.0012,-0.0058,-7.9e-05,0.0022,0.0042,-0.12,0.21,-1.7e-06,0.43,-0.00018,0.00062,-3.3e-06,0,0,-4.9e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16790000,0.71,0.00048,-0.013,0.71,0.0056,0.0068,-0.018,0,0,-4.9e+02,-0.0012,-0.0058,-8e-05,0.002,0.0039,-0.12,0.21,-1.7e-06,0.43,-0.00016,0.00063,-1.6e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0093,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16890000,0.71,0.00054,-0.013,0.71,0.0053,0.0078,-0.016,0,0,-4.9e+02,-0.0012,-0.0058,-8.2e-05,0.0023,0.0045,-0.13,0.21,-2e-06,0.43,-0.00017,0.00062,-1.2e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +16990000,0.71,0.00051,-0.013,0.71,0.0054,0.0055,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.2e-05,0.0022,0.0056,-0.13,0.21,-2.4e-06,0.43,-0.00019,0.00061,-2.2e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17090000,0.71,0.00055,-0.013,0.71,0.0057,0.007,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.0032,0.0059,-0.13,0.21,-2.7e-06,0.43,-0.0002,0.00061,-1e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17190000,0.71,0.00061,-0.013,0.71,0.0058,0.0079,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-7.7e-05,0.0039,0.0058,-0.13,0.21,-3e-06,0.43,-0.0002,0.00062,-7.8e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0068,0.0088,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17290000,0.71,0.00063,-0.013,0.71,0.0076,0.0087,-0.011,0,0,-4.9e+02,-0.0012,-0.0059,-7.9e-05,0.0043,0.0068,-0.13,0.21,-3.4e-06,0.43,-0.00022,0.00061,-5.2e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17390000,0.71,0.00068,-0.013,0.71,0.0074,0.0091,-0.0095,0,0,-4.9e+02,-0.0012,-0.0059,-7.3e-05,0.0051,0.0065,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.5e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17490000,0.71,0.00063,-0.013,0.71,0.009,0.0093,-0.0078,0,0,-4.9e+02,-0.0012,-0.0059,-7.4e-05,0.0045,0.0063,-0.13,0.21,-3.3e-06,0.43,-0.00023,0.00061,7.8e-06,0,0,-4.9e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17590000,0.71,0.0006,-0.013,0.71,0.0098,0.0084,-0.0024,0,0,-4.9e+02,-0.0012,-0.0059,-7e-05,0.0048,0.0065,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00061,5.1e-06,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0065,0.0083,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17690000,0.71,0.00057,-0.013,0.71,0.011,0.01,-0.003,0,0,-4.9e+02,-0.0012,-0.0059,-6.9e-05,0.0049,0.0063,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.045,0.046,0.048,1.3e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17790000,0.71,0.00056,-0.013,0.71,0.012,0.011,-0.0042,0,0,-4.9e+02,-0.0012,-0.0058,-6e-05,0.0059,0.0052,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00064,2.3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.0081,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17890000,0.71,0.00055,-0.013,0.71,0.015,0.012,-0.0041,0,0,-4.9e+02,-0.0012,-0.0058,-5.8e-05,0.0057,0.0046,-0.13,0.21,-3.2e-06,0.43,-0.00023,0.00064,2.8e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17990000,0.71,0.00049,-0.013,0.71,0.016,0.009,-0.0027,0,0,-4.9e+02,-0.0012,-0.0058,-5.7e-05,0.0054,0.0049,-0.13,0.21,-3.3e-06,0.43,-0.00024,0.00064,2.5e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0079,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +18090000,0.71,0.00047,-0.013,0.71,0.017,0.0082,-0.00045,0,0,-4.9e+02,-0.0012,-0.0059,-6.2e-05,0.0049,0.0058,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00061,1.9e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18190000,0.71,0.00043,-0.013,0.71,0.017,0.0092,0.001,0,0,-4.9e+02,-0.0012,-0.0059,-5.8e-05,0.0051,0.0055,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00062,2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.041,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0077,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18290000,0.71,0.00035,-0.013,0.71,0.018,0.0088,0.0022,0,0,-4.9e+02,-0.0012,-0.0059,-6e-05,0.0047,0.0058,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00061,1.4e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18390000,0.71,0.00031,-0.013,0.71,0.02,0.011,0.0035,0,0,-4.9e+02,-0.0012,-0.0059,-5.4e-05,0.0044,0.005,-0.13,0.21,-3.3e-06,0.43,-0.00024,0.00062,1.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.006,0.0075,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18490000,0.71,0.00038,-0.013,0.71,0.021,0.012,0.0031,0,0,-4.9e+02,-0.0012,-0.0059,-5.3e-05,0.005,0.0051,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00063,2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18590000,0.71,0.00039,-0.013,0.71,0.02,0.013,0.0014,0,0,-4.9e+02,-0.0012,-0.0058,-4.5e-05,0.0058,0.0046,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00065,2.5e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18690000,0.71,0.00031,-0.013,0.71,0.022,0.012,-0.00039,0,0,-4.9e+02,-0.0012,-0.0059,-4.7e-05,0.0051,0.0046,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00064,1.9e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18790000,0.71,0.00032,-0.013,0.71,0.021,0.012,-0.00061,0,0,-4.9e+02,-0.0012,-0.0059,-4.6e-05,0.0053,0.0052,-0.13,0.21,-3.8e-06,0.43,-0.00025,0.00063,1.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00067,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18890000,0.71,0.00041,-0.013,0.71,0.021,0.014,1.9e-05,0,0,-4.9e+02,-0.0012,-0.0058,-4e-05,0.0062,0.0046,-0.13,0.21,-3.8e-06,0.43,-0.00026,0.00065,2.9e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +18990000,0.71,0.00046,-0.013,0.71,0.021,0.015,-0.0012,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.0069,0.0048,-0.13,0.21,-4.1e-06,0.43,-0.00027,0.00066,3e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.015,0.017,0.012,0.039,0.04,0.044,9.7e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.00062,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19090000,0.71,0.00051,-0.013,0.71,0.021,0.016,0.0018,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.0075,0.0051,-0.13,0.21,-4.4e-06,0.43,-0.00028,0.00066,3.4e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.016,0.019,0.012,0.042,0.044,0.044,9.6e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.0006,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19190000,0.71,0.00055,-0.013,0.71,0.02,0.016,0.0019,0,0,-4.9e+02,-0.0013,-0.0059,-2.7e-05,0.008,0.0053,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00067,3.8e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.012,0.038,0.04,0.043,9.3e-07,1.5e-06,2.1e-06,0.0055,0.0068,0.00058,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19290000,0.71,0.00058,-0.013,0.71,0.02,0.015,0.0046,0,0,-4.9e+02,-0.0013,-0.0059,-3.1e-05,0.0078,0.0057,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00066,4e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.019,0.012,0.042,0.044,0.043,9.2e-07,1.5e-06,2.1e-06,0.0055,0.0067,0.00056,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19390000,0.71,0.00054,-0.013,0.71,0.019,0.014,0.0084,0,0,-4.9e+02,-0.0013,-0.0059,-2.4e-05,0.0078,0.0055,-0.13,0.21,-4.7e-06,0.43,-0.00029,0.00065,3.6e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.011,0.038,0.04,0.043,8.9e-07,1.5e-06,2.1e-06,0.0055,0.0066,0.00054,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19490000,0.71,0.00055,-0.013,0.71,0.019,0.015,0.0049,0,0,-4.9e+02,-0.0013,-0.0058,-1.9e-05,0.0078,0.0048,-0.13,0.21,-4.5e-06,0.43,-0.00029,0.00066,3.9e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.018,0.011,0.042,0.044,0.043,8.8e-07,1.4e-06,2.1e-06,0.0054,0.0066,0.00052,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19590000,0.71,0.00063,-0.013,0.71,0.017,0.015,0.0043,0,0,-4.9e+02,-0.0013,-0.0058,-7.4e-06,0.0086,0.0046,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00068,4.7e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.014,0.017,0.011,0.038,0.039,0.042,8.5e-07,1.4e-06,2.1e-06,0.0054,0.0065,0.0005,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19690000,0.71,0.00067,-0.013,0.71,0.017,0.013,0.0059,0,0,-4.9e+02,-0.0013,-0.0058,-1.1e-05,0.0089,0.0052,-0.13,0.21,-4.9e-06,0.43,-0.0003,0.00068,3.9e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.015,0.018,0.011,0.042,0.043,0.042,8.4e-07,1.4e-06,2.1e-06,0.0053,0.0065,0.00049,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19790000,0.71,0.00074,-0.013,0.71,0.014,0.011,0.0064,0,0,-4.9e+02,-0.0013,-0.0059,-6.3e-06,0.0094,0.0055,-0.13,0.21,-5.2e-06,0.43,-0.00031,0.00068,3.8e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.042,8.2e-07,1.3e-06,2.1e-06,0.0053,0.0064,0.00047,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19890000,0.71,0.00066,-0.013,0.71,0.015,0.013,0.0076,0,0,-4.9e+02,-0.0013,-0.0058,1.8e-06,0.0092,0.0045,-0.13,0.21,-4.9e-06,0.43,-0.0003,0.00069,4.5e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.015,0.018,0.01,0.041,0.043,0.042,8.1e-07,1.3e-06,2.1e-06,0.0053,0.0063,0.00046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19990000,0.71,0.00063,-0.013,0.7,0.013,0.013,0.01,0,0,-4.9e+02,-0.0013,-0.0058,1.7e-05,0.0095,0.0037,-0.13,0.21,-4.8e-06,0.43,-0.0003,0.0007,5.1e-05,0,0,-4.9e+02,0.0001,9.4e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.041,7.9e-07,1.3e-06,2.1e-06,0.0052,0.0062,0.00044,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +20090000,0.71,0.00066,-0.013,0.7,0.013,0.013,0.011,0,0,-4.9e+02,-0.0013,-0.0058,2.7e-05,0.01,0.0029,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00072,6.2e-05,0,0,-4.9e+02,0.00011,9.5e-05,0.036,0.014,0.018,0.01,0.041,0.043,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20190000,0.71,0.00069,-0.013,0.7,0.012,0.012,0.013,0,0,-4.9e+02,-0.0013,-0.0058,3.6e-05,0.0099,0.0026,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00072,6.1e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.013,0.016,0.0098,0.038,0.039,0.041,7.6e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00042,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20290000,0.71,0.0007,-0.013,0.7,0.01,0.012,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4e-05,0.01,0.0025,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00073,6.3e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.014,0.018,0.0097,0.041,0.043,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.0004,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20390000,0.71,0.00065,-0.013,0.7,0.0085,0.0097,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.4e-05,0.01,0.0026,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00073,5.3e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.0051,0.006,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20490000,0.71,0.00071,-0.013,0.7,0.0086,0.0096,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.01,0.0029,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00072,5.1e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.006,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20590000,0.71,0.00074,-0.013,0.7,0.0077,0.0076,0.0099,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.01,0.0035,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00072,5.1e-05,0,0,-4.9e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0059,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20690000,0.71,0.00077,-0.013,0.7,0.0084,0.0076,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.01,0.0032,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00072,5.2e-05,0,0,-4.9e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.005,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20790000,0.71,0.0008,-0.013,0.7,0.0062,0.0071,0.012,0,0,-4.9e+02,-0.0013,-0.0058,5e-05,0.011,0.0034,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00073,4.7e-05,0,0,-4.9e+02,9.8e-05,8.8e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0058,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20890000,0.71,0.00081,-0.013,0.7,0.0061,0.0068,0.011,0,0,-4.9e+02,-0.0013,-0.0058,5.8e-05,0.011,0.003,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00074,5.2e-05,0,0,-4.9e+02,9.9e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +20990000,0.71,0.00083,-0.013,0.7,0.0044,0.0045,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.2e-05,0.011,0.0032,-0.13,0.21,-4.9e-06,0.43,-0.00033,0.00074,4.9e-05,0,0,-4.9e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0049,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21090000,0.71,0.00081,-0.013,0.7,0.0053,0.0037,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.011,0.0027,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00075,4.6e-05,0,0,-4.9e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21190000,0.71,0.00081,-0.013,0.7,0.0056,0.0028,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.011,0.0029,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00075,4.3e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.3e-07,9.5e-07,2e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21290000,0.71,0.0009,-0.013,0.7,0.0049,0.0029,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.8e-05,0.011,0.0025,-0.13,0.21,-4.7e-06,0.43,-0.00033,0.00077,4.7e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.5e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21390000,0.71,0.00088,-0.013,0.7,0.0039,0.00086,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.011,0.0026,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00076,4.8e-05,0,0,-4.9e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0055,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21490000,0.71,0.00088,-0.013,0.7,0.0044,0.0013,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.011,0.0023,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00076,5.3e-05,0,0,-4.9e+02,9.4e-05,8.4e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21590000,0.71,0.00087,-0.013,0.7,0.0033,0.0018,0.012,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.011,0.0023,-0.13,0.21,-5e-06,0.43,-0.00032,0.00076,5e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0054,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21690000,0.71,0.00084,-0.013,0.7,0.0049,0.0021,0.014,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.011,0.0019,-0.13,0.21,-4.9e-06,0.43,-0.0003,0.00076,5.1e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21790000,0.71,0.00083,-0.013,0.7,0.003,0.0042,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.011,0.0021,-0.13,0.21,-5.5e-06,0.43,-0.00032,0.00076,5.3e-05,0,0,-4.9e+02,9e-05,8.1e-05,0.036,0.012,0.015,0.0079,0.037,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0048,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.011,0.002,-0.13,0.21,-5.4e-06,0.43,-0.00032,0.00076,5.1e-05,0,0,-4.9e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21990000,0.71,0.00085,-0.013,0.7,0.0025,0.0054,0.014,0,0,-4.9e+02,-0.0013,-0.0058,6.9e-05,0.012,0.0019,-0.13,0.21,-5.8e-06,0.43,-0.00033,0.00076,5.2e-05,0,0,-4.9e+02,8.9e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.5e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22090000,0.71,0.00087,-0.013,0.7,0.0024,0.007,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.9e-05,0.012,0.0019,-0.13,0.21,-5.8e-06,0.43,-0.00033,0.00076,5.2e-05,0,0,-4.9e+02,8.9e-05,8e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22190000,0.71,0.00084,-0.013,0.7,0.0019,0.007,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.5e-05,0.012,0.002,-0.13,0.21,-5.6e-06,0.43,-0.00034,0.00077,4.2e-05,0,0,-4.9e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22290000,0.71,0.00087,-0.013,0.7,0.0013,0.0067,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.012,0.002,-0.13,0.21,-5.6e-06,0.43,-0.00034,0.00076,4.3e-05,0,0,-4.9e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22390000,0.71,0.00089,-0.013,0.7,-0.0011,0.0064,0.015,0,0,-4.9e+02,-0.0013,-0.0058,7.9e-05,0.012,0.0023,-0.13,0.21,-5.6e-06,0.43,-0.00034,0.00077,4.4e-05,0,0,-4.9e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22490000,0.71,0.00093,-0.013,0.7,-0.0022,0.0073,0.016,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.012,0.0024,-0.13,0.21,-5.5e-06,0.43,-0.00036,0.00078,4.1e-05,0,0,-4.9e+02,8.7e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22590000,0.71,0.00095,-0.013,0.7,-0.0038,0.0066,0.015,0,0,-4.9e+02,-0.0014,-0.0058,8.4e-05,0.013,0.0026,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00079,3.8e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22690000,0.71,0.001,-0.013,0.7,-0.0052,0.008,0.016,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.013,0.0025,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.0008,3.8e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22790000,0.71,0.001,-0.013,0.7,-0.0072,0.0069,0.017,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.013,0.0033,-0.13,0.21,-5.6e-06,0.43,-0.00038,0.00078,4.3e-05,0,0,-4.9e+02,8.3e-05,7.5e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.8e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22890000,0.71,0.00099,-0.013,0.7,-0.0076,0.0078,0.019,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.013,0.0031,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00077,3.9e-05,0,0,-4.9e+02,8.4e-05,7.5e-05,0.036,0.013,0.016,0.0071,0.039,0.042,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22990000,0.71,0.00097,-0.013,0.7,-0.0077,0.0069,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.8e-05,0.013,0.003,-0.13,0.21,-5.2e-06,0.43,-0.00037,0.00078,3.6e-05,0,0,-4.9e+02,8.2e-05,7.4e-05,0.036,0.012,0.015,0.007,0.036,0.038,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23090000,0.71,0.00092,-0.013,0.7,-0.0081,0.0068,0.02,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.012,0.0033,-0.13,0.21,-5.4e-06,0.43,-0.00037,0.00076,3.4e-05,0,0,-4.9e+02,8.3e-05,7.4e-05,0.036,0.013,0.016,0.007,0.039,0.042,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23190000,0.71,0.00097,-0.013,0.7,-0.0094,0.0048,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.2e-05,0.013,0.0035,-0.13,0.21,-5.4e-06,0.43,-0.00036,0.00076,2.6e-05,0,0,-4.9e+02,8.1e-05,7.3e-05,0.036,0.012,0.014,0.0069,0.036,0.038,0.035,4.6e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23290000,0.71,0.00089,-0.013,0.7,-0.0093,0.0042,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.4e-05,0.012,0.0033,-0.13,0.21,-5.3e-06,0.43,-0.00036,0.00076,2.5e-05,0,0,-4.9e+02,8.2e-05,7.3e-05,0.036,0.013,0.016,0.0069,0.039,0.042,0.036,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23390000,0.71,0.00095,-0.013,0.7,-0.0095,0.003,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.7e-05,0.012,0.0033,-0.13,0.21,-5.4e-06,0.43,-0.00034,0.00075,2.6e-05,0,0,-4.9e+02,8e-05,7.2e-05,0.036,0.012,0.014,0.0068,0.036,0.038,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23490000,0.71,0.0033,-0.01,0.7,-0.016,0.0033,-0.013,0,0,-4.9e+02,-0.0014,-0.0058,9.3e-05,0.012,0.0031,-0.13,0.21,-5.4e-06,0.43,-0.00033,0.00078,5.1e-05,0,0,-4.9e+02,8.1e-05,7.2e-05,0.036,0.013,0.015,0.0068,0.039,0.042,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23590000,0.71,0.0086,-0.0026,0.7,-0.027,0.0032,-0.045,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.012,0.0031,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00084,0.00012,0,0,-4.9e+02,7.9e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23690000,0.71,0.0082,0.0032,0.71,-0.058,-0.0046,-0.095,0,0,-4.9e+02,-0.0014,-0.0058,9e-05,0.012,0.0031,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00078,9e-05,0,0,-4.9e+02,8e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0047,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23790000,0.71,0.0052,-0.00012,0.71,-0.083,-0.016,-0.15,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.011,0.0026,-0.13,0.21,-4.5e-06,0.43,-0.00038,0.00078,0.00044,0,0,-4.9e+02,7.8e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23890000,0.71,0.0026,-0.0062,0.71,-0.1,-0.025,-0.2,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.012,0.0028,-0.13,0.21,-4.4e-06,0.43,-0.00041,0.00083,0.00035,0,0,-4.9e+02,7.9e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23990000,0.71,0.0012,-0.011,0.71,-0.1,-0.029,-0.26,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.012,0.0028,-0.13,0.21,-4e-06,0.43,-0.00038,0.00083,0.00032,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24090000,0.71,0.0025,-0.0095,0.71,-0.1,-0.028,-0.3,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.011,0.0025,-0.13,0.21,-3.6e-06,0.43,-0.00041,0.0008,0.00036,0,0,-4.9e+02,7.8e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24190000,0.71,0.0036,-0.0072,0.71,-0.11,-0.03,-0.35,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.011,0.0024,-0.13,0.21,-2.8e-06,0.43,-0.00041,0.00083,0.00036,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24290000,0.71,0.0041,-0.0064,0.71,-0.12,-0.033,-0.41,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.011,0.0023,-0.13,0.21,-2.5e-06,0.43,-0.00045,0.00088,0.00042,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24390000,0.71,0.0042,-0.0065,0.71,-0.13,-0.041,-0.46,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.011,0.003,-0.13,0.21,2.9e-07,0.43,-0.00033,0.00092,0.00042,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24490000,0.71,0.005,-0.0024,0.71,-0.14,-0.046,-0.51,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.011,0.003,-0.13,0.21,2.9e-07,0.43,-0.00033,0.00093,0.00041,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24590000,0.71,0.0055,0.0013,0.71,-0.16,-0.057,-0.56,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.003,-0.13,0.21,1.4e-06,0.43,2.7e-05,0.00058,0.00036,0,0,-4.9e+02,7.4e-05,6.7e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24690000,0.71,0.0056,0.0022,0.71,-0.18,-0.07,-0.64,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.0028,-0.13,0.21,2.4e-06,0.43,-1.8e-05,0.00062,0.00054,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24790000,0.71,0.0053,0.00097,0.71,-0.2,-0.083,-0.73,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.0029,-0.13,0.21,1.8e-06,0.43,1.5e-05,0.00059,0.0003,0,0,-4.9e+02,7.3e-05,6.6e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24890000,0.71,0.0071,0.0027,0.71,-0.22,-0.094,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.0029,-0.13,0.21,2.6e-06,0.43,-9.1e-05,0.00075,0.00032,0,0,-4.9e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24990000,0.71,0.0089,0.0044,0.71,-0.24,-0.1,-0.81,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.0099,0.0026,-0.13,0.21,2.1e-06,0.43,-0.00017,0.00085,-2.2e-05,0,0,-4.9e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25090000,0.71,0.0092,0.0038,0.71,-0.27,-0.11,-0.86,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.0098,0.0027,-0.13,0.21,1.7e-06,0.43,-0.00019,0.00086,-5.8e-05,0,0,-4.9e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25190000,0.71,0.0087,0.0024,0.71,-0.3,-0.13,-0.91,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.0092,0.003,-0.13,0.21,7.1e-06,0.43,7e-05,0.00083,7.7e-05,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25290000,0.71,0.011,0.0092,0.71,-0.33,-0.14,-0.96,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.0092,0.003,-0.13,0.21,6.9e-06,0.43,9.5e-05,0.00077,8.4e-05,0,0,-4.9e+02,7.2e-05,6.5e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.0085,0.0027,-0.13,0.21,1.1e-05,0.43,0.0005,0.00046,0.00012,0,0,-4.9e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,0.00014,0.0084,0.0026,-0.13,0.21,9.7e-06,0.43,0.00067,0.00013,0.00031,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0077,0.003,-0.13,0.21,1.6e-05,0.43,0.00097,0.00014,0.00032,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25690000,0.71,0.015,0.022,0.71,-0.49,-0.23,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0077,0.003,-0.13,0.21,1.6e-05,0.43,0.00096,0.00016,0.0004,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 +25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.007,0.0023,-0.13,0.21,2e-05,0.43,0.0013,-8.6e-05,-5.5e-05,0,0,-4.9e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25890000,0.71,0.018,0.029,0.71,-0.62,-0.29,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00017,0.0072,0.0022,-0.13,0.21,2.2e-05,0.43,0.0014,-1.8e-05,-0.00012,0,0,-4.9e+02,7.1e-05,6.3e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25990000,0.7,0.017,0.025,0.71,-0.67,-0.32,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00019,0.0063,0.0026,-0.13,0.21,2.9e-05,0.43,0.0023,-0.00058,-0.00058,0,0,-4.9e+02,7e-05,6.2e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 +26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0064,0.0027,-0.13,0.21,2.5e-05,0.43,0.0024,-0.0005,-0.0012,0,0,-4.9e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 +26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0053,0.0016,-0.13,0.21,3.9e-05,0.43,0.0023,0.0004,-0.0014,0,0,-4.9e+02,7.1e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 +26290000,0.7,0.025,0.047,0.71,-0.89,-0.43,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0053,0.0016,-0.13,0.21,3.8e-05,0.43,0.0023,0.00027,-0.0014,0,0,-4.9e+02,7.1e-05,6.2e-05,0.03,0.015,0.028,0.0059,0.039,0.043,0.033,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.00099,3.9e-05,0.00099,0.0013,0.001,0.00099,1,1,0.01 +26390000,0.7,0.024,0.044,0.71,-0.96,-0.49,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00021,0.0044,0.0024,-0.13,0.21,4.6e-05,0.44,0.0036,-0.00018,-0.0024,0,0,-4.9e+02,7.1e-05,6.1e-05,0.028,0.014,0.027,0.0058,0.036,0.039,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00096,3.9e-05,0.00095,0.0012,0.00096,0.00095,1,1,0.01 +26490000,0.7,0.031,0.06,0.71,-1.1,-0.53,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00021,0.0044,0.0024,-0.13,0.21,3.9e-05,0.44,0.004,-0.00099,-0.0027,0,0,-4.9e+02,7.2e-05,6.1e-05,0.028,0.016,0.031,0.0058,0.039,0.044,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00092,3.9e-05,0.00092,0.0012,0.00092,0.00091,1,1,0.01 +26590000,0.7,0.037,0.076,0.71,-1.2,-0.59,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.0028,0.0014,-0.13,0.21,3.9e-05,0.44,0.0042,-0.00064,-0.0048,0,0,-4.9e+02,7.2e-05,6e-05,0.025,0.015,0.031,0.0058,0.036,0.04,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0041,0.00013,0.00087,3.9e-05,0.00086,0.001,0.00087,0.00086,1,1,0.01 +26690000,0.7,0.039,0.079,0.71,-1.3,-0.65,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.0028,0.0013,-0.13,0.21,4.5e-05,0.44,0.004,-0.00013,-0.004,0,0,-4.9e+02,7.2e-05,6.1e-05,0.025,0.017,0.037,0.0058,0.04,0.045,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00081,3.9e-05,0.0008,0.001,0.00081,0.00079,1,1,0.01 +26790000,0.7,0.036,0.074,0.71,-1.4,-0.74,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00022,0.0012,0.0022,-0.13,0.21,8.4e-05,0.44,0.0055,0.00063,-0.0038,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.016,0.036,0.0057,0.036,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00076,3.9e-05,0.00075,0.00092,0.00076,0.00074,1,1,0.01 +26890000,0.7,0.045,0.096,0.71,-1.6,-0.8,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00023,0.0013,0.0021,-0.13,0.21,9e-05,0.44,0.0054,0.0013,-0.0041,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.018,0.043,0.0058,0.04,0.046,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00072,3.9e-05,0.0007,0.00092,0.00072,0.0007,1,1,0.01 +26990000,0.7,0.051,0.12,0.71,-1.7,-0.89,-1.3,0,0,-4.9e+02,-0.00099,-0.0059,0.00022,-0.00069,0.00024,-0.13,0.21,0.00013,0.44,0.0061,0.0034,-0.0056,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.017,0.042,0.0057,0.037,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00065,3.9e-05,0.00063,0.00079,0.00065,0.00062,1,1,0.01 +27090000,0.7,0.052,0.12,0.71,-1.9,-0.98,-1.3,0,0,-4.9e+02,-0.00098,-0.0059,0.00022,-0.00073,0.00025,-0.13,0.21,0.00013,0.44,0.0061,0.0035,-0.0052,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.02,0.052,0.0057,0.04,0.048,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00059,3.9e-05,0.00056,0.00078,0.0006,0.00056,1,1,0.01 +27190000,0.7,0.05,0.11,0.7,-2.1,-1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00022,-0.0018,6.1e-05,-0.13,0.21,4.8e-05,0.44,0.0021,0.0027,-0.0049,0,0,-4.9e+02,7.5e-05,6e-05,0.016,0.02,0.051,0.0057,0.043,0.05,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00055,3.9e-05,0.00051,0.00066,0.00055,0.00051,1,1,0.01 +27290000,0.71,0.044,0.095,0.7,-2.3,-1.1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00023,-0.0018,4.3e-05,-0.13,0.21,5.6e-05,0.44,0.002,0.0034,-0.0049,0,0,-4.9e+02,7.6e-05,6.1e-05,0.016,0.022,0.059,0.0057,0.047,0.057,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00052,3.9e-05,0.00048,0.00066,0.00052,0.00048,1,1,0.01 +27390000,0.71,0.038,0.079,0.7,-2.4,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0058,0.00022,-0.003,-0.0017,-0.13,0.21,1.3e-05,0.44,-0.00049,0.0032,-0.0064,0,0,-4.9e+02,7.6e-05,6e-05,0.013,0.021,0.052,0.0057,0.049,0.059,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00049,3.9e-05,0.00046,0.00055,0.00049,0.00046,1,1,0.01 +27490000,0.71,0.032,0.064,0.7,-2.5,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0058,0.00022,-0.003,-0.0016,-0.13,0.21,1.9e-05,0.44,-0.00057,0.0032,-0.0067,0,0,-4.9e+02,7.7e-05,6.1e-05,0.013,0.023,0.056,0.0057,0.054,0.067,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00048,3.9e-05,0.00045,0.00055,0.00048,0.00044,1,1,0.01 +27590000,0.72,0.028,0.051,0.69,-2.6,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00023,-0.0037,-0.001,-0.12,0.21,-3.7e-05,0.44,-0.0032,0.0027,-0.0066,0,0,-4.9e+02,7.7e-05,6.1e-05,0.011,0.021,0.047,0.0057,0.056,0.068,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00044,1,1,0.01 +27690000,0.72,0.027,0.05,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00023,-0.0036,-0.00089,-0.12,0.21,-3.3e-05,0.44,-0.0033,0.0026,-0.0068,0,0,-4.9e+02,7.8e-05,6.1e-05,0.011,0.022,0.049,0.0057,0.062,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00043,1,1,0.01 +27790000,0.72,0.027,0.051,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00022,-0.0042,-0.0007,-0.12,0.21,-6e-05,0.44,-0.005,0.0021,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.02,0.042,0.0056,0.064,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00045,3.9e-05,0.00043,0.00041,0.00045,0.00043,1,1,0.01 +27890000,0.72,0.027,0.049,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00023,-0.0042,-0.00071,-0.12,0.21,-6.1e-05,0.44,-0.005,0.002,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.021,0.043,0.0057,0.07,0.087,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00044,3.9e-05,0.00043,0.00041,0.00044,0.00042,1,1,0.01 +27990000,0.72,0.026,0.046,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00024,-0.004,0.00022,-0.12,0.21,-7.7e-05,0.44,-0.0064,0.0015,-0.0071,0,0,-4.9e+02,8e-05,6.1e-05,0.0087,0.02,0.038,0.0056,0.072,0.087,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00043,3.9e-05,0.00042,0.00037,0.00043,0.00042,1,1,0.01 +28090000,0.72,0.032,0.059,0.69,-2.8,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00023,-0.0042,0.00043,-0.12,0.21,-7.7e-05,0.44,-0.0065,0.0014,-0.0072,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0086,0.021,0.039,0.0057,0.078,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00042,3.9e-05,0.00042,0.00036,0.00042,0.00041,1,1,0.01 +28190000,0.72,0.037,0.072,0.69,-2.8,-1.2,-0.94,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0042,0.00087,-0.12,0.21,-9.5e-05,0.44,-0.0073,0.00091,-0.0071,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0079,0.02,0.034,0.0056,0.08,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00041,3.9e-05,0.00041,0.00033,0.00041,0.00041,1,1,0.01 +28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0043,0.00098,-0.12,0.21,-9.8e-05,0.44,-0.0073,0.0013,-0.0069,0,0,-4.9e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,0,0,-4.9e+02,-0.00094,-0.0059,0.00023,-0.0042,0.0013,-0.12,0.21,-8.5e-05,0.44,-0.0075,0.0014,-0.007,0,0,-4.9e+02,8.3e-05,6.2e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28490000,0.73,0.0028,0.006,0.69,-2.8,-1.2,1.1,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0039,0.0015,-0.12,0.21,-6.7e-05,0.44,-0.0076,0.0014,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28590000,0.73,0.00088,0.0025,0.69,-2.7,-1.2,0.97,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0039,0.0016,-0.12,0.21,-6.9e-05,0.44,-0.0075,0.0015,-0.007,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28690000,0.73,0.00018,0.0016,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00095,-0.0059,0.00024,-0.0036,0.0019,-0.12,0.21,-5.2e-05,0.44,-0.0076,0.0014,-0.0068,0,0,-4.9e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 +28790000,0.73,-1.6e-05,0.0015,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00098,-0.0059,0.00024,-0.0031,0.0022,-0.12,0.21,-9.3e-05,0.44,-0.0091,0.00063,-0.006,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 +28890000,0.73,-6.2e-06,0.0018,0.69,-2.5,-1.2,0.97,0,0,-4.9e+02,-0.00099,-0.0059,0.00024,-0.0029,0.0025,-0.12,0.21,-7.6e-05,0.44,-0.0091,0.00059,-0.0059,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 +28990000,0.73,0.00034,0.0024,0.68,-2.5,-1.1,0.97,0,0,-4.9e+02,-0.001,-0.0059,0.00025,-0.0016,0.0029,-0.12,0.21,-0.00011,0.44,-0.011,-0.00033,-0.0047,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.0003,0.00039,0.00039,1,1,0.01 +29090000,0.73,0.00051,0.0028,0.68,-2.4,-1.1,0.96,0,0,-4.9e+02,-0.001,-0.0059,0.00025,-0.0014,0.0033,-0.12,0.21,-9.3e-05,0.44,-0.011,-0.00039,-0.0046,0,0,-4.9e+02,8.8e-05,6.2e-05,0.0073,0.021,0.025,0.0057,0.14,0.17,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29190000,0.73,0.00076,0.0032,0.68,-2.4,-1.1,0.95,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00098,0.0034,-0.12,0.21,-0.00013,0.44,-0.011,-0.00062,-0.0042,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0072,0.02,0.023,0.0056,0.14,0.17,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29290000,0.73,0.0011,0.004,0.68,-2.3,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00095,0.0038,-0.12,0.21,-0.00012,0.44,-0.011,-0.00069,-0.0041,0,0,-4.9e+02,8.9e-05,6.2e-05,0.0072,0.021,0.024,0.0056,0.14,0.18,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29390000,0.73,0.0017,0.0055,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00022,0.0043,-0.12,0.21,-0.00015,0.44,-0.012,-0.0014,-0.0033,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.18,0.031,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 +29490000,0.73,0.0022,0.0066,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00021,0.0045,-0.12,0.21,-0.00015,0.44,-0.012,-0.0013,-0.0033,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.15,0.19,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 +29590000,0.73,0.0027,0.0076,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.00041,0.0045,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.19,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00038,1,1,0.01 +29690000,0.73,0.003,0.0082,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00025,0.00035,0.0049,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-4.9e+02,9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29790000,0.73,0.0034,0.0087,0.68,-2.2,-1.1,0.96,0,0,-4.9e+02,-0.0012,-0.0059,0.00025,0.0014,0.0047,-0.12,0.21,-0.00018,0.44,-0.012,-0.0019,-0.0023,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29890000,0.73,0.0034,0.0088,0.68,-2.1,-1.1,0.95,0,0,-4.9e+02,-0.0012,-0.0059,0.00024,0.0011,0.0052,-0.12,0.21,-0.00018,0.44,-0.013,-0.0019,-0.0023,0,0,-4.9e+02,9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.17,0.21,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29990000,0.73,0.0036,0.0089,0.68,-2.1,-1.1,0.94,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0016,0.0049,-0.12,0.21,-0.0002,0.44,-0.013,-0.0021,-0.002,0,0,-4.9e+02,8.9e-05,6e-05,0.0071,0.02,0.024,0.0056,0.17,0.21,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +30090000,0.73,0.0035,0.0088,0.68,-2.1,-1.1,0.92,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0013,0.0052,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.18,0.22,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +30190000,0.73,0.0036,0.0085,0.68,-2.1,-1.1,0.91,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0021,0.0047,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.02,0.025,0.0056,0.18,0.22,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00038,0.00028,0.00037,0.00038,1,1,0.01 +30290000,0.73,0.0034,0.0083,0.68,-2,-1.1,0.9,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0019,0.0049,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.9e-05,6.1e-05,0.007,0.021,0.026,0.0056,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30390000,0.73,0.0035,0.008,0.68,-2,-1.1,0.89,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0027,0.0047,-0.12,0.21,-0.00022,0.43,-0.012,-0.0025,-0.0014,0,0,-4.9e+02,8.7e-05,6e-05,0.007,0.021,0.025,0.0055,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30490000,0.73,0.0034,0.0077,0.68,-2,-1.1,0.87,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0026,0.0049,-0.12,0.21,-0.00022,0.43,-0.012,-0.0024,-0.0014,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.022,0.027,0.0056,0.2,0.24,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.8e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30590000,0.73,0.0034,0.0073,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0034,0.0045,-0.12,0.21,-0.00024,0.43,-0.012,-0.0025,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.021,0.026,0.0055,0.2,0.24,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30690000,0.73,0.0031,0.0069,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0032,0.0048,-0.12,0.21,-0.00024,0.43,-0.012,-0.0024,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.022,0.028,0.0055,0.21,0.25,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30790000,0.73,0.0031,0.0065,0.68,-1.9,-1,0.82,0,0,-4.9e+02,-0.0012,-0.0059,0.00014,0.004,0.0042,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00079,0,0,-4.9e+02,8.4e-05,6e-05,0.0068,0.021,0.027,0.0055,0.21,0.25,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30890000,0.73,0.0029,0.006,0.68,-1.9,-1,0.81,0,0,-4.9e+02,-0.0013,-0.0059,0.00015,0.004,0.0046,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00077,0,0,-4.9e+02,8.5e-05,6e-05,0.0068,0.022,0.029,0.0055,0.22,0.26,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30990000,0.73,0.003,0.0054,0.68,-1.8,-1,0.8,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0049,0.004,-0.12,0.21,-0.00025,0.43,-0.011,-0.0027,-0.00043,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.021,0.028,0.0055,0.22,0.26,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.5e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31090000,0.73,0.0027,0.0049,0.68,-1.8,-1,0.79,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0046,0.0044,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00043,0,0,-4.9e+02,8.4e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31190000,0.73,0.0026,0.0044,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.0049,0.0044,-0.12,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00026,0,0,-4.9e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31290000,0.73,0.0023,0.0039,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.9e-05,0.0047,0.0048,-0.11,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00029,0,0,-4.9e+02,8.2e-05,6e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00035,0.00037,1,1,0.01 +31390000,0.73,0.0022,0.0032,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.0051,0.0045,-0.11,0.21,-0.0003,0.43,-0.011,-0.0028,-3.3e-05,0,0,-4.9e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31490000,0.73,0.0019,0.0026,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.005,0.0051,-0.11,0.21,-0.0003,0.43,-0.011,-0.0029,3.9e-06,0,0,-4.9e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31590000,0.73,0.002,0.0021,0.68,-1.7,-0.97,0.77,0,0,-4.9e+02,-0.0013,-0.0058,4.6e-05,0.0059,0.0047,-0.11,0.21,-0.00029,0.43,-0.01,-0.0029,0.00024,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 +31690000,0.73,0.0016,0.0013,0.68,-1.6,-0.97,0.78,0,0,-4.9e+02,-0.0013,-0.0058,4.8e-05,0.0056,0.0051,-0.11,0.21,-0.0003,0.43,-0.01,-0.0029,0.00021,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 +31790000,0.73,0.0015,0.00056,0.69,-1.6,-0.95,0.78,0,0,-4.9e+02,-0.0013,-0.0058,2.3e-05,0.0067,0.0049,-0.11,0.21,-0.0003,0.43,-0.0097,-0.0029,0.00055,0,0,-4.9e+02,7.6e-05,5.9e-05,0.0062,0.021,0.029,0.0054,0.26,0.3,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 +31890000,0.73,0.0013,-0.00017,0.69,-1.6,-0.95,0.77,0,0,-4.9e+02,-0.0013,-0.0058,2.3e-05,0.0066,0.0054,-0.11,0.21,-0.00029,0.43,-0.0097,-0.0029,0.00058,0,0,-4.9e+02,7.7e-05,5.9e-05,0.0062,0.022,0.031,0.0054,0.27,0.31,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 +31990000,0.73,0.0012,-0.00078,0.69,-1.6,-0.93,0.77,0,0,-4.9e+02,-0.0013,-0.0058,-8e-06,0.0071,0.0053,-0.11,0.21,-0.00029,0.43,-0.0093,-0.003,0.00075,0,0,-4.9e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32090000,0.73,0.00083,-0.0015,0.69,-1.5,-0.93,0.78,0,0,-4.9e+02,-0.0013,-0.0058,-8.9e-06,0.0069,0.0058,-0.11,0.21,-0.00029,0.43,-0.0093,-0.003,0.00076,0,0,-4.9e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32190000,0.73,0.00062,-0.0025,0.69,-1.5,-0.91,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-4.3e-05,0.0074,0.0058,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.6e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32290000,0.73,0.00035,-0.0032,0.69,-1.5,-0.91,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-4.3e-05,0.0072,0.0064,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32390000,0.73,0.00025,-0.004,0.69,-1.5,-0.89,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-6.1e-05,0.0077,0.0063,-0.11,0.2,-0.00029,0.43,-0.0084,-0.0031,0.0011,0,0,-4.9e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32490000,0.72,0.0001,-0.0042,0.69,-1.4,-0.88,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-5.9e-05,0.0075,0.0067,-0.11,0.2,-0.0003,0.43,-0.0084,-0.0031,0.0011,0,0,-4.9e+02,7.2e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32590000,0.72,0.00015,-0.0046,0.69,-1.4,-0.87,0.78,0,0,-4.9e+02,-0.0014,-0.0057,-8e-05,0.0079,0.0067,-0.11,0.21,-0.0003,0.43,-0.008,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32690000,0.72,0.00011,-0.0046,0.69,-1.4,-0.86,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.0079,0.0071,-0.11,0.2,-0.0003,0.43,-0.008,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.022,0.032,0.0053,0.31,0.35,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00036,1,1,0.01 +32790000,0.72,0.00023,-0.0047,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.0001,0.0082,0.0071,-0.11,0.2,-0.0003,0.43,-0.0076,-0.0031,0.0015,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0054,0.022,0.03,0.0053,0.3,0.35,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +32890000,0.72,0.00031,-0.0046,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.0081,0.0077,-0.11,0.2,-0.0003,0.43,-0.0076,-0.0032,0.0016,0,0,-4.9e+02,6.9e-05,5.8e-05,0.0054,0.022,0.031,0.0053,0.32,0.36,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +32990000,0.72,0.00052,-0.0047,0.69,-1.3,-0.82,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.0086,0.0078,-0.11,0.2,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-4.9e+02,6.7e-05,5.7e-05,0.0052,0.021,0.029,0.0053,0.31,0.36,0.03,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +33090000,0.72,0.00049,-0.0048,0.69,-1.3,-0.82,0.76,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.0085,0.0081,-0.11,0.2,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0053,0.022,0.031,0.0053,0.33,0.37,0.031,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +33190000,0.72,0.0039,-0.0039,0.7,-1.2,-0.8,0.7,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.0086,0.008,-0.11,0.21,-0.0003,0.43,-0.0068,-0.0031,0.0017,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.029,0.0053,0.32,0.37,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 +33290000,0.67,0.016,-0.0033,0.74,-1.2,-0.78,0.68,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.0084,0.0083,-0.11,0.2,-0.00028,0.43,-0.007,-0.0032,0.0016,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.031,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.3e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 +33390000,0.56,0.014,-0.0037,0.83,-1.2,-0.77,0.88,0,0,-4.9e+02,-0.0014,-0.0057,-0.00013,0.0086,0.0086,-0.11,0.21,-0.00034,0.43,-0.0063,-0.0032,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 +33490000,0.43,0.007,-0.0011,0.9,-1.2,-0.76,0.89,0,0,-4.9e+02,-0.0014,-0.0057,-0.00015,0.0086,0.0087,-0.11,0.21,-0.00043,0.43,-0.0058,-0.002,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 +33590000,0.27,0.00094,-0.0036,0.96,-1.2,-0.75,0.86,0,0,-4.9e+02,-0.0014,-0.0057,-0.00018,0.0086,0.0087,-0.11,0.21,-0.00067,0.43,-0.0038,-0.0013,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 +33690000,0.098,-0.0027,-0.0066,1,-1.1,-0.74,0.87,0,0,-4.9e+02,-0.0014,-0.0057,-0.00019,0.0086,0.0087,-0.11,0.21,-0.00072,0.43,-0.0035,-0.001,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 +33790000,-0.074,-0.0045,-0.0084,1,-1.1,-0.72,0.85,0,0,-4.9e+02,-0.0014,-0.0057,-0.00021,0.0086,0.0087,-0.11,0.21,-0.00088,0.43,-0.0021,-0.001,0.0023,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 +33890000,-0.24,-0.0059,-0.009,0.97,-1,-0.69,0.83,0,0,-4.9e+02,-0.0014,-0.0057,-0.00022,0.0086,0.0087,-0.11,0.21,-0.001,0.43,-0.0012,-0.0011,0.0024,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 +33990000,-0.39,-0.0046,-0.012,0.92,-0.94,-0.64,0.8,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.0086,0.0088,-0.11,0.21,-0.00098,0.43,-0.0011,-0.00064,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 +34090000,-0.5,-0.0037,-0.014,0.87,-0.88,-0.59,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.0086,0.0091,-0.11,0.21,-0.00095,0.43,-0.0012,-0.00052,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0014,0.023,0.03,0.0052,0.37,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,3e-05,3.4e-05,0.00036,2.2e-05,2.3e-05,0.00036,1,1,0.01 +34190000,-0.57,-0.0037,-0.012,0.82,-0.85,-0.54,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0064,0.012,-0.11,0.21,-0.00094,0.43,-0.001,-0.00031,0.0027,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0013,0.023,0.029,0.0052,0.37,0.38,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.5e-05,3.4e-05,0.00036,1.8e-05,1.8e-05,0.00036,1,1,0.01 +34290000,-0.61,-0.0046,-0.0091,0.79,-0.8,-0.48,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0062,0.012,-0.11,0.21,-0.00096,0.43,-0.00092,-0.00018,0.0027,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0012,0.025,0.032,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.2e-05,3.4e-05,0.00036,1.5e-05,1.5e-05,0.00036,1,1,0.01 +34390000,-0.63,-0.0052,-0.0062,0.77,-0.77,-0.44,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0034,0.016,-0.11,0.21,-0.00092,0.43,-0.00091,1.7e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0012,0.025,0.031,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.2e-05,2e-05,3.3e-05,0.00036,1.3e-05,1.3e-05,0.00036,1,1,0.01 +34490000,-0.65,-0.0062,-0.004,0.76,-0.72,-0.39,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0032,0.016,-0.11,0.21,-0.00093,0.43,-0.00085,-2.4e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0011,0.027,0.035,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.6e-07,0.0034,0.0035,8.1e-05,1.8e-05,3.3e-05,0.00036,1.2e-05,1.2e-05,0.00036,1,1,0.01 +34590000,-0.66,-0.0062,-0.0026,0.75,-0.7,-0.37,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,-0.0018,0.023,-0.11,0.21,-0.00088,0.43,-0.00092,3.1e-05,0.0032,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.027,0.034,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,1.1e-05,1e-05,0.00036,1,1,0.01 +34690000,-0.67,-0.0066,-0.0018,0.75,-0.64,-0.32,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,-0.002,0.023,-0.11,0.21,-0.00091,0.43,-0.00078,0.00023,0.0031,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.03,0.037,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,9.9e-06,9.4e-06,0.00036,1,1,0.01 +34790000,-0.67,-0.006,-0.0013,0.74,-0.63,-0.3,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.008,0.03,-0.11,0.21,-0.0009,0.43,-0.00062,0.00032,0.0032,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.029,0.036,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,9.1e-06,8.6e-06,0.00036,1,1,0.01 +34890000,-0.67,-0.006,-0.0012,0.74,-0.57,-0.26,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.0081,0.03,-0.11,0.21,-0.00089,0.43,-0.00063,0.00028,0.0033,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.032,0.04,0.0052,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,8.4e-06,7.9e-06,0.00036,1,1,0.01 +34990000,-0.67,-0.013,-0.0037,0.74,0.47,0.35,-0.043,0,0,-4.9e+02,-0.0016,-0.0058,-6.8e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00051,0.00034,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.001,0.034,0.046,0.0054,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.3e-05,3.3e-05,0.00036,8e-06,7.4e-06,0.00036,1,1,0.01 +35090000,-0.67,-0.013,-0.0037,0.74,0.6,0.39,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-6.9e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00047,0.00028,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.037,0.051,0.0055,0.42,0.43,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.5e-06,6.9e-06,0.00036,1,1,0.01 +35190000,-0.67,-0.013,-0.0038,0.74,0.63,0.43,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-7e-05,-0.016,0.039,-0.11,0.21,-0.00088,0.43,-0.00041,0.00032,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.041,0.055,0.0055,0.43,0.44,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.1e-06,6.4e-06,0.00036,1,1,0.01 +35290000,-0.67,-0.013,-0.0038,0.74,0.66,0.47,-0.099,0,0,-4.9e+02,-0.0016,-0.0058,-7.1e-05,-0.016,0.039,-0.11,0.21,-0.00089,0.43,-0.00033,0.00032,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.044,0.06,0.0055,0.44,0.45,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.8e-06,6.1e-06,0.00036,1,1,0.01 +35390000,-0.67,-0.013,-0.0038,0.74,0.69,0.51,-0.096,0,0,-4.9e+02,-0.0016,-0.0058,-7.4e-05,-0.016,0.039,-0.11,0.21,-0.00091,0.43,-0.00023,0.0003,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.048,0.064,0.0055,0.46,0.47,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.5e-06,5.8e-06,0.00036,1,1,0.01 +35490000,-0.67,-0.013,-0.0038,0.74,0.73,0.56,-0.095,0,0,-4.9e+02,-0.0016,-0.0058,-7.6e-05,-0.016,0.039,-0.11,0.21,-0.00092,0.43,-0.00014,0.00025,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.053,0.069,0.0055,0.47,0.48,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.2e-06,5.5e-06,0.00036,1,1,0.01 +35590000,-0.67,-0.013,-0.0038,0.74,0.76,0.6,-0.094,0,0,-4.9e+02,-0.0016,-0.0058,-7.5e-05,-0.016,0.039,-0.11,0.21,-0.00092,0.43,-0.00017,0.00025,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.057,0.075,0.0055,0.49,0.5,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6e-06,5.2e-06,0.00036,1,1,0.01 +35690000,-0.67,-0.013,-0.0038,0.74,0.79,0.64,-0.092,0,0,-4.9e+02,-0.0016,-0.0058,-7.7e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-9.2e-05,0.00024,0.0036,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.062,0.08,0.0056,0.51,0.52,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0032,8e-05,1e-05,3.3e-05,0.00036,5.8e-06,5e-06,0.00036,1,1,0.01 +35790000,-0.67,-0.013,-0.0038,0.74,0.82,0.68,-0.089,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-6.8e-05,0.00024,0.0036,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.067,0.086,0.0056,0.53,0.54,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0032,8e-05,1e-05,3.3e-05,0.00036,5.6e-06,4.8e-06,0.00036,1,1,0.023 +35890000,-0.67,-0.013,-0.0039,0.74,0.85,0.72,-0.086,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00094,0.43,-5.3e-05,0.00022,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.072,0.092,0.0056,0.55,0.56,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.4e-06,4.6e-06,0.00036,1,1,0.048 +35990000,-0.67,-0.013,-0.0038,0.74,0.88,0.76,-0.083,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-7.8e-05,0.00021,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.078,0.099,0.0056,0.57,0.59,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,9.9e-06,3.3e-05,0.00036,5.3e-06,4.5e-06,0.00036,1,1,0.073 +36090000,-0.68,-0.013,-0.0038,0.74,0.91,0.81,-0.079,0,0,-4.9e+02,-0.0016,-0.0058,-7.9e-05,-0.016,0.039,-0.11,0.21,-0.00094,0.43,-4.3e-05,0.00019,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.083,0.11,0.0056,0.6,0.62,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.7e-06,3.3e-05,0.00036,5.2e-06,4.3e-06,0.00036,1,1,0.099 +36190000,-0.68,-0.013,-0.0038,0.74,0.94,0.85,-0.075,0,0,-4.9e+02,-0.0016,-0.0058,-8.4e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,5.5e-05,0.00018,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.089,0.11,0.0056,0.63,0.65,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.5e-06,3.3e-05,0.00036,5e-06,4.2e-06,0.00036,1,1,0.12 +36290000,-0.68,-0.013,-0.0038,0.74,0.97,0.89,-0.07,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.016,0.039,-0.11,0.21,-0.00096,0.43,7.5e-05,0.00018,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.096,0.12,0.0056,0.66,0.68,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.4e-06,3.3e-05,0.00036,4.9e-06,4.1e-06,0.00036,1,1,0.15 +36390000,-0.68,-0.013,-0.0038,0.74,1,0.93,-0.067,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.016,0.039,-0.11,0.21,-0.00096,0.43,7.2e-05,0.00022,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.1,0.13,0.0056,0.69,0.72,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.3e-06,3.3e-05,0.00036,4.8e-06,3.9e-06,0.00036,1,1,0.17 +36490000,-0.68,-0.013,-0.0038,0.74,1,0.97,-0.063,0,0,-4.9e+02,-0.0016,-0.0058,-8.3e-05,-0.015,0.039,-0.11,0.21,-0.00095,0.43,4.2e-05,0.00023,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.11,0.13,0.0056,0.72,0.76,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.2e-06,3.3e-05,0.00036,4.7e-06,3.8e-06,0.00036,1,1,0.2 +36590000,-0.68,-0.013,-0.0038,0.74,1.1,1,-0.057,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.015,0.039,-0.11,0.21,-0.00096,0.43,9e-05,0.00025,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.14,0.0056,0.76,0.8,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.1e-06,3.3e-05,0.00036,4.6e-06,3.7e-06,0.00036,1,1,0.23 +36690000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.053,0,0,-4.9e+02,-0.0016,-0.0058,-8.8e-05,-0.015,0.039,-0.11,0.21,-0.00097,0.43,0.00012,0.00027,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.15,0.0057,0.8,0.85,0.032,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9e-06,3.3e-05,0.00036,4.6e-06,3.6e-06,0.00036,1,1,0.25 +36790000,-0.68,-0.013,-0.0037,0.74,1.1,1.1,-0.047,0,0,-4.9e+02,-0.0016,-0.0058,-9.1e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00017,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.13,0.16,0.0057,0.84,0.9,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.9e-06,3.3e-05,0.00036,4.5e-06,3.5e-06,0.00036,1,1,0.28 +36890000,-0.68,-0.013,-0.0037,0.74,1.2,1.1,-0.042,0,0,-4.9e+02,-0.0016,-0.0058,-9.4e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00021,0.00022,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0056,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00036,1,1,0.3 +36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00023,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00036,1,1,0.33 +37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00036,1,1,0.35 +37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.6e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.38 +37290000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.02,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00024,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00035,1,1,0.41 +37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00027,0.00027,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00035,1,1,0.43 +37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0093,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.015,0.038,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00035,1,1,0.46 +37590000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0027,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00032,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.2,0.23,0.0057,1.3,1.5,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.3e-06,3.3e-05,0.00036,4e-06,3e-06,0.00035,1,1,0.48 +37690000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,0.0046,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00033,0.00029,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.21,0.24,0.0057,1.4,1.6,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.51 +37790000,-0.68,-0.013,-0.0036,0.74,1.5,1.5,0.012,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00034,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.22,0.26,0.0056,1.5,1.7,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.54 +37890000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.017,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0038,0,0,-4.9e+02,4.5e-05,4.5e-05,0.00096,0.23,0.27,0.0056,1.6,1.8,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.56 +37990000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.025,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.24,0.28,0.0056,1.7,1.9,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.59 +38090000,-0.68,-0.013,-0.0036,0.74,1.5,1.7,0.034,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00037,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.25,0.29,0.0056,1.8,2,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.61 +38190000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.04,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00029,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.26,0.3,0.0056,1.9,2.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.64 +38290000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.046,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00028,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.27,0.31,0.0056,2.1,2.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.67 +38390000,-0.68,-0.013,-0.0035,0.74,1.6,1.8,0.052,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.036,-0.11,0.21,-0.001,0.43,0.00037,0.00029,0.0037,0,0,-4.9e+02,4.6e-05,4.6e-05,0.00096,0.28,0.33,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.69 +38490000,-0.68,-0.013,-0.0035,0.74,1.7,1.8,0.058,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.036,-0.11,0.21,-0.001,0.43,0.00038,0.00031,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.72 +38590000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.063,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00035,1,1,0.75 +38690000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.069,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.77 +38790000,-0.68,-0.013,-0.0034,0.74,1.8,1.9,0.075,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 +38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.00031,0.0038,0,0,-4.9e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index a63e88ee9de4..b9bbada6df04 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.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 -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,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 -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,-1.1e-06,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 -290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-2.1e-10,0,0,-1e-05,0,0,0,0,0,0,0,0,0.095,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 -390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.063,-0.00056,-0.00071,-0.013,-7.1e-09,-5.8e-09,1.5e-11,0,0,2.2e-06,0,0,0,0,0,0,0,0,0.095,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 -490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.069,-0.00015,-0.00046,-0.011,-1.2e-06,7.4e-07,4.1e-08,0,0,-1e-06,0,0,0,0,0,0,0,0,0.095,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 -590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.12,-0.00028,-0.0012,-0.029,-1.3e-06,7.7e-07,4.5e-08,0,0,7.8e-05,0,0,0,0,0,0,0,0,0.095,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 -690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.05,-8e-05,-0.00078,-0.0088,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.095,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 -790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,0.095,0.018,0.018,0.00077,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 -890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.093,0.00015,-0.0011,-0.031,-2.1e-05,1e-06,4.9e-07,0,0,-8.1e-05,0,0,0,0,0,0,0,0,0.095,0.019,0.019,0.00051,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 -990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1e-06,4.9e-07,0,0,-2.6e-05,0,0,0,0,0,0,0,0,0.095,0.021,0.021,0.00062,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 -1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0.00077,-0.0014,-0.062,-6e-05,-1.5e-05,9.8e-07,0,0,1.1e-05,0,0,0,0,0,0,0,0,0.095,0.023,0.023,0.00043,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 -1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,9.6e-07,0,0,-0.00056,0,0,0,0,0,0,0,0,0.095,0.025,0.025,0.00051,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 -1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.11,0.0019,-0.0024,-0.048,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00083,0,0,0,0,0,0,0,0,0.095,0.026,0.026,0.00038,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 -1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.097,0.0043,-0.0044,-0.038,-0.00016,-9.2e-05,1.5e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.095,0.028,0.028,0.00043,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 -1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.12,0.0034,-0.0032,-0.053,-0.00039,-0.00033,1.2e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,0.095,0.027,0.027,0.00033,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 -1590000,1,-0.012,-0.014,0.0004,0.031,-0.024,-0.13,0.0061,-0.0055,-0.063,-0.00039,-0.00033,1.2e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.095,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 -1690000,1,-0.012,-0.014,0.00045,0.028,-0.019,-0.13,0.0043,-0.0037,-0.068,-0.00073,-0.00074,-3.4e-07,0,0,-0.0019,0,0,0,0,0,0,0,0,0.095,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 -1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0.0076,-0.0059,-0.067,-0.00073,-0.00073,-2.9e-07,0,0,-0.0029,0,0,0,0,0,0,0,0,0.095,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 -1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0.011,-0.0083,-0.075,-0.00072,-0.00072,-2.7e-07,0,0,-0.0033,0,0,0,0,0,0,0,0,0.095,0.031,0.031,0.00037,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 -1990000,1,-0.011,-0.014,0.00041,0.035,-0.018,-0.14,0.0082,-0.0054,-0.074,-0.0011,-0.0013,-3.6e-06,0,0,-0.0047,0,0,0,0,0,0,0,0,0.095,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 -2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0.012,-0.0073,-0.071,-0.0011,-0.0012,-3.5e-06,0,0,-0.0066,0,0,0,0,0,0,0,0,0.095,0.027,0.027,0.00032,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 -2190000,1,-0.011,-0.014,0.00039,0.033,-0.014,-0.14,0.0081,-0.0043,-0.077,-0.0014,-0.0018,-8.7e-06,0,0,-0.0076,0,0,0,0,0,0,0,0,0.095,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 -2290000,1,-0.011,-0.014,0.00039,0.038,-0.014,-0.14,0.012,-0.0057,-0.075,-0.0014,-0.0018,-8.5e-06,0,0,-0.0099,0,0,0,0,0,0,0,0,0.095,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 -2390000,1,-0.011,-0.013,0.0004,0.029,-0.01,-0.14,0.0075,-0.0033,-0.072,-0.0017,-0.0023,-1.4e-05,0,0,-0.013,0,0,0,0,0,0,0,0,0.095,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 -2490000,1,-0.011,-0.014,0.00048,0.033,-0.0091,-0.14,0.011,-0.0043,-0.079,-0.0017,-0.0023,-1.4e-05,0,0,-0.014,0,0,0,0,0,0,0,0,0.095,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 -2590000,1,-0.01,-0.013,0.0004,0.023,-0.0062,-0.15,0.0066,-0.0024,-0.084,-0.0018,-0.0027,-2e-05,0,0,-0.015,0,0,0,0,0,0,0,0,0.095,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 -2690000,1,-0.01,-0.013,0.00044,0.027,-0.0055,-0.15,0.0091,-0.003,-0.084,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,0.095,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 -2790000,1,-0.01,-0.013,0.00038,0.022,-0.0032,-0.14,0.0059,-0.0017,-0.081,-0.0019,-0.003,-2.5e-05,0,0,-0.022,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 -2890000,1,-0.01,-0.013,0.00031,0.026,-0.005,-0.14,0.0082,-0.0021,-0.081,-0.0019,-0.003,-2.5e-05,0,0,-0.026,0,0,0,0,0,0,0,0,0.095,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 -2990000,1,-0.01,-0.013,0.00033,0.02,-0.0039,-0.15,0.0054,-0.0013,-0.086,-0.002,-0.0033,-3e-05,0,0,-0.028,0,0,0,0,0,0,0,0,0.095,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 -3090000,1,-0.01,-0.013,0.00053,0.025,-0.0068,-0.15,0.0077,-0.0019,-0.087,-0.002,-0.0033,-3e-05,0,0,-0.031,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 -3190000,1,-0.01,-0.013,0.00058,0.02,-0.0065,-0.15,0.0051,-0.0014,-0.097,-0.0021,-0.0036,-3.5e-05,0,0,-0.033,0,0,0,0,0,0,0,0,0.095,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 -3290000,1,-0.01,-0.013,0.0006,0.023,-0.0067,-0.15,0.0073,-0.0021,-0.11,-0.0021,-0.0036,-3.4e-05,0,0,-0.035,0,0,0,0,0,0,0,0,0.095,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 -3390000,1,-0.0098,-0.013,0.00061,0.019,-0.0036,-0.15,0.0049,-0.0014,-0.1,-0.0021,-0.0038,-3.8e-05,0,0,-0.04,0,0,0,0,0,0,0,0,0.095,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 -3490000,1,-0.0097,-0.013,0.0006,0.025,-0.0023,-0.15,0.0072,-0.0017,-0.1,-0.0021,-0.0038,-3.8e-05,0,0,-0.044,0,0,0,0,0,0,0,0,0.095,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 -3590000,1,-0.0095,-0.012,0.00056,0.021,-0.0018,-0.15,0.0051,-0.001,-0.11,-0.0022,-0.004,-4.2e-05,0,0,-0.047,0,0,0,0,0,0,0,0,0.095,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 -3690000,1,-0.0095,-0.013,0.00054,0.024,-0.0011,-0.15,0.0074,-0.0012,-0.11,-0.0022,-0.004,-4.2e-05,0,0,-0.052,0,0,0,0,0,0,0,0,0.095,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 -3790000,1,-0.0094,-0.012,0.00057,0.019,0.0033,-0.15,0.0051,-0.00056,-0.11,-0.0022,-0.0043,-4.7e-05,0,0,-0.055,0,0,0,0,0,0,0,0,0.095,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 -3890000,1,-0.0094,-0.013,0.00065,0.021,0.0046,-0.14,0.0072,-0.00017,-0.11,-0.0022,-0.0042,-4.7e-05,0,0,-0.059,0,0,0,0,0,0,0,0,0.095,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -3990000,1,-0.0094,-0.013,0.00072,0.026,0.0042,-0.14,0.0096,0.00021,-0.11,-0.0022,-0.0042,-4.7e-05,0,0,-0.064,0,0,0,0,0,0,0,0,0.095,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -4090000,1,-0.0093,-0.012,0.00078,0.022,0.0037,-0.12,0.0071,0.00045,-0.098,-0.0022,-0.0044,-5.1e-05,0,0,-0.072,0,0,0,0,0,0,0,0,0.095,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4190000,1,-0.0094,-0.012,0.00075,0.024,0.0034,-0.12,0.0094,0.0008,-0.1,-0.0022,-0.0044,-5.1e-05,0,0,-0.074,0,0,0,0,0,0,0,0,0.095,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4290000,1,-0.0095,-0.012,0.00076,0.021,0.0033,-0.12,0.0068,0.00069,-0.11,-0.0022,-0.0046,-5.6e-05,0,0,-0.077,0,0,0,0,0,0,0,0,0.095,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4390000,1,-0.0094,-0.012,0.00072,0.025,0.0018,-0.11,0.0091,0.00086,-0.094,-0.0022,-0.0046,-5.6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,0.095,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4490000,1,-0.0094,-0.012,0.00078,0.021,0.0036,-0.11,0.0067,0.00072,-0.095,-0.0022,-0.0048,-6.1e-05,0,0,-0.086,0,0,0,0,0,0,0,0,0.095,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4590000,1,-0.0094,-0.012,0.00085,0.023,0.0024,-0.11,0.0089,0.001,-0.098,-0.0022,-0.0048,-6.1e-05,0,0,-0.088,0,0,0,0,0,0,0,0,0.095,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4690000,1,-0.0094,-0.012,0.00079,0.017,0.0027,-0.1,0.0064,0.00075,-0.09,-0.0021,-0.005,-6.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,0.095,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4790000,1,-0.0093,-0.012,0.00089,0.015,0.0048,-0.099,0.008,0.0012,-0.092,-0.0021,-0.005,-6.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,0.095,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4890000,1,-0.0093,-0.012,0.00093,0.01,0.0024,-0.093,0.0053,0.0009,-0.088,-0.0021,-0.0051,-6.9e-05,0,0,-0.099,0,0,0,0,0,0,0,0,0.095,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -4990000,1,-0.0092,-0.012,0.00091,0.013,0.0031,-0.085,0.0065,0.0012,-0.083,-0.0021,-0.0051,-6.9e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.095,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5090000,1,-0.0091,-0.011,0.00099,0.01,0.0034,-0.082,0.0045,0.00088,-0.082,-0.0021,-0.0052,-7.2e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.095,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5190000,1,-0.0089,-0.012,0.001,0.0099,0.007,-0.08,0.0055,0.0014,-0.079,-0.0021,-0.0052,-7.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0071,-0.068,0.0038,0.0013,-0.072,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0.0046,0.0021,-0.067,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.06,0.0031,0.002,-0.065,-0.002,-0.0054,-7.7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5590000,1,-0.0088,-0.012,0.001,0.0083,0.016,-0.053,0.004,0.0033,-0.058,-0.002,-0.0054,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5690000,1,-0.0089,-0.011,0.00093,0.0077,0.016,-0.052,0.0028,0.0029,-0.055,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5790000,1,-0.0088,-0.011,0.00088,0.0089,0.018,-0.049,0.0036,0.0046,-0.053,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5890000,1,-0.0088,-0.011,0.00092,0.0095,0.015,-0.048,0.0027,0.0037,-0.056,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5990000,1,-0.0088,-0.012,0.00089,0.011,0.017,-0.041,0.0038,0.0053,-0.05,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -6090000,1,-0.0088,-0.011,0.00071,0.011,0.018,-0.039,0.0049,0.0071,-0.047,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6190000,1,-0.0089,-0.011,0.00072,0.0087,0.017,-0.038,0.0038,0.0057,-0.047,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6290000,1,-0.0089,-0.011,0.00075,0.008,0.019,-0.041,0.0046,0.0075,-0.053,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6390000,1,-0.0089,-0.011,0.00076,0.0082,0.016,-0.042,0.0034,0.006,-0.056,-0.0018,-0.0056,-8.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6490000,1,-0.0089,-0.011,0.00066,0.0057,0.016,-0.039,0.0041,0.0076,-0.053,-0.0018,-0.0056,-8.7e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6590000,1,-0.0089,-0.011,0.00059,0.0039,0.015,-0.042,0.0029,0.0058,-0.056,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6690000,1,-0.0088,-0.011,0.00055,0.0022,0.018,-0.044,0.0032,0.0075,-0.057,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6790000,1,-0.0089,-0.011,0.00052,0.003,0.015,-0.042,0.0021,0.006,-0.058,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6890000,1,-0.0087,-0.011,0.00043,0.0023,0.015,-0.039,0.0024,0.0075,-0.055,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 -6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0.0023,0.0048,-0.055,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,0.095,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 -7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0.0014,0.0074,-0.056,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,0.095,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 -7190000,0.98,-0.0065,-0.012,0.18,-5.8e-05,0.019,-0.037,0.0016,0.0091,-0.058,-0.0016,-0.0056,-9.2e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.7e-06,0,0,0.095,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 -7290000,0.98,-0.0064,-0.012,0.18,-0.00067,0.024,-0.034,0.00055,0.012,-0.054,-0.0016,-0.0057,-9.2e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,0.095,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00094,-0.032,0.00016,0.014,-0.052,-0.0017,-0.0057,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.8e-05,0,0,0.095,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7490000,0.98,-0.0063,-0.012,0.18,0.00097,0.0035,-0.026,0.00016,0.014,-0.046,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.9e-05,0,0,0.095,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.006,-0.023,0.00032,0.014,-0.041,-0.0017,-0.0056,-9e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00034,-0.00039,-9.5e-06,0,0,0.095,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7690000,0.98,-0.0064,-0.013,0.18,0.0021,0.0093,-0.022,0.00055,0.015,-0.036,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-3.4e-05,0.43,-0.00031,-0.0004,2.5e-06,0,0,0.095,0.0016,0.0015,0.042,25,25,0.025,52,52,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00017,0.0013,0.0014,0.0016,0.0013,1,1,2 -7790000,0.98,-0.0064,-0.013,0.18,0.0056,0.01,-0.025,0.00093,0.015,-0.041,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.9e-05,0.43,-0.00021,-0.00039,-1.5e-06,0,0,0.095,0.0016,0.0016,0.042,24,24,0.024,35,35,0.061,6.3e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00015,0.0013,0.0014,0.0016,0.0013,1,1,2 -7890000,0.98,-0.0065,-0.013,0.18,0.0046,0.014,-0.025,0.0013,0.017,-0.045,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.6e-05,0.43,-0.00019,-0.0004,4.4e-05,0,0,0.095,0.0016,0.0016,0.042,24,24,0.023,36,36,0.06,6.3e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00013,0.0013,0.0014,0.0016,0.0013,1,1,2 -7990000,0.98,-0.0063,-0.013,0.18,0.0032,0.017,-0.022,0.00098,0.017,-0.042,-0.0016,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-2.5e-05,0.43,-0.0002,-0.00042,7.4e-05,0,0,0.095,0.0017,0.0016,0.042,24,24,0.022,28,28,0.059,6.2e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00012,0.0013,0.0014,0.0016,0.0013,1,1,2 -8090000,0.98,-0.0063,-0.013,0.18,0.0043,0.019,-0.022,0.0013,0.019,-0.044,-0.0016,-0.0056,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-2.2e-05,0.43,-0.00017,-0.00042,9.9e-05,0,0,0.095,0.0017,0.0017,0.042,24,24,0.022,30,30,0.059,6.2e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,0.00011,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8190000,0.98,-0.0064,-0.012,0.18,0.0052,0.022,-0.018,0.0015,0.019,-0.038,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.44,-0.00014,-0.00041,0.00014,0,0,0.095,0.0018,0.0017,0.041,23,23,0.021,24,24,0.058,6.1e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0013,0.0001,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8290000,0.98,-0.0062,-0.012,0.18,0.002,0.028,-0.017,0.0003,0.022,-0.038,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.43,-0.00014,-0.00044,6.7e-05,0,0,0.095,0.0018,0.0017,0.041,23,23,0.02,27,27,0.057,6.1e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0013,9.6e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8390000,0.98,-0.0062,-0.012,0.18,-0.0023,0.028,-0.016,-0.00016,0.022,-0.036,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00044,2.1e-05,0,0,0.095,0.0019,0.0018,0.041,21,21,0.02,23,23,0.057,6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0013,9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8490000,0.98,-0.0059,-0.012,0.18,-0.0061,0.035,-0.017,-0.0015,0.026,-0.041,-0.0017,-0.0059,-9.6e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00048,-7.4e-06,0,0,0.095,0.0019,0.0018,0.041,21,21,0.019,25,25,0.056,5.9e-05,5.6e-05,2.2e-06,0.04,0.04,0.0011,0.0013,8.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8590000,0.98,-0.006,-0.012,0.18,-0.00034,0.034,-0.012,0.00033,0.026,-0.036,-0.0016,-0.0057,-9.6e-05,-0.00036,0.00036,-0.14,0.2,-1.7e-05,0.43,-0.00018,-0.00043,3.3e-06,0,0,0.095,0.0019,0.0018,0.041,19,19,0.018,22,22,0.055,5.8e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0013,7.9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8690000,0.98,-0.006,-0.012,0.18,-0.0022,0.037,-0.014,-0.00034,0.03,-0.038,-0.0016,-0.0058,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-1.6e-05,0.43,-0.00022,-0.00045,-8.7e-05,0,0,0.095,0.002,0.0018,0.041,19,19,0.018,24,24,0.055,5.8e-05,5.3e-05,2.2e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8790000,0.98,-0.006,-0.012,0.18,-0.005,0.039,-0.014,-0.0023,0.033,-0.035,-0.0016,-0.0058,-9.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00019,-0.00045,-0.00013,0,0,0.095,0.002,0.0018,0.041,19,19,0.018,27,27,0.055,5.7e-05,5.2e-05,2.2e-06,0.04,0.04,0.00099,0.0013,7.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8890000,0.98,-0.006,-0.012,0.18,0.00054,0.041,-0.0093,0.00054,0.033,-0.029,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00026,-0.00043,-0.00011,0,0,0.095,0.002,0.0018,0.041,17,17,0.017,24,24,0.054,5.6e-05,5e-05,2.2e-06,0.04,0.04,0.00094,0.0013,6.7e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -8990000,0.98,-0.0059,-0.013,0.18,0.01,0.047,-0.0085,0.0062,0.038,-0.032,-0.0017,-0.0056,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00037,-0.00039,-6.6e-05,0,0,0.095,0.0021,0.0018,0.041,17,17,0.017,27,27,0.054,5.5e-05,4.9e-05,2.2e-06,0.04,0.04,0.00091,0.0013,6.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -9090000,0.98,-0.0055,-0.012,0.18,-0.00032,0.056,-0.0095,0.00069,0.041,-0.032,-0.0018,-0.0057,-8.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00028,-0.00049,-8.8e-05,0,0,0.095,0.0021,0.0018,0.041,15,15,0.016,24,24,0.053,5.3e-05,4.7e-05,2.2e-06,0.04,0.04,0.00087,0.0013,6.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -9190000,0.98,-0.0053,-0.013,0.18,0.0029,0.064,-0.0089,0.00089,0.049,-0.032,-0.0018,-0.0057,-8.6e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00031,-0.00053,-0.00011,0,0,0.095,0.0021,0.0018,0.041,15,15,0.016,27,27,0.052,5.2e-05,4.5e-05,2.2e-06,0.04,0.04,0.00083,0.0013,5.9e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.3 -9290000,0.98,-0.0053,-0.013,0.18,0.013,0.062,-0.0074,0.0055,0.046,-0.03,-0.0018,-0.0056,-8.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.0004,-0.00049,-8.5e-05,0,0,0.095,0.0021,0.0018,0.041,13,13,0.015,24,24,0.052,5.1e-05,4.4e-05,2.2e-06,0.04,0.04,0.00079,0.0013,5.6e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 -9390000,0.98,-0.0054,-0.013,0.18,0.014,0.06,-0.0062,0.0071,0.049,-0.03,-0.0017,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00035,-0.00043,-3.8e-05,0,0,0.095,0.0021,0.0018,0.041,13,13,0.015,26,26,0.052,5e-05,4.2e-05,2.2e-06,0.04,0.04,0.00077,0.0013,5.4e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 -9490000,0.98,-0.0053,-0.013,0.18,0.0087,0.061,-0.0045,0.0044,0.048,-0.027,-0.0018,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00033,-0.00048,-7.6e-05,0,0,0.095,0.0021,0.0018,0.041,12,12,0.015,23,23,0.051,4.8e-05,4e-05,2.2e-06,0.04,0.04,0.00074,0.0013,5.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 -9590000,0.98,-0.0057,-0.013,0.18,0.0078,0.052,-0.0045,0.0046,0.048,-0.029,-0.0016,-0.0057,-9.3e-05,-0.00036,0.00036,-0.14,0.2,-1e-05,0.43,-0.00031,-0.00041,-0.00012,0,0,0.095,0.0022,0.0018,0.041,12,12,0.014,26,26,0.05,4.7e-05,3.9e-05,2.2e-06,0.04,0.04,0.00071,0.0013,5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 -9690000,0.98,-0.0059,-0.013,0.18,0.0096,0.047,-0.0016,0.0057,0.043,-0.027,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-9.2e-06,0.43,-0.00031,-0.00037,-9.9e-05,0,0,0.095,0.0022,0.0018,0.041,10,10,0.014,23,23,0.05,4.6e-05,3.7e-05,2.2e-06,0.04,0.04,0.00068,0.0013,4.8e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9790000,0.98,-0.0061,-0.012,0.18,0.001,0.041,-0.003,0.00039,0.042,-0.028,-0.0015,-0.0058,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.7e-06,0.43,-0.00018,-0.00036,-0.00015,0,0,0.095,0.0022,0.0017,0.041,10,10,0.014,25,25,0.05,4.4e-05,3.5e-05,2.2e-06,0.04,0.04,0.00066,0.0013,4.7e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9890000,0.98,-0.0061,-0.012,0.18,0.0086,0.04,-0.0016,0.0039,0.04,-0.029,-0.0015,-0.0057,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.6e-06,0.43,-0.00023,-0.00034,-9.4e-05,0,0,0.095,0.0022,0.0017,0.041,8.6,8.7,0.013,22,22,0.049,4.3e-05,3.4e-05,2.2e-06,0.04,0.04,0.00063,0.0013,4.5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9990000,0.98,-0.0063,-0.012,0.18,0.0025,0.034,-0.00096,-0.0002,0.039,-0.031,-0.0014,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-6.4e-06,0.43,-0.00014,-0.00033,-0.00013,0,0,0.095,0.0022,0.0017,0.041,8.6,8.7,0.013,24,24,0.049,4.2e-05,3.2e-05,2.2e-06,0.04,0.04,0.00061,0.0013,4.4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -10090000,0.98,-0.0067,-0.012,0.18,0.00073,0.019,0.00023,0.00018,0.03,-0.029,-0.0013,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-4.9e-06,0.43,-9.7e-05,-0.00025,-0.00015,0,0,0.095,0.0022,0.0017,0.041,7.4,7.5,0.013,21,21,0.048,4e-05,3.1e-05,2.2e-06,0.04,0.04,0.00059,0.0013,4.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10190000,0.98,-0.0072,-0.012,0.18,0.0053,0.0047,0.0011,0.0043,0.022,-0.03,-0.0011,-0.0058,-0.00012,-0.00036,0.00036,-0.14,0.2,-3.3e-06,0.43,-9.1e-05,-0.00012,-0.00015,0,0,0.095,0.0022,0.0016,0.041,7.4,7.6,0.012,23,23,0.048,3.9e-05,2.9e-05,2.2e-06,0.04,0.04,0.00057,0.0013,4.1e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10290000,0.98,-0.0071,-0.012,0.18,0.012,0.0085,3e-05,0.008,0.023,-0.029,-0.0012,-0.0057,-0.00011,-0.00036,0.00036,-0.14,0.2,-3.7e-06,0.43,-0.00016,-0.00011,-0.00012,0,0,0.095,0.0022,0.0016,0.041,6.4,6.5,0.012,20,20,0.048,3.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10390000,0.98,-0.0071,-0.012,0.18,0.0076,0.0026,-0.0025,0.00078,3e-05,-0.028,-0.0012,-0.0056,-0.00011,-0.0003,0.00041,-0.14,0.2,-3.9e-06,0.43,-0.0002,-0.00011,-6.5e-05,0,0,0.095,0.0022,0.0016,0.041,0.25,0.25,0.56,0.25,0.25,0.048,3.6e-05,2.7e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10490000,0.98,-0.0073,-0.012,0.18,0.0096,0.00082,0.007,0.0016,0.00011,-0.023,-0.0011,-0.0057,-0.00011,-0.00033,0.00027,-0.14,0.2,-3e-06,0.43,-0.00016,-5.9e-05,-8.7e-05,0,0,0.095,0.0021,0.0016,0.041,0.25,0.26,0.55,0.26,0.26,0.057,3.5e-05,2.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10590000,0.98,-0.007,-0.012,0.18,0.00028,-0.00036,0.013,-0.0011,-0.0055,-0.021,-0.0012,-0.0056,-0.00011,-0.0003,0.00025,-0.14,0.2,-3.6e-06,0.43,-0.00018,-9.8e-05,-7.3e-05,0,0,0.095,0.0021,0.0015,0.041,0.13,0.13,0.27,0.13,0.13,0.055,3.3e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10690000,0.98,-0.0071,-0.013,0.18,0.0031,-0.0029,0.016,-0.0009,-0.0057,-0.018,-0.0011,-0.0056,-0.00011,-0.00027,0.00018,-0.14,0.2,-3.3e-06,0.43,-0.00021,-4.9e-05,-7e-05,0,0,0.095,0.0021,0.0015,0.041,0.13,0.14,0.26,0.14,0.14,0.065,3.2e-05,2.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10790000,0.98,-0.0073,-0.013,0.18,0.0057,-0.0061,0.014,-0.00055,-0.005,-0.015,-0.0011,-0.0056,-0.00011,-0.00016,7.3e-05,-0.14,0.2,-3.1e-06,0.43,-0.00026,1.3e-05,-9e-05,0,0,0.095,0.002,0.0014,0.041,0.09,0.094,0.17,0.09,0.09,0.061,3e-05,2.1e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10890000,0.98,-0.0069,-0.013,0.18,0.008,-0.003,0.0099,0.00021,-0.0052,-0.018,-0.0012,-0.0055,-0.00011,-5.5e-05,0.0002,-0.14,0.2,-4.1e-06,0.43,-0.00033,-3e-05,-7.2e-05,0,0,0.095,0.002,0.0014,0.041,0.096,0.1,0.16,0.096,0.096,0.068,2.9e-05,2e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -10990000,0.98,-0.0069,-0.013,0.18,0.0054,0.0031,0.016,-0.00022,-0.0036,-0.012,-0.0012,-0.0056,-0.00011,-0.00021,0.00015,-0.14,0.2,-4.1e-06,0.43,-0.00026,-5.9e-05,-0.00012,0,0,0.095,0.0019,0.0014,0.041,0.074,0.081,0.12,0.072,0.072,0.065,2.6e-05,1.9e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11090000,0.98,-0.0075,-0.013,0.18,0.0093,0.0016,0.019,0.00071,-0.0039,-0.0075,-0.0011,-0.0056,-0.00011,-0.0003,-5.7e-05,-0.14,0.2,-2.9e-06,0.43,-0.00024,2.4e-05,-0.0001,0,0,0.095,0.0019,0.0013,0.041,0.081,0.092,0.11,0.078,0.078,0.069,2.6e-05,1.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11190000,0.98,-0.0077,-0.013,0.18,0.0082,0.0022,0.026,0.0013,-0.0032,-0.00047,-0.001,-0.0056,-0.00012,-0.00042,-1.2e-05,-0.14,0.2,-2.4e-06,0.43,-0.00017,-1.3e-06,-0.00012,0,0,0.095,0.0017,0.0013,0.04,0.066,0.075,0.084,0.062,0.062,0.066,2.2e-05,1.6e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11290000,0.98,-0.0077,-0.012,0.18,0.0065,-0.00037,0.025,0.0016,-0.0032,-0.00025,-0.001,-0.0057,-0.00012,-0.00052,0.00011,-0.14,0.2,-2.2e-06,0.43,-0.0001,-4e-05,-0.00015,0,0,0.095,0.0017,0.0012,0.04,0.074,0.088,0.078,0.067,0.068,0.069,2.2e-05,1.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11390000,0.98,-0.0076,-0.012,0.18,0.0045,0.00076,0.016,0.0009,-0.0025,-0.0086,-0.001,-0.0057,-0.00012,-0.00055,1.9e-05,-0.14,0.2,-2.3e-06,0.43,-7.8e-05,-5.1e-05,-0.00018,0,0,0.095,0.0015,0.0012,0.04,0.062,0.073,0.064,0.056,0.056,0.066,1.9e-05,1.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11490000,0.98,-0.0075,-0.012,0.18,0.0052,-0.00076,0.02,0.0016,-0.0025,-0.0026,-0.001,-0.0057,-0.00012,-0.00053,-6.4e-05,-0.14,0.2,-2.3e-06,0.43,-0.0001,-3e-05,-0.00017,0,0,0.095,0.0015,0.0011,0.04,0.07,0.086,0.058,0.061,0.062,0.067,1.8e-05,1.3e-05,2.2e-06,0.04,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11590000,0.98,-0.0076,-0.012,0.18,0.0047,-0.00067,0.018,0.00077,-0.002,-0.0037,-0.001,-0.0058,-0.00012,-0.00061,2.7e-05,-0.14,0.2,-2.3e-06,0.43,-5e-05,-7e-05,-0.00019,0,0,0.095,0.0014,0.0011,0.04,0.06,0.072,0.049,0.052,0.052,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11690000,0.98,-0.0075,-0.012,0.18,0.0041,0.0017,0.018,0.00093,-0.0018,-0.0051,-0.0011,-0.0058,-0.00012,-0.00057,0.00015,-0.14,0.2,-2.5e-06,0.43,-4.4e-05,-8.6e-05,-0.00022,0,0,0.095,0.0014,0.001,0.04,0.068,0.084,0.046,0.058,0.059,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11790000,0.98,-0.0075,-0.012,0.18,0.0028,0.0026,0.019,0.00044,-0.0024,-0.0021,-0.0011,-0.0058,-0.00012,-4.6e-05,0.00011,-0.14,0.2,-2.6e-06,0.43,-5.7e-05,-9.1e-05,-0.00022,0,0,0.095,0.0012,0.00096,0.039,0.058,0.07,0.039,0.049,0.05,0.062,1.2e-05,9.4e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11890000,0.98,-0.0077,-0.012,0.18,0.0039,0.0012,0.017,0.0003,-0.0026,-0.0015,-0.0011,-0.0058,-0.00012,-0.00025,0.00027,-0.14,0.2,-2.4e-06,0.43,-2.6e-05,-9.1e-05,-0.00026,0,0,0.095,0.0012,0.00095,0.039,0.066,0.082,0.036,0.056,0.057,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11990000,0.98,-0.0077,-0.012,0.18,0.0067,0.0034,0.014,0.0014,-0.0027,-0.0051,-0.0011,-0.0058,-0.00012,-0.00032,0.00043,-0.14,0.2,-2.7e-06,0.43,-4.4e-05,-9.8e-05,-0.00026,0,0,0.095,0.0011,0.00088,0.039,0.056,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -12090000,0.98,-0.0077,-0.012,0.18,0.0098,0.00099,0.017,0.0028,-0.0029,0.00085,-0.001,-0.0058,-0.00012,-0.00051,0.00017,-0.14,0.2,-2.4e-06,0.43,-4.3e-05,-6.6e-05,-0.00025,0,0,0.095,0.0011,0.00088,0.039,0.064,0.079,0.029,0.054,0.056,0.06,9.9e-06,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12190000,0.98,-0.0077,-0.012,0.18,0.011,-0.00018,0.016,0.0025,-0.0017,0.0027,-0.001,-0.0058,-0.00012,-0.0012,-0.00048,-0.14,0.2,-2.3e-06,0.43,-7.1e-06,-3.1e-05,-0.00028,0,0,0.095,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12290000,0.98,-0.0078,-0.012,0.18,0.0081,-0.0037,0.015,0.0032,-0.0024,0.0036,-0.001,-0.0058,-0.00013,-0.0015,-0.0003,-0.14,0.2,-2.1e-06,0.43,2e-05,-3.9e-05,-0.00028,0,0,0.095,0.00094,0.00081,0.039,0.062,0.075,0.024,0.053,0.055,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12390000,0.98,-0.0079,-0.012,0.18,0.008,-0.0049,0.013,0.0027,-0.0021,-0.0024,-0.001,-0.0058,-0.00013,-0.0018,-0.00078,-0.14,0.2,-2e-06,0.43,4.7e-05,-1.8e-05,-0.0003,0,0,0.095,0.00084,0.00075,0.039,0.053,0.062,0.022,0.046,0.047,0.056,6.8e-06,5.5e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12490000,0.98,-0.008,-0.012,0.18,0.0096,-0.0064,0.017,0.0041,-0.0032,-0.00046,-0.00099,-0.0058,-0.00013,-0.0021,-0.001,-0.14,0.2,-1.9e-06,0.43,5.5e-05,1.2e-05,-0.00031,0,0,0.095,0.00085,0.00074,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.7e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12590000,0.98,-0.008,-0.012,0.18,0.012,-0.01,0.018,0.0043,-0.0042,0.0013,-0.00098,-0.0058,-0.00013,-0.0023,-0.0009,-0.14,0.2,-1.8e-06,0.43,6.9e-05,1.2e-05,-0.00032,0,0,0.095,0.00077,0.00069,0.039,0.051,0.059,0.019,0.046,0.047,0.054,5.7e-06,4.7e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12690000,0.98,-0.008,-0.012,0.18,0.012,-0.014,0.018,0.0054,-0.0056,0.0028,-0.00097,-0.0058,-0.00013,-0.0024,-0.00085,-0.14,0.2,-1.8e-06,0.43,7.9e-05,1.7e-05,-0.00033,0,0,0.095,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.054,0.054,5.6e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.019,0.0054,-0.0062,0.005,-0.00098,-0.0058,-0.00013,-0.0018,-0.0013,-0.14,0.2,-1.8e-06,0.43,4.4e-05,4e-05,-0.00031,0,0,0.095,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12890000,0.98,-0.008,-0.012,0.18,0.016,-0.013,0.021,0.0071,-0.007,0.0079,-0.00099,-0.0058,-0.00012,-0.0014,-0.0015,-0.14,0.2,-1.9e-06,0.43,2.4e-05,4.5e-05,-0.00029,0,0,0.095,0.0007,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0.005,-0.0047,0.0091,-0.001,-0.0058,-0.00012,-0.0013,-0.0016,-0.14,0.2,-2.4e-06,0.43,3.8e-05,2e-05,-0.00032,0,0,0.095,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0.0067,-0.0048,0.0078,-0.001,-0.0058,-0.00012,-0.00076,-0.0018,-0.14,0.2,-2.4e-06,0.43,1.5e-05,2.5e-05,-0.0003,0,0,0.095,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.4e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13190000,0.98,-0.0079,-0.012,0.18,0.0097,-0.0078,0.017,0.003,-0.0048,0.0084,-0.0011,-0.0058,-0.00012,-0.00054,-0.0023,-0.14,0.2,-2.6e-06,0.43,3.9e-05,2.3e-05,-0.00031,0,0,0.095,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0.0046,-0.0059,0.0076,-0.001,-0.0058,-0.00012,-0.00076,-0.0028,-0.14,0.2,-2.4e-06,0.43,4.8e-05,4.4e-05,-0.00031,0,0,0.095,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.5e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13390000,0.98,-0.0079,-0.012,0.18,0.0091,-0.0081,0.014,0.0032,-0.0049,0.0081,-0.0011,-0.0058,-0.00012,-0.0014,-0.0028,-0.14,0.2,-2.7e-06,0.43,8.6e-05,4.1e-05,-0.00035,0,0,0.095,0.00057,0.00055,0.038,0.042,0.047,0.012,0.045,0.046,0.047,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13490000,0.98,-0.0079,-0.012,0.18,0.011,-0.0068,0.014,0.0048,-0.0056,0.0041,-0.001,-0.0058,-0.00012,-0.0015,-0.0033,-0.14,0.2,-2.6e-06,0.43,8.9e-05,6.4e-05,-0.00035,0,0,0.095,0.00057,0.00055,0.038,0.047,0.052,0.011,0.052,0.053,0.046,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13590000,0.98,-0.0079,-0.012,0.18,0.014,-0.0071,0.015,0.0065,-0.0048,0.0025,-0.001,-0.0058,-0.00013,-0.002,-0.0034,-0.14,0.2,-2.6e-06,0.43,9.8e-05,5.7e-05,-0.00035,0,0,0.095,0.00054,0.00053,0.038,0.04,0.044,0.011,0.045,0.046,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13690000,0.98,-0.0078,-0.012,0.18,0.014,-0.0082,0.015,0.0077,-0.0052,0.0051,-0.0011,-0.0058,-0.00012,-0.0017,-0.0031,-0.14,0.2,-2.6e-06,0.43,8.5e-05,3.7e-05,-0.00033,0,0,0.095,0.00054,0.00053,0.038,0.044,0.049,0.011,0.052,0.053,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13790000,0.98,-0.0077,-0.012,0.18,0.019,-0.0044,0.015,0.0098,-0.0027,0.0044,-0.0011,-0.0058,-0.00013,-0.0022,-0.0026,-0.14,0.2,-2.8e-06,0.43,8.3e-05,1.5e-05,-0.00034,0,0,0.095,0.00052,0.00051,0.038,0.038,0.042,0.01,0.045,0.046,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13890000,0.98,-0.0076,-0.012,0.18,0.019,-0.0029,0.016,0.011,-0.0025,0.0066,-0.0011,-0.0058,-0.00012,-0.0015,-0.002,-0.14,0.2,-3e-06,0.43,6.2e-05,1.1e-07,-0.00034,0,0,0.095,0.00052,0.00051,0.038,0.042,0.046,0.01,0.051,0.053,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13990000,0.98,-0.0076,-0.012,0.18,0.019,-0.001,0.014,0.0088,-0.003,0.0053,-0.0011,-0.0058,-0.00012,-0.001,-0.0025,-0.14,0.2,-2.9e-06,0.43,7e-05,-1.1e-06,-0.00031,0,0,0.095,0.0005,0.0005,0.038,0.036,0.039,0.0097,0.045,0.046,0.043,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -14090000,0.98,-0.0077,-0.011,0.18,0.017,-0.0082,0.015,0.011,-0.0049,0.0015,-0.0011,-0.0058,-0.00013,-0.0028,-0.0023,-0.14,0.2,-2.8e-06,0.43,0.00012,6.9e-06,-0.00035,0,0,0.095,0.0005,0.0005,0.038,0.04,0.043,0.0096,0.051,0.053,0.042,2.2e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14190000,0.98,-0.0078,-0.011,0.18,0.016,-0.0075,0.015,0.01,-0.0045,0.0015,-0.001,-0.0058,-0.00014,-0.0039,-0.0034,-0.14,0.2,-2.6e-06,0.43,0.00018,3.2e-05,-0.00037,0,0,0.095,0.00048,0.00048,0.038,0.034,0.037,0.0094,0.045,0.046,0.042,2.1e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14290000,0.98,-0.0077,-0.011,0.18,0.018,-0.0081,0.013,0.012,-0.0054,0.0057,-0.001,-0.0058,-0.00014,-0.004,-0.0036,-0.14,0.2,-2.6e-06,0.43,0.00018,3.6e-05,-0.00036,0,0,0.095,0.00048,0.00048,0.038,0.038,0.041,0.0092,0.051,0.052,0.041,2e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14390000,0.98,-0.0078,-0.011,0.18,0.018,-0.0095,0.015,0.011,-0.0055,0.01,-0.001,-0.0058,-0.00014,-0.0038,-0.0044,-0.14,0.2,-2.4e-06,0.43,0.00019,4.1e-05,-0.00035,0,0,0.095,0.00047,0.00047,0.038,0.033,0.035,0.009,0.045,0.046,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14490000,0.98,-0.008,-0.011,0.18,0.018,-0.011,0.019,0.013,-0.0073,0.012,-0.001,-0.0058,-0.00014,-0.0049,-0.0045,-0.14,0.2,-2.3e-06,0.43,0.00022,5e-05,-0.00037,0,0,0.095,0.00047,0.00047,0.038,0.036,0.038,0.009,0.051,0.052,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14590000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0.0099,-0.0073,0.008,-0.001,-0.0058,-0.00015,-0.0054,-0.0058,-0.14,0.2,-2.1e-06,0.43,0.00027,6.9e-05,-0.00037,0,0,0.095,0.00045,0.00046,0.038,0.031,0.033,0.0088,0.045,0.045,0.04,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14690000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0.012,-0.0083,0.0081,-0.001,-0.0058,-0.00014,-0.0053,-0.0062,-0.14,0.2,-2e-06,0.43,0.00027,7.5e-05,-0.00037,0,0,0.095,0.00045,0.00046,0.038,0.034,0.036,0.0088,0.05,0.051,0.039,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14790000,0.98,-0.0082,-0.011,0.18,0.019,-0.0034,0.016,0.01,-0.0022,0.011,-0.0011,-0.0058,-0.00014,-0.0047,-0.0076,-0.14,0.2,-2.2e-06,0.43,0.00027,7.3e-05,-0.00035,0,0,0.095,0.00044,0.00045,0.038,0.03,0.031,0.0086,0.044,0.045,0.039,1.7e-06,1.5e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14890000,0.98,-0.0082,-0.011,0.18,0.021,-0.0055,0.02,0.013,-0.0023,0.012,-0.0011,-0.0058,-0.00013,-0.0042,-0.0093,-0.14,0.2,-1.8e-06,0.43,0.00027,9.5e-05,-0.00031,0,0,0.095,0.00044,0.00045,0.038,0.032,0.034,0.0087,0.05,0.051,0.039,1.6e-06,1.5e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -14990000,0.98,-0.0083,-0.011,0.18,0.02,-0.0057,0.023,0.012,-0.0029,0.014,-0.0011,-0.0058,-0.00013,-0.0041,-0.011,-0.14,0.2,-1.5e-06,0.43,0.0003,0.00012,-0.00029,0,0,0.095,0.00043,0.00045,0.038,0.028,0.03,0.0085,0.044,0.045,0.038,1.6e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15090000,0.98,-0.0083,-0.011,0.18,0.021,-0.0049,0.026,0.014,-0.0036,0.016,-0.0011,-0.0058,-0.00013,-0.0042,-0.011,-0.14,0.2,-1.5e-06,0.43,0.0003,0.00013,-0.00029,0,0,0.095,0.00043,0.00045,0.038,0.03,0.032,0.0085,0.05,0.051,0.038,1.5e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15190000,0.98,-0.0085,-0.011,0.18,0.019,-0.0059,0.027,0.012,-0.0032,0.017,-0.0011,-0.0058,-0.00014,-0.0048,-0.012,-0.14,0.2,-1.4e-06,0.43,0.00034,0.00014,-0.00029,0,0,0.095,0.00043,0.00044,0.038,0.027,0.028,0.0085,0.044,0.045,0.038,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15290000,0.98,-0.0086,-0.012,0.18,0.022,-0.0065,0.026,0.015,-0.0034,0.014,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.2e-06,0.43,0.00033,0.00017,-0.00027,0,0,0.095,0.00043,0.00044,0.038,0.029,0.031,0.0085,0.049,0.05,0.037,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15390000,0.98,-0.0088,-0.011,0.18,0.021,-0.0039,0.025,0.012,-0.0028,0.014,-0.0011,-0.0058,-0.00013,-0.0045,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,0.095,0.00042,0.00043,0.038,0.025,0.027,0.0084,0.044,0.044,0.037,1.4e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15490000,0.98,-0.0088,-0.011,0.18,0.022,-0.0067,0.025,0.014,-0.0034,0.015,-0.0011,-0.0058,-0.00013,-0.0046,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,0.095,0.00042,0.00043,0.038,0.028,0.029,0.0085,0.049,0.05,0.037,1.4e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15590000,0.98,-0.0088,-0.011,0.18,0.023,-0.0081,0.025,0.014,-0.0059,0.013,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00036,0.00019,-0.00028,0,0,0.095,0.00041,0.00043,0.038,0.024,0.026,0.0084,0.043,0.044,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15690000,0.98,-0.0086,-0.011,0.18,0.024,-0.01,0.025,0.015,-0.0063,0.014,-0.0011,-0.0058,-0.00012,-0.0032,-0.012,-0.14,0.2,-1.8e-06,0.43,0.00031,0.00013,-0.00026,0,0,0.095,0.00041,0.00043,0.038,0.026,0.028,0.0085,0.049,0.049,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15790000,0.98,-0.0086,-0.011,0.18,0.019,-0.0094,0.025,0.012,-0.0047,0.015,-0.0011,-0.0058,-0.00012,-0.0021,-0.012,-0.14,0.2,-2.2e-06,0.43,0.0003,0.00012,-0.00026,0,0,0.095,0.00041,0.00042,0.038,0.023,0.025,0.0084,0.043,0.044,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15890000,0.98,-0.0087,-0.011,0.18,0.02,-0.0085,0.025,0.015,-0.0061,0.014,-0.0011,-0.0058,-0.00012,-0.0031,-0.012,-0.14,0.2,-1.9e-06,0.43,0.00034,0.00014,-0.00027,0,0,0.095,0.00041,0.00042,0.038,0.025,0.026,0.0085,0.048,0.049,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15990000,0.98,-0.0086,-0.011,0.18,0.018,-0.0074,0.022,0.013,-0.0051,0.013,-0.0011,-0.0058,-0.00012,-0.0034,-0.014,-0.14,0.2,-2.1e-06,0.43,0.00038,0.00019,-0.00029,0,0,0.095,0.0004,0.00042,0.038,0.022,0.023,0.0084,0.043,0.043,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -16090000,0.98,-0.0087,-0.011,0.18,0.019,-0.0092,0.02,0.015,-0.0063,0.013,-0.0011,-0.0058,-0.00013,-0.004,-0.014,-0.14,0.2,-2e-06,0.43,0.00041,0.00021,-0.00031,0,0,0.095,0.0004,0.00042,0.038,0.024,0.025,0.0085,0.048,0.049,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16190000,0.98,-0.0086,-0.011,0.18,0.015,-0.0076,0.018,0.011,-0.0054,0.0094,-0.0011,-0.0058,-0.00013,-0.0047,-0.015,-0.13,0.2,-2e-06,0.43,0.00044,0.00021,-0.00033,0,0,0.095,0.0004,0.00041,0.038,0.021,0.022,0.0084,0.043,0.043,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16290000,0.98,-0.0086,-0.011,0.18,0.017,-0.009,0.018,0.013,-0.0061,0.011,-0.0011,-0.0058,-0.00013,-0.0044,-0.014,-0.13,0.2,-2e-06,0.43,0.00043,0.0002,-0.00032,0,0,0.095,0.0004,0.00041,0.038,0.023,0.024,0.0085,0.047,0.048,0.036,1.2e-06,1e-06,2e-06,0.035,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16390000,0.98,-0.0087,-0.011,0.18,0.019,-0.0081,0.018,0.013,-0.0048,0.011,-0.0011,-0.0058,-0.00013,-0.004,-0.017,-0.13,0.2,-1.7e-06,0.43,0.00045,0.00024,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.021,0.022,0.0084,0.042,0.043,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16490000,0.98,-0.0089,-0.011,0.18,0.022,-0.01,0.021,0.015,-0.0059,0.015,-0.0011,-0.0058,-0.00013,-0.0044,-0.017,-0.13,0.2,-1.5e-06,0.43,0.00046,0.00025,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.022,0.023,0.0085,0.047,0.048,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16590000,0.98,-0.0089,-0.011,0.18,0.026,-0.01,0.024,0.013,-0.0049,0.015,-0.0011,-0.0058,-0.00013,-0.0043,-0.018,-0.13,0.2,-1.7e-06,0.43,0.00048,0.00025,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.02,0.021,0.0084,0.042,0.042,0.036,1.1e-06,9.7e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16690000,0.98,-0.0089,-0.011,0.18,0.028,-0.015,0.024,0.016,-0.0059,0.015,-0.0011,-0.0058,-0.00013,-0.0037,-0.017,-0.13,0.2,-1.9e-06,0.43,0.00046,0.00024,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.021,0.022,0.0085,0.047,0.047,0.036,1.1e-06,9.6e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16790000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.023,0.013,-0.0048,0.015,-0.0011,-0.0058,-0.00012,-0.0031,-0.017,-0.13,0.2,-2.2e-06,0.43,0.00046,0.00024,-0.00029,0,0,0.095,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.042,0.042,0.036,1e-06,9.3e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16890000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.024,0.016,-0.0056,0.013,-0.0011,-0.0058,-0.00012,-0.0021,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00043,0.00023,-0.00028,0,0,0.095,0.00038,0.0004,0.038,0.02,0.022,0.0085,0.046,0.047,0.036,1e-06,9.2e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -16990000,0.98,-0.0087,-0.011,0.18,0.026,-0.014,0.024,0.014,-0.0046,0.012,-0.0011,-0.0058,-0.00011,-0.0017,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00044,0.00025,-0.00028,0,0,0.095,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,1e-06,9e-07,2e-06,0.035,0.032,0.00042,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17090000,0.98,-0.0089,-0.011,0.18,0.03,-0.017,0.023,0.018,-0.0063,0.011,-0.0011,-0.0058,-0.00012,-0.0022,-0.019,-0.13,0.2,-2.4e-06,0.43,0.00048,0.00032,-0.00029,0,0,0.095,0.00038,0.0004,0.038,0.02,0.021,0.0085,0.046,0.046,0.035,9.9e-07,8.9e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17190000,0.98,-0.009,-0.011,0.18,0.028,-0.021,0.025,0.014,-0.0096,0.014,-0.0011,-0.0058,-0.00012,-0.0031,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00052,0.00033,-0.00033,0,0,0.095,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,9.6e-07,8.7e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17290000,0.98,-0.009,-0.011,0.18,0.031,-0.023,0.025,0.017,-0.012,0.014,-0.0011,-0.0058,-0.00013,-0.0038,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00055,0.00034,-0.00035,0,0,0.095,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.045,0.046,0.036,9.6e-07,8.6e-07,2e-06,0.035,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17390000,0.98,-0.0089,-0.011,0.18,0.025,-0.023,0.024,0.016,-0.009,0.013,-0.0011,-0.0058,-0.00012,-0.0025,-0.018,-0.13,0.2,-3.2e-06,0.43,0.00053,0.00034,-0.00034,0,0,0.095,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.041,0.041,0.035,9.3e-07,8.4e-07,2e-06,0.034,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17490000,0.98,-0.009,-0.011,0.18,0.024,-0.025,0.024,0.019,-0.012,0.015,-0.0011,-0.0058,-0.00013,-0.0028,-0.019,-0.13,0.2,-3.2e-06,0.43,0.00054,0.00034,-0.00035,0,0,0.095,0.00037,0.00039,0.038,0.018,0.019,0.0085,0.045,0.046,0.036,9.3e-07,8.3e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17590000,0.98,-0.0089,-0.011,0.18,0.021,-0.022,0.023,0.014,-0.0098,0.013,-0.0011,-0.0058,-0.00013,-0.0023,-0.019,-0.13,0.2,-3.8e-06,0.43,0.00055,0.00034,-0.00035,0,0,0.095,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.04,0.041,0.035,9e-07,8.1e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17690000,0.98,-0.009,-0.011,0.18,0.022,-0.023,0.024,0.016,-0.012,0.015,-0.0011,-0.0058,-0.00013,-0.002,-0.019,-0.13,0.2,-3.5e-06,0.43,0.00053,0.00032,-0.00033,0,0,0.095,0.00037,0.00039,0.038,0.018,0.019,0.0084,0.045,0.045,0.035,9e-07,8e-07,2e-06,0.034,0.031,0.00038,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17790000,0.98,-0.0091,-0.011,0.18,0.023,-0.022,0.025,0.015,-0.011,0.02,-0.0011,-0.0058,-0.00013,-0.0013,-0.019,-0.13,0.2,-3.5e-06,0.43,0.0005,0.00029,-0.00031,0,0,0.095,0.00037,0.00039,0.038,0.016,0.017,0.0084,0.04,0.041,0.036,8.8e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17890000,0.98,-0.0089,-0.011,0.18,0.026,-0.024,0.025,0.017,-0.013,0.025,-0.0011,-0.0058,-0.00012,-0.00072,-0.018,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.0003,0,0,0.095,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.044,0.045,0.036,8.7e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17990000,0.98,-0.0089,-0.011,0.18,0.025,-0.02,0.024,0.016,-0.0083,0.026,-0.0012,-0.0058,-0.00012,9.1e-05,-0.02,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.00029,0,0,0.095,0.00036,0.00038,0.038,0.016,0.017,0.0083,0.04,0.04,0.035,8.5e-07,7.6e-07,2e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -18090000,0.98,-0.009,-0.011,0.18,0.026,-0.021,0.024,0.018,-0.01,0.024,-0.0012,-0.0058,-0.00012,0.00014,-0.02,-0.13,0.2,-3.3e-06,0.43,0.00047,0.00027,-0.00028,0,0,0.095,0.00036,0.00038,0.038,0.017,0.018,0.0083,0.044,0.045,0.036,8.5e-07,7.5e-07,1.9e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18190000,0.98,-0.0091,-0.011,0.18,0.025,-0.019,0.024,0.017,-0.0083,0.022,-0.0012,-0.0058,-0.00011,0.0017,-0.021,-0.13,0.2,-3.6e-06,0.43,0.00045,0.00029,-0.00025,0,0,0.095,0.00036,0.00038,0.038,0.015,0.016,0.0082,0.04,0.04,0.035,8.2e-07,7.4e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18290000,0.98,-0.0092,-0.011,0.18,0.028,-0.02,0.023,0.02,-0.01,0.02,-0.0012,-0.0058,-0.00011,0.0019,-0.022,-0.13,0.2,-3.4e-06,0.43,0.00045,0.0003,-0.00024,0,0,0.095,0.00036,0.00038,0.038,0.016,0.018,0.0082,0.044,0.044,0.036,8.2e-07,7.3e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18390000,0.98,-0.0091,-0.011,0.18,0.027,-0.019,0.023,0.019,-0.0089,0.019,-0.0012,-0.0058,-0.00011,0.0021,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00046,0.00031,-0.00025,0,0,0.095,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18490000,0.98,-0.0091,-0.011,0.18,0.031,-0.019,0.022,0.022,-0.01,0.021,-0.0012,-0.0058,-0.00011,0.0027,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00044,0.00031,-0.00024,0,0,0.095,0.00036,0.00038,0.038,0.016,0.017,0.0082,0.043,0.044,0.036,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18590000,0.98,-0.0089,-0.011,0.18,0.028,-0.019,0.022,0.019,-0.0093,0.024,-0.0012,-0.0058,-0.00011,0.003,-0.021,-0.13,0.2,-4.2e-06,0.43,0.00044,0.00028,-0.00024,0,0,0.095,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,7.8e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18690000,0.98,-0.0088,-0.011,0.18,0.028,-0.018,0.021,0.021,-0.011,0.022,-0.0012,-0.0058,-0.00011,0.0039,-0.02,-0.13,0.2,-4.2e-06,0.43,0.0004,0.00025,-0.00022,0,0,0.095,0.00036,0.00038,0.038,0.016,0.017,0.0081,0.043,0.044,0.035,7.7e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18790000,0.98,-0.0087,-0.011,0.18,0.026,-0.017,0.02,0.019,-0.0093,0.02,-0.0012,-0.0058,-0.00011,0.0044,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00041,0.00026,-0.00023,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.008,0.039,0.04,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00032,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18890000,0.98,-0.0087,-0.011,0.18,0.027,-0.018,0.018,0.022,-0.011,0.016,-0.0012,-0.0058,-0.00011,0.0042,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00042,0.00028,-0.00024,0,0,0.095,0.00035,0.00037,0.038,0.015,0.017,0.008,0.043,0.043,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -18990000,0.98,-0.0087,-0.011,0.18,0.024,-0.018,0.019,0.019,-0.0097,0.02,-0.0012,-0.0058,-0.00011,0.005,-0.019,-0.13,0.2,-5.3e-06,0.43,0.00042,0.00027,-0.00024,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19090000,0.98,-0.0087,-0.011,0.18,0.023,-0.018,0.02,0.022,-0.011,0.016,-0.0012,-0.0058,-0.0001,0.0055,-0.019,-0.13,0.2,-5.2e-06,0.43,0.00041,0.00027,-0.00023,0,0,0.095,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19190000,0.98,-0.0086,-0.011,0.18,0.02,-0.018,0.02,0.019,-0.01,0.016,-0.0012,-0.0058,-0.00011,0.0053,-0.019,-0.13,0.2,-6e-06,0.43,0.00043,0.00028,-0.00026,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19290000,0.98,-0.0086,-0.011,0.18,0.021,-0.019,0.02,0.021,-0.012,0.015,-0.0012,-0.0058,-0.00011,0.0053,-0.019,-0.13,0.2,-6e-06,0.43,0.00043,0.00029,-0.00026,0,0,0.095,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.016,0.022,0.018,-0.0097,0.014,-0.0012,-0.0058,-0.00011,0.0059,-0.019,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00028,-0.00026,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19490000,0.98,-0.0088,-0.011,0.18,0.021,-0.017,0.021,0.021,-0.012,0.014,-0.0012,-0.0058,-0.00012,0.0053,-0.02,-0.13,0.2,-6e-06,0.43,0.00043,0.00026,-0.00025,0,0,0.095,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19590000,0.98,-0.0087,-0.011,0.18,0.018,-0.019,0.023,0.019,-0.011,0.014,-0.0012,-0.0058,-0.00012,0.0055,-0.02,-0.13,0.2,-6.3e-06,0.43,0.00043,0.00026,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19690000,0.98,-0.0088,-0.011,0.18,0.018,-0.017,0.022,0.022,-0.013,0.014,-0.0012,-0.0058,-0.00011,0.006,-0.021,-0.13,0.2,-6e-06,0.43,0.00042,0.00026,-0.00024,0,0,0.095,0.00034,0.00036,0.038,0.014,0.016,0.0076,0.042,0.042,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19790000,0.98,-0.0089,-0.011,0.18,0.016,-0.016,0.02,0.021,-0.011,0.01,-0.0012,-0.0058,-0.00012,0.0061,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.013,0.014,0.0076,0.038,0.039,0.035,6.6e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19890000,0.98,-0.0089,-0.011,0.18,0.017,-0.016,0.021,0.023,-0.013,0.0089,-0.0012,-0.0058,-0.00012,0.006,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00026,0,0,0.095,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.5e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19990000,0.98,-0.0089,-0.012,0.18,0.015,-0.016,0.018,0.02,-0.01,0.006,-0.0012,-0.0058,-0.00012,0.007,-0.021,-0.13,0.2,-7e-06,0.43,0.00042,0.00027,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.038,0.038,0.035,6.4e-07,5.7e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -20090000,0.98,-0.0089,-0.012,0.18,0.017,-0.019,0.019,0.022,-0.012,0.0095,-0.0012,-0.0058,-0.00012,0.0071,-0.021,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00028,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.4e-07,5.6e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5.1 -20190000,0.98,-0.0089,-0.012,0.18,0.017,-0.016,0.02,0.022,-0.011,0.0094,-0.0012,-0.0058,-0.00013,0.0071,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00042,0.00029,-0.00027,0,0,0.11,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.037,0.038,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20290000,0.98,-0.009,-0.012,0.18,0.015,-0.018,0.02,0.023,-0.012,0.01,-0.0012,-0.0058,-0.00012,0.0071,-0.022,-0.13,0.2,-7.5e-06,0.43,0.00042,0.0003,-0.00027,0,0,0.11,0.00034,0.00036,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20390000,0.98,-0.0089,-0.012,0.18,0.013,-0.016,0.02,0.022,-0.011,0.012,-0.0012,-0.0058,-0.00012,0.0081,-0.022,-0.13,0.2,-7.6e-06,0.43,0.00041,0.00028,-0.00026,0,0,0.11,0.00033,0.00035,0.038,0.013,0.014,0.0073,0.037,0.038,0.035,6.1e-07,5.4e-07,1.8e-06,0.031,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20490000,0.98,-0.0089,-0.012,0.18,0.0096,-0.017,0.02,0.023,-0.012,0.0099,-0.0012,-0.0058,-0.00012,0.0082,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00041,0.00027,-0.00026,0,0,0.11,0.00034,0.00035,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6e-07,5.3e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20590000,0.98,-0.0088,-0.012,0.18,0.0088,-0.017,0.02,0.022,-0.01,0.0085,-0.0013,-0.0058,-0.00011,0.0094,-0.022,-0.13,0.2,-7.3e-06,0.43,0.00039,0.00025,-0.00025,0,0,0.11,0.00033,0.00035,0.038,0.013,0.014,0.0072,0.037,0.038,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20690000,0.98,-0.0087,-0.012,0.18,0.0077,-0.017,0.021,0.022,-0.012,0.0092,-0.0013,-0.0058,-0.00012,0.0092,-0.022,-0.13,0.2,-7.5e-06,0.43,0.00039,0.00025,-0.00025,0,0,0.11,0.00033,0.00035,0.038,0.013,0.015,0.0072,0.04,0.042,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20790000,0.98,-0.0081,-0.012,0.18,0.0038,-0.013,0.0061,0.019,-0.01,0.008,-0.0013,-0.0058,-0.00011,0.01,-0.022,-0.13,0.2,-7.9e-06,0.43,0.00038,0.00025,-0.00025,0,0,0.11,0.00033,0.00035,0.038,0.013,0.014,0.0071,0.037,0.038,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20890000,0.98,0.00098,-0.0077,0.18,-4.9e-05,-0.002,-0.11,0.02,-0.011,0.002,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00037,-0.00019,0,0,0.1,0.00033,0.00035,0.038,0.013,0.015,0.0071,0.04,0.041,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20990000,0.98,0.0044,-0.0043,0.18,-0.012,0.017,-0.25,0.017,-0.0086,-0.013,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00036,-0.00015,0,0,0.087,0.00033,0.00035,0.038,0.013,0.014,0.007,0.037,0.038,0.034,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21090000,0.98,0.0027,-0.0047,0.18,-0.024,0.032,-0.37,0.016,-0.006,-0.043,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-7e-06,0.43,0.00042,0.00016,-0.00019,0,0,0.057,0.00033,0.00035,0.038,0.014,0.015,0.007,0.04,0.041,0.035,5.6e-07,4.9e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21190000,0.98,-6.1e-05,-0.0063,0.18,-0.029,0.039,-0.5,0.013,-0.0039,-0.08,-0.0013,-0.0058,-0.0001,0.01,-0.024,-0.13,0.2,-6.3e-06,0.43,0.00043,0.00019,-0.00018,0,0,0.02,0.00033,0.00034,0.038,0.013,0.014,0.0069,0.037,0.038,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21290000,0.98,-0.0023,-0.0076,0.18,-0.029,0.041,-0.63,0.0099,-0.0003,-0.14,-0.0013,-0.0058,-0.00011,0.0096,-0.024,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00023,-0.00011,0,0,-0.038,0.00033,0.00034,0.037,0.014,0.015,0.0069,0.04,0.041,0.034,5.4e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21390000,0.98,-0.0037,-0.0082,0.18,-0.026,0.038,-0.75,0.0082,0.0037,-0.2,-0.0013,-0.0058,-0.00011,0.0097,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00044,0.00015,-0.00012,0,0,-0.1,0.00032,0.00034,0.037,0.013,0.014,0.0068,0.037,0.038,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21490000,0.98,-0.0045,-0.0086,0.18,-0.021,0.035,-0.89,0.0054,0.0077,-0.29,-0.0013,-0.0058,-0.0001,0.01,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00018,-0.00017,0,0,-0.19,0.00032,0.00034,0.037,0.014,0.016,0.0068,0.04,0.041,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21590000,0.98,-0.0049,-0.0086,0.18,-0.015,0.032,-1,0.0044,0.0099,-0.38,-0.0013,-0.0058,-9.5e-05,0.011,-0.024,-0.13,0.2,-6.5e-06,0.43,0.00042,0.00023,-0.00018,0,0,-0.28,0.00032,0.00034,0.037,0.013,0.015,0.0067,0.037,0.038,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21690000,0.98,-0.0052,-0.0085,0.18,-0.012,0.027,-1.1,0.0032,0.013,-0.49,-0.0013,-0.0058,-8.8e-05,0.011,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00027,-0.00016,0,0,-0.39,0.00032,0.00034,0.037,0.014,0.016,0.0067,0.04,0.041,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21790000,0.98,-0.0055,-0.0086,0.18,-0.0065,0.022,-1.3,0.007,0.012,-0.61,-0.0013,-0.0058,-7.8e-05,0.012,-0.023,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00027,-0.00022,0,0,-0.51,0.00032,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21890000,0.98,-0.0058,-0.0088,0.18,-0.0029,0.017,-1.4,0.0062,0.014,-0.75,-0.0013,-0.0058,-8.1e-05,0.012,-0.023,-0.13,0.2,-6.8e-06,0.43,0.00036,0.00029,-0.00021,0,0,-0.65,0.00032,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21990000,0.98,-0.0064,-0.009,0.18,-0.00083,0.013,-1.4,0.01,0.011,-0.89,-0.0013,-0.0058,-7.5e-05,0.012,-0.022,-0.13,0.2,-6.5e-06,0.43,0.00039,0.0003,-0.00023,0,0,-0.79,0.00031,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22090000,0.98,-0.0071,-0.0099,0.18,0.0012,0.0095,-1.4,0.0095,0.013,-1,-0.0013,-0.0058,-6.6e-05,0.012,-0.022,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00031,-0.00024,0,0,-0.93,0.00031,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22190000,0.98,-0.0075,-0.01,0.18,0.007,0.0051,-1.4,0.015,0.008,-1.2,-0.0013,-0.0058,-5.8e-05,0.013,-0.02,-0.13,0.2,-6.6e-06,0.43,0.00039,0.00033,-0.00024,0,0,-1.1,0.00031,0.00032,0.037,0.013,0.014,0.0065,0.037,0.038,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22290000,0.98,-0.0082,-0.01,0.18,0.013,-0.00054,-1.4,0.016,0.0081,-1.3,-0.0013,-0.0058,-6e-05,0.013,-0.021,-0.13,0.2,-6.5e-06,0.43,0.00039,0.00033,-0.00023,0,0,-1.2,0.00031,0.00032,0.037,0.014,0.015,0.0065,0.04,0.041,0.034,4.8e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22390000,0.98,-0.0085,-0.01,0.18,0.017,-0.0091,-1.4,0.022,-0.00072,-1.5,-0.0013,-0.0058,-6.3e-05,0.012,-0.02,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00033,-0.00026,0,0,-1.4,0.0003,0.00032,0.037,0.013,0.014,0.0064,0.037,0.038,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22490000,0.98,-0.0087,-0.011,0.18,0.021,-0.016,-1.4,0.024,-0.0023,-1.6,-0.0013,-0.0058,-6.7e-05,0.011,-0.02,-0.13,0.2,-6.9e-06,0.43,0.00041,0.00035,-0.00028,0,0,-1.5,0.00031,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.7e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22590000,0.98,-0.0086,-0.012,0.18,0.029,-0.024,-1.4,0.034,-0.01,-1.7,-0.0013,-0.0058,-5.8e-05,0.012,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00042,0.00037,-0.00027,0,0,-1.6,0.0003,0.00032,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22690000,0.98,-0.0085,-0.012,0.18,0.032,-0.028,-1.4,0.038,-0.013,-1.9,-0.0013,-0.0058,-6.1e-05,0.012,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00041,0.00037,-0.00028,0,0,-1.8,0.0003,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22790000,0.98,-0.0085,-0.012,0.18,0.038,-0.036,-1.4,0.048,-0.022,-2,-0.0013,-0.0058,-6.4e-05,0.011,-0.021,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00034,-0.00031,0,0,-1.9,0.0003,0.00031,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22890000,0.98,-0.0087,-0.012,0.18,0.042,-0.041,-1.4,0.052,-0.026,-2.2,-0.0013,-0.0058,-5.6e-05,0.011,-0.021,-0.13,0.2,-6.3e-06,0.43,0.00041,0.00035,-0.00029,0,0,-2.1,0.0003,0.00031,0.037,0.013,0.015,0.0063,0.04,0.041,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22990000,0.98,-0.0086,-0.013,0.18,0.046,-0.045,-1.4,0.061,-0.035,-2.3,-0.0013,-0.0058,-4.7e-05,0.012,-0.02,-0.13,0.2,-6.7e-06,0.43,0.0004,0.00032,-0.00026,0,0,-2.2,0.0003,0.00031,0.037,0.012,0.014,0.0062,0.036,0.038,0.033,4.3e-07,3.9e-07,1.6e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23090000,0.98,-0.0086,-0.013,0.18,0.051,-0.05,-1.4,0.065,-0.04,-2.5,-0.0013,-0.0058,-4.6e-05,0.012,-0.02,-0.13,0.2,-6.7e-06,0.43,0.00039,0.00033,-0.00025,0,0,-2.4,0.0003,0.00031,0.037,0.013,0.014,0.0062,0.04,0.041,0.033,4.3e-07,3.9e-07,1.5e-06,0.028,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23190000,0.98,-0.0086,-0.013,0.18,0.057,-0.052,-1.4,0.076,-0.05,-2.6,-0.0013,-0.0058,-4.9e-05,0.012,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00037,0.0003,-0.00025,0,0,-2.5,0.00029,0.00031,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23290000,0.98,-0.0091,-0.014,0.18,0.062,-0.057,-1.4,0.082,-0.055,-2.8,-0.0013,-0.0058,-4.6e-05,0.012,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00034,0.00034,-0.00025,0,0,-2.7,0.00029,0.00031,0.037,0.013,0.014,0.0062,0.039,0.041,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23390000,0.98,-0.009,-0.014,0.18,0.067,-0.06,-1.4,0.092,-0.061,-2.9,-0.0013,-0.0058,-5.9e-05,0.012,-0.019,-0.13,0.2,-7.7e-06,0.43,0.00035,0.00031,-0.00023,0,0,-2.8,0.00029,0.0003,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23490000,0.98,-0.0091,-0.014,0.18,0.072,-0.062,-1.4,0.1,-0.067,-3,-0.0013,-0.0058,-5.1e-05,0.012,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00033,0.00037,-0.00028,0,0,-2.9,0.00029,0.0003,0.037,0.013,0.014,0.0061,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23590000,0.98,-0.0093,-0.014,0.18,0.075,-0.064,-1.4,0.11,-0.076,-3.2,-0.0013,-0.0058,-4.8e-05,0.013,-0.019,-0.13,0.2,-8.4e-06,0.43,0.00029,0.00033,-0.00024,0,0,-3.1,0.00029,0.0003,0.037,0.012,0.013,0.006,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23690000,0.98,-0.01,-0.015,0.18,0.074,-0.066,-1.3,0.11,-0.082,-3.3,-0.0013,-0.0058,-4e-05,0.013,-0.02,-0.13,0.2,-8.3e-06,0.43,0.00027,0.00036,-0.00024,0,0,-3.2,0.00029,0.0003,0.037,0.012,0.014,0.006,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23790000,0.98,-0.012,-0.017,0.18,0.068,-0.062,-0.95,0.12,-0.086,-3.4,-0.0013,-0.0058,-3.8e-05,0.014,-0.02,-0.13,0.2,-8e-06,0.43,0.00025,0.00039,-0.00022,0,0,-3.3,0.00029,0.0003,0.037,0.011,0.013,0.006,0.036,0.037,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23890000,0.98,-0.015,-0.021,0.18,0.064,-0.062,-0.52,0.13,-0.092,-3.5,-0.0013,-0.0058,-3.6e-05,0.014,-0.02,-0.13,0.2,-8e-06,0.43,0.00024,0.00041,-0.00024,0,0,-3.4,0.00029,0.0003,0.037,0.012,0.013,0.006,0.039,0.041,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23990000,0.98,-0.017,-0.024,0.18,0.064,-0.061,-0.13,0.14,-0.095,-3.6,-0.0013,-0.0058,-4.2e-05,0.015,-0.019,-0.13,0.2,-8.1e-06,0.43,0.0002,0.00043,3.8e-06,0,0,-3.5,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.033,3.9e-07,3.5e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24090000,0.98,-0.017,-0.023,0.18,0.07,-0.069,0.099,0.14,-0.1,-3.6,-0.0013,-0.0058,-4.5e-05,0.015,-0.019,-0.13,0.2,-8.1e-06,0.43,0.00022,0.0004,3.7e-06,0,0,-3.5,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.9e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24190000,0.98,-0.014,-0.019,0.18,0.08,-0.074,0.089,0.15,-0.1,-3.6,-0.0013,-0.0058,-4.5e-05,0.017,-0.019,-0.13,0.2,-7.7e-06,0.43,0.00022,0.00037,0.0001,0,0,-3.5,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.032,3.8e-07,3.5e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24290000,0.98,-0.012,-0.016,0.18,0.084,-0.078,0.067,0.16,-0.11,-3.6,-0.0013,-0.0058,-4.3e-05,0.017,-0.019,-0.13,0.2,-7.3e-06,0.43,0.00025,0.00032,9.8e-05,0,0,-3.5,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.8e-07,3.5e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24390000,0.98,-0.011,-0.015,0.18,0.077,-0.073,0.083,0.16,-0.11,-3.6,-0.0013,-0.0058,-4.2e-05,0.018,-0.02,-0.13,0.2,-6e-06,0.43,0.00024,0.00036,0.00016,0,0,-3.5,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24490000,0.98,-0.011,-0.015,0.18,0.073,-0.069,0.081,0.17,-0.12,-3.6,-0.0013,-0.0058,-2.8e-05,0.019,-0.021,-0.13,0.2,-6e-06,0.43,0.00017,0.00047,0.00021,0,0,-3.5,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24590000,0.98,-0.012,-0.015,0.18,0.069,-0.066,0.077,0.17,-0.12,-3.6,-0.0013,-0.0058,-3.9e-05,0.02,-0.021,-0.13,0.2,-4.7e-06,0.43,0.0002,0.00044,0.00023,0,0,-3.5,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24690000,0.98,-0.012,-0.015,0.18,0.068,-0.065,0.076,0.18,-0.12,-3.5,-0.0013,-0.0058,-3.8e-05,0.02,-0.021,-0.13,0.2,-4.9e-06,0.43,0.00019,0.00047,0.00022,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24790000,0.98,-0.012,-0.014,0.18,0.065,-0.063,0.068,0.18,-0.12,-3.5,-0.0014,-0.0058,-4.6e-05,0.022,-0.022,-0.13,0.2,-4.3e-06,0.43,0.00019,0.00047,0.00021,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24890000,0.98,-0.012,-0.014,0.18,0.063,-0.067,0.057,0.19,-0.13,-3.5,-0.0014,-0.0058,-4e-05,0.022,-0.022,-0.13,0.2,-4.1e-06,0.43,0.00018,0.00048,0.00024,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24990000,0.98,-0.012,-0.014,0.18,0.055,-0.063,0.05,0.19,-0.12,-3.5,-0.0014,-0.0058,-5.4e-05,0.024,-0.023,-0.13,0.2,-4.3e-06,0.43,0.00015,0.00059,0.00025,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25090000,0.98,-0.012,-0.014,0.18,0.051,-0.063,0.048,0.19,-0.12,-3.5,-0.0014,-0.0058,-5.4e-05,0.024,-0.023,-0.13,0.2,-4.9e-06,0.43,0.00013,0.00066,0.00029,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25190000,0.98,-0.012,-0.014,0.18,0.046,-0.057,0.048,0.19,-0.12,-3.5,-0.0014,-0.0058,-7.2e-05,0.025,-0.024,-0.13,0.2,-4.9e-06,0.43,0.00011,0.00069,0.00028,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25290000,0.98,-0.012,-0.013,0.18,0.041,-0.059,0.043,0.2,-0.12,-3.5,-0.0014,-0.0058,-7.9e-05,0.025,-0.023,-0.13,0.2,-5.5e-06,0.43,0.0001,0.00072,0.00025,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25390000,0.98,-0.012,-0.013,0.18,0.033,-0.052,0.041,0.19,-0.11,-3.5,-0.0014,-0.0058,-9.4e-05,0.027,-0.024,-0.13,0.2,-6.3e-06,0.43,6.6e-05,0.00077,0.00026,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25490000,0.98,-0.013,-0.013,0.18,0.028,-0.052,0.041,0.19,-0.12,-3.5,-0.0014,-0.0058,-9.6e-05,0.027,-0.025,-0.13,0.2,-6.1e-06,0.43,6e-05,0.00073,0.00024,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25590000,0.98,-0.013,-0.012,0.18,0.023,-0.049,0.042,0.19,-0.11,-3.5,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-6.9e-06,0.43,2.9e-05,0.00076,0.00021,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25690000,0.98,-0.012,-0.012,0.18,0.023,-0.048,0.031,0.19,-0.11,-3.5,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-7e-06,0.43,3.4e-05,0.00078,0.00022,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25790000,0.98,-0.012,-0.012,0.18,0.012,-0.04,0.031,0.18,-0.1,-3.5,-0.0014,-0.0058,-0.00012,0.029,-0.026,-0.13,0.2,-7.5e-06,0.43,-2.6e-05,0.00071,0.00021,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25890000,0.98,-0.012,-0.012,0.18,0.006,-0.039,0.033,0.18,-0.11,-3.5,-0.0014,-0.0058,-0.00013,0.029,-0.026,-0.13,0.2,-7.6e-06,0.43,-4.6e-05,0.00068,0.00016,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25990000,0.98,-0.012,-0.012,0.18,-0.003,-0.032,0.027,0.17,-0.097,-3.5,-0.0014,-0.0058,-0.00015,0.031,-0.026,-0.13,0.2,-9e-06,0.43,-0.0001,0.00066,0.00014,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26090000,0.98,-0.012,-0.012,0.18,-0.0076,-0.032,0.026,0.17,-0.1,-3.5,-0.0014,-0.0058,-0.00014,0.031,-0.026,-0.13,0.2,-8.5e-06,0.43,-9.8e-05,0.00065,0.00017,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.025,0.021,0.16,-0.092,-3.5,-0.0015,-0.0058,-0.00014,0.032,-0.026,-0.13,0.2,-8.9e-06,0.43,-0.00012,0.00065,0.00019,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.024,0.015,0.16,-0.095,-3.5,-0.0015,-0.0058,-0.00015,0.032,-0.027,-0.13,0.2,-9.5e-06,0.43,-0.00013,0.00066,0.00016,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26390000,0.98,-0.011,-0.013,0.18,-0.021,-0.017,0.019,0.15,-0.086,-3.5,-0.0015,-0.0058,-0.00016,0.033,-0.027,-0.13,0.2,-1.1e-05,0.43,-0.00017,0.00064,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26490000,0.98,-0.011,-0.013,0.18,-0.023,-0.015,0.028,0.14,-0.087,-3.5,-0.0015,-0.0058,-0.00016,0.033,-0.027,-0.13,0.2,-1.1e-05,0.43,-0.00017,0.00067,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26590000,0.98,-0.01,-0.013,0.18,-0.026,-0.0064,0.029,0.13,-0.079,-3.5,-0.0015,-0.0058,-0.00017,0.034,-0.028,-0.13,0.2,-1.2e-05,0.43,-0.0002,0.00069,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26690000,0.98,-0.01,-0.012,0.18,-0.027,-0.0024,0.027,0.13,-0.08,-3.5,-0.0015,-0.0058,-0.00018,0.033,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00021,0.0007,0.00012,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26790000,0.98,-0.0099,-0.012,0.18,-0.035,0.0019,0.027,0.12,-0.073,-3.5,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00024,0.00068,0.00011,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26890000,0.98,-0.0092,-0.012,0.18,-0.04,0.0048,0.022,0.11,-0.073,-3.5,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00024,0.00066,0.00011,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26990000,0.98,-0.0087,-0.013,0.18,-0.048,0.012,0.022,0.098,-0.065,-3.5,-0.0015,-0.0058,-0.00019,0.035,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00026,0.00065,0.00012,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27090000,0.98,-0.0085,-0.013,0.18,-0.05,0.018,0.025,0.093,-0.064,-3.5,-0.0015,-0.0058,-0.00019,0.035,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00025,0.00064,0.00012,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27190000,0.98,-0.0086,-0.013,0.18,-0.056,0.025,0.027,0.081,-0.056,-3.5,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00027,0.00063,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27290000,0.98,-0.0088,-0.014,0.18,-0.063,0.03,0.14,0.076,-0.054,-3.5,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00027,0.00063,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0054,0.038,0.039,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27390000,0.98,-0.01,-0.016,0.18,-0.071,0.037,0.46,0.064,-0.045,-3.5,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00033,0.00056,0.00018,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27490000,0.98,-0.012,-0.018,0.18,-0.074,0.042,0.78,0.057,-0.041,-3.5,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.13,0.2,-1.6e-05,0.43,-0.00031,0.00052,0.00018,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.013,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27590000,0.98,-0.011,-0.017,0.18,-0.07,0.046,0.87,0.046,-0.036,-3.4,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00029,0.00047,0.00021,0,0,-3.3,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27690000,0.98,-0.01,-0.014,0.18,-0.065,0.042,0.78,0.04,-0.031,-3.3,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00046,0.00019,0,0,-3.2,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27790000,0.98,-0.0088,-0.012,0.18,-0.065,0.04,0.77,0.032,-0.027,-3.2,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00042,0.0002,0,0,-3.1,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27890000,0.98,-0.0085,-0.013,0.18,-0.071,0.047,0.81,0.026,-0.023,-3.2,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00042,0.0002,0,0,-3.1,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27990000,0.98,-0.0089,-0.013,0.18,-0.072,0.049,0.8,0.019,-0.019,-3.1,-0.0015,-0.0058,-0.0002,0.034,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00027,0.00036,0.00022,0,0,-3,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28090000,0.98,-0.0092,-0.013,0.18,-0.076,0.05,0.8,0.011,-0.014,-3,-0.0015,-0.0058,-0.00019,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00038,0.00021,0,0,-2.9,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28190000,0.98,-0.0086,-0.013,0.18,-0.077,0.047,0.81,0.004,-0.012,-2.9,-0.0015,-0.0058,-0.00019,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00026,0.00037,0.00022,0,0,-2.8,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28290000,0.98,-0.0081,-0.013,0.18,-0.081,0.051,0.81,-0.0032,-0.0067,-2.9,-0.0015,-0.0058,-0.00018,0.034,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00039,0.00019,0,0,-2.8,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28390000,0.98,-0.0081,-0.014,0.18,-0.082,0.055,0.81,-0.0097,-0.0023,-2.8,-0.0014,-0.0058,-0.00017,0.033,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00032,0.0003,0.0002,0,0,-2.7,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28490000,0.98,-0.0084,-0.015,0.18,-0.085,0.058,0.81,-0.019,0.0032,-2.7,-0.0014,-0.0058,-0.00017,0.033,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00029,0.00029,0.00023,0,0,-2.6,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28590000,0.98,-0.0085,-0.014,0.18,-0.079,0.054,0.81,-0.023,0.0026,-2.6,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00026,0.00027,0.00022,0,0,-2.5,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28690000,0.98,-0.0082,-0.014,0.18,-0.078,0.055,0.81,-0.031,0.008,-2.6,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00023,0.00027,0.00022,0,0,-2.5,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.056,0.81,-0.035,0.011,-2.5,-0.0014,-0.0058,-0.00015,0.032,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00025,0.00017,0.00022,0,0,-2.4,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28890000,0.98,-0.0074,-0.013,0.18,-0.08,0.058,0.81,-0.043,0.017,-2.4,-0.0014,-0.0058,-0.00014,0.032,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00025,0.0002,0.00019,0,0,-2.3,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28990000,0.98,-0.0072,-0.013,0.18,-0.077,0.055,0.81,-0.044,0.017,-2.3,-0.0014,-0.0058,-0.00014,0.031,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00029,5.9e-05,0.00023,0,0,-2.2,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29090000,0.98,-0.007,-0.013,0.18,-0.08,0.057,0.81,-0.052,0.023,-2.3,-0.0014,-0.0058,-0.00014,0.031,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00031,4.1e-05,0.00023,0,0,-2.2,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.81,-0.052,0.023,-2.2,-0.0014,-0.0058,-0.00012,0.03,-0.028,-0.12,0.2,-2e-05,0.43,-0.00033,-8.9e-05,0.00024,0,0,-2.1,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,-0.061,0.029,-2.1,-0.0014,-0.0058,-0.00012,0.03,-0.028,-0.12,0.2,-2e-05,0.43,-0.00033,-0.00011,0.00024,0,0,-2,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29390000,0.98,-0.0076,-0.013,0.18,-0.078,0.061,0.81,-0.06,0.031,-2,-0.0014,-0.0058,-0.00011,0.029,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00036,-0.00024,0.00025,0,0,-1.9,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29490000,0.98,-0.0076,-0.013,0.18,-0.081,0.062,0.81,-0.069,0.038,-2,-0.0014,-0.0058,-0.0001,0.029,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00039,-0.00021,0.00023,0,0,-1.9,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,-0.067,0.038,-1.9,-0.0014,-0.0058,-8.7e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00043,-0.00031,0.00023,0,0,-1.8,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,-0.076,0.044,-1.8,-0.0014,-0.0058,-8e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00045,-0.00028,0.00021,0,0,-1.7,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,-0.072,0.043,-1.7,-0.0014,-0.0058,-6.4e-05,0.027,-0.028,-0.12,0.2,-2.5e-05,0.43,-0.00051,-0.0004,0.00021,0,0,-1.6,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29890000,0.98,-0.0068,-0.013,0.18,-0.08,0.056,0.8,-0.08,0.048,-1.7,-0.0014,-0.0058,-5.7e-05,0.028,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00053,-0.00037,0.00019,0,0,-1.6,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29990000,0.98,-0.007,-0.013,0.18,-0.075,0.051,0.8,-0.076,0.044,-1.6,-0.0014,-0.0058,-4.6e-05,0.027,-0.029,-0.12,0.2,-2.6e-05,0.43,-0.00051,-0.00042,0.00021,0,0,-1.5,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30090000,0.98,-0.0071,-0.013,0.18,-0.077,0.051,0.8,-0.084,0.049,-1.5,-0.0014,-0.0058,-5.6e-05,0.026,-0.028,-0.12,0.2,-2.7e-05,0.43,-0.00046,-0.00048,0.00026,0,0,-1.4,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30190000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,-0.078,0.047,-1.5,-0.0014,-0.0058,-5.9e-05,0.026,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00047,-0.0007,0.00032,0,0,-1.4,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30290000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,-0.085,0.052,-1.4,-0.0014,-0.0058,-5.7e-05,0.026,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00051,-0.00075,0.00032,0,0,-1.3,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30390000,0.98,-0.0071,-0.013,0.18,-0.065,0.043,0.8,-0.078,0.049,-1.3,-0.0014,-0.0058,-4e-05,0.025,-0.029,-0.12,0.2,-3e-05,0.43,-0.00065,-0.00096,0.00032,0,0,-1.2,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30490000,0.98,-0.0071,-0.013,0.18,-0.069,0.043,0.8,-0.085,0.054,-1.2,-0.0014,-0.0058,-3.4e-05,0.025,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00069,-0.00095,0.0003,0,0,-1.1,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30590000,0.98,-0.0074,-0.014,0.18,-0.064,0.041,0.8,-0.077,0.051,-1.2,-0.0014,-0.0058,-1.8e-05,0.025,-0.03,-0.12,0.2,-3e-05,0.43,-0.00079,-0.0011,0.0003,0,0,-1.1,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30690000,0.98,-0.0078,-0.014,0.18,-0.063,0.04,0.8,-0.084,0.055,-1.1,-0.0014,-0.0058,-1.8e-05,0.025,-0.03,-0.12,0.2,-3e-05,0.43,-0.00077,-0.001,0.00029,0,0,-0.99,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30790000,0.98,-0.0075,-0.013,0.17,-0.056,0.034,0.8,-0.076,0.053,-1,-0.0014,-0.0058,-1.4e-05,0.024,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00085,-0.0012,0.00033,0,0,-0.92,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30890000,0.98,-0.0069,-0.013,0.17,-0.056,0.031,0.79,-0.081,0.056,-0.95,-0.0014,-0.0058,-2.1e-05,0.024,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00075,-0.0011,0.00035,0,0,-0.85,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30990000,0.98,-0.0071,-0.013,0.17,-0.049,0.025,0.8,-0.071,0.049,-0.88,-0.0014,-0.0057,-1.3e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00079,-0.0013,0.00038,0,0,-0.78,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31090000,0.98,-0.0073,-0.013,0.17,-0.048,0.024,0.79,-0.075,0.052,-0.81,-0.0014,-0.0057,-1.8e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00074,-0.0012,0.00039,0,0,-0.71,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31190000,0.98,-0.0075,-0.013,0.17,-0.043,0.02,0.8,-0.066,0.047,-0.74,-0.0013,-0.0057,-1.7e-06,0.024,-0.033,-0.12,0.2,-3.4e-05,0.43,-0.00084,-0.0012,0.00038,0,0,-0.64,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.018,0.8,-0.07,0.049,-0.67,-0.0013,-0.0057,3.4e-06,0.024,-0.033,-0.12,0.2,-3.3e-05,0.43,-0.00087,-0.0012,0.00037,0,0,-0.57,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,-0.061,0.043,-0.59,-0.0013,-0.0057,2.7e-06,0.023,-0.033,-0.12,0.2,-3.3e-05,0.43,-0.00096,-0.0015,0.00042,0,0,-0.49,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0081,0.8,-0.065,0.044,-0.52,-0.0013,-0.0057,1e-06,0.023,-0.033,-0.12,0.2,-3.2e-05,0.43,-0.001,-0.0017,0.00043,0,0,-0.42,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.5e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31590000,0.98,-0.0071,-0.014,0.17,-0.033,0.0062,0.8,-0.054,0.04,-0.45,-0.0013,-0.0057,1.1e-05,0.023,-0.033,-0.12,0.2,-3.2e-05,0.43,-0.0011,-0.0018,0.00045,0,0,-0.35,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31690000,0.98,-0.0071,-0.014,0.17,-0.036,0.0056,0.8,-0.058,0.041,-0.38,-0.0013,-0.0057,1.9e-05,0.023,-0.033,-0.12,0.2,-3.1e-05,0.43,-0.0012,-0.0018,0.00043,0,0,-0.28,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31790000,0.98,-0.0074,-0.015,0.17,-0.027,0.003,0.8,-0.045,0.039,-0.3,-0.0013,-0.0057,2.4e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.0019,0.00045,0,0,-0.2,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31890000,0.99,-0.0071,-0.015,0.17,-0.025,0.00083,0.8,-0.049,0.039,-0.23,-0.0013,-0.0057,2.8e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.002,0.00047,0,0,-0.13,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31990000,0.99,-0.0074,-0.014,0.17,-0.017,-0.00026,0.79,-0.036,0.036,-0.17,-0.0013,-0.0057,2.5e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.0022,0.00052,0,0,-0.066,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32090000,0.99,-0.0077,-0.014,0.17,-0.019,-0.0037,0.8,-0.038,0.036,-0.094,-0.0013,-0.0057,2.5e-05,0.024,-0.034,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0023,0.00054,0,0,0.0058,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32190000,0.99,-0.008,-0.014,0.17,-0.013,-0.0073,0.8,-0.026,0.034,-0.026,-0.0013,-0.0057,2.1e-05,0.024,-0.034,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0024,0.00058,0,0,0.074,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32290000,0.99,-0.0079,-0.015,0.17,-0.013,-0.0098,0.8,-0.027,0.033,0.044,-0.0013,-0.0057,2.6e-05,0.025,-0.034,-0.12,0.2,-2.8e-05,0.43,-0.0015,-0.0025,0.00059,0,0,0.14,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32390000,0.99,-0.008,-0.015,0.17,-0.0063,-0.011,0.8,-0.015,0.03,0.12,-0.0013,-0.0057,2.3e-05,0.025,-0.034,-0.12,0.2,-2.6e-05,0.43,-0.0016,-0.0027,0.00065,0,0,0.22,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32490000,0.99,-0.011,-0.012,0.17,0.018,-0.077,-0.076,-0.014,0.024,0.12,-0.0013,-0.0057,1.9e-05,0.025,-0.034,-0.12,0.2,-2.7e-05,0.43,-0.0015,-0.0027,0.00064,0,0,0.22,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32590000,0.99,-0.011,-0.013,0.17,0.021,-0.079,-0.079,0.0015,0.02,0.1,-0.0013,-0.0057,1.8e-05,0.025,-0.034,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,0.2,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32690000,0.99,-0.011,-0.012,0.17,0.016,-0.085,-0.08,0.0033,0.012,0.09,-0.0013,-0.0057,1.7e-05,0.025,-0.034,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,0.19,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32790000,0.99,-0.011,-0.013,0.17,0.017,-0.084,-0.081,0.015,0.0093,0.074,-0.0013,-0.0057,1.6e-05,0.025,-0.034,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0022,0.00078,0,0,0.17,0.00028,0.00027,0.035,0.011,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0.017,0.00078,0.059,-0.0013,-0.0057,1.9e-05,0.025,-0.034,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0021,0.00079,0,0,0.16,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0.028,-0.0031,0.046,-0.0014,-0.0057,2.1e-05,0.025,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0015,-0.002,0.00085,0,0,0.15,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -33090000,0.98,-0.011,-0.013,0.17,0.013,-0.093,-0.079,0.029,-0.012,0.038,-0.0014,-0.0057,2.1e-05,0.025,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0015,-0.002,0.00085,0,0,0.14,0.00027,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0.037,-0.015,0.031,-0.0014,-0.0057,1.2e-05,0.026,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0015,-0.002,0.00088,0,0,0.13,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33290000,0.98,-0.01,-0.013,0.17,0.0073,-0.096,-0.078,0.037,-0.024,0.023,-0.0014,-0.0057,2.3e-05,0.026,-0.034,-0.12,0.2,-2.1e-05,0.43,-0.0016,-0.0021,0.00092,0,0,0.12,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33390000,0.98,-0.01,-0.013,0.17,0.0052,-0.09,-0.076,0.043,-0.022,0.014,-0.0014,-0.0057,1.6e-05,0.028,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0016,-0.002,0.00096,0,0,0.12,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 -33490000,0.98,-0.01,-0.013,0.17,-0.00014,-0.093,-0.075,0.042,-0.031,0.0042,-0.0014,-0.0057,2.1e-05,0.028,-0.032,-0.12,0.2,-1.7e-05,0.43,-0.0017,-0.0021,0.00098,0,0,0.12,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 -33590000,0.98,-0.01,-0.013,0.17,-0.0029,-0.092,-0.072,0.046,-0.029,-0.0037,-0.0014,-0.0057,2e-05,0.029,-0.031,-0.12,0.2,-1.3e-05,0.43,-0.0018,-0.0022,0.001,0,0,0.12,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.083 -33690000,0.98,-0.01,-0.013,0.17,-0.0062,-0.095,-0.073,0.046,-0.038,-0.012,-0.0014,-0.0057,2.4e-05,0.029,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.0018,-0.002,0.001,0,0,0.12,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.11 -33790000,0.98,-0.01,-0.013,0.17,-0.0081,-0.092,-0.068,0.052,-0.036,-0.019,-0.0014,-0.0057,1.4e-05,0.031,-0.031,-0.12,0.2,-1.2e-05,0.43,-0.0017,-0.0019,0.0011,0,0,0.12,0.00027,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.13 -33890000,0.98,-0.01,-0.013,0.17,-0.012,-0.093,-0.067,0.051,-0.045,-0.025,-0.0014,-0.0057,2.4e-05,0.031,-0.031,-0.12,0.2,-1.2e-05,0.43,-0.0018,-0.0018,0.0011,0,0,0.12,0.00027,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.16 -33990000,0.98,-0.0099,-0.013,0.17,-0.013,-0.088,-0.064,0.055,-0.04,-0.029,-0.0014,-0.0057,1.1e-05,0.033,-0.031,-0.12,0.2,-8.6e-06,0.43,-0.0017,-0.0018,0.0011,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.18 -34090000,0.98,-0.0098,-0.013,0.17,-0.017,-0.093,-0.062,0.054,-0.049,-0.033,-0.0014,-0.0057,1.4e-05,0.033,-0.031,-0.12,0.2,-9e-06,0.43,-0.0017,-0.0017,0.0011,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.21 -34190000,0.98,-0.0098,-0.013,0.17,-0.018,-0.088,-0.06,0.058,-0.044,-0.037,-0.0014,-0.0057,1e-05,0.035,-0.031,-0.12,0.2,-6.6e-06,0.43,-0.0017,-0.0016,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.23 -34290000,0.98,-0.0096,-0.013,0.17,-0.019,-0.092,-0.058,0.056,-0.052,-0.043,-0.0014,-0.0057,1.7e-05,0.035,-0.031,-0.12,0.2,-5.9e-06,0.43,-0.0017,-0.0016,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.26 -34390000,0.98,-0.0095,-0.013,0.17,-0.021,-0.085,-0.054,0.058,-0.047,-0.047,-0.0014,-0.0056,9.7e-06,0.036,-0.03,-0.12,0.2,-3.8e-06,0.43,-0.0016,-0.0015,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.28 -34490000,0.98,-0.0095,-0.013,0.17,-0.024,-0.089,-0.052,0.057,-0.055,-0.05,-0.0014,-0.0056,1.8e-05,0.036,-0.03,-0.12,0.2,-3.5e-06,0.43,-0.0017,-0.0015,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.31 -34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.081,0.74,0.059,-0.05,-0.021,-0.0014,-0.0056,1.1e-05,0.038,-0.03,-0.12,0.2,-1e-06,0.43,-0.0017,-0.0014,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 -34690000,0.98,-0.0094,-0.013,0.17,-0.03,-0.079,1.7,0.056,-0.058,0.098,-0.0014,-0.0056,1.4e-05,0.038,-0.03,-0.12,0.2,-7.2e-07,0.43,-0.0017,-0.0014,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 -34790000,0.98,-0.0093,-0.013,0.17,-0.033,-0.07,2.7,0.058,-0.052,0.28,-0.0015,-0.0056,7.7e-06,0.04,-0.031,-0.12,0.2,1.9e-06,0.43,-0.0017,-0.0013,0.0013,0,0,0.12,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 -34890000,0.98,-0.0092,-0.013,0.17,-0.038,-0.069,3.7,0.055,-0.058,0.57,-0.0015,-0.0056,1.1e-05,0.04,-0.031,-0.12,0.2,2e-06,0.43,-0.0017,-0.0013,0.0013,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.035,0,0,-4.9e+02,1.6e-09,-1.4e-09,-6.4e-11,0,0,-3.2e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 +290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.038,0,0,-4.9e+02,7.3e-09,-1.1e-08,-3.9e-10,0,0,-2e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 +390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.05,0,0,-4.9e+02,-2.1e-10,-1.4e-08,-3.2e-10,0,0,-1.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 +490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.055,0,0,-4.9e+02,-1.2e-06,7.3e-07,4.1e-08,0,0,-1.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 +590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.1,0,0,-4.9e+02,-1.3e-06,7.6e-07,4.5e-08,0,0,6.3e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 +690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.039,0,0,-4.9e+02,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00017,0,0,0,0,0,0,0,0,-4.9e+02,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 +790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.045,0,0,-4.9e+02,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,-4.9e+02,0.018,0.018,0.00077,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 +890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.086,0,0,-4.9e+02,-2.2e-05,1e-06,4.9e-07,0,0,-7.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.019,0.019,0.00051,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 +990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.11,0,0,-4.9e+02,-2.2e-05,9.9e-07,4.9e-07,0,0,-9.4e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.021,0.021,0.00062,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 +1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0,0,-4.9e+02,-6.1e-05,-1.5e-05,9.8e-07,0,0,3.8e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.023,0.023,0.00043,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 +1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.1,0,0,-4.9e+02,-5.8e-05,-1.4e-05,9.7e-07,0,0,-0.00052,0,0,0,0,0,0,0,0,-4.9e+02,0.025,0.025,0.00051,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 +1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.1,0,0,-4.9e+02,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00079,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.00038,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 +1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.094,0,0,-4.9e+02,-0.00016,-9.3e-05,1.5e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00043,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 +1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.11,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,-4.9e+02,0.027,0.027,0.00033,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1590000,1,-0.012,-0.014,0.0004,0.031,-0.024,-0.13,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.012,-0.014,0.00045,0.028,-0.019,-0.13,0,0,-4.9e+02,-0.00073,-0.00074,-3.4e-07,0,0,-0.0018,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0,0,-4.9e+02,-0.00073,-0.00073,-2.9e-07,0,0,-0.0028,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0,0,-4.9e+02,-0.00072,-0.00072,-2.7e-07,0,0,-0.0032,0,0,0,0,0,0,0,0,-4.9e+02,0.031,0.031,0.00037,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,0.00041,0.035,-0.018,-0.14,0,0,-4.9e+02,-0.0011,-0.0013,-3.6e-06,0,0,-0.0046,0,0,0,0,0,0,0,0,-4.9e+02,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0,0,-4.9e+02,-0.0011,-0.0012,-3.5e-06,0,0,-0.0064,0,0,0,0,0,0,0,0,-4.9e+02,0.027,0.027,0.00032,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,0.00039,0.033,-0.014,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.7e-06,0,0,-0.0075,0,0,0,0,0,0,0,0,-4.9e+02,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,0.00039,0.038,-0.014,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.5e-06,0,0,-0.0097,0,0,0,0,0,0,0,0,-4.9e+02,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,0.0004,0.029,-0.01,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.4e-05,0,0,-0.012,0,0,0,0,0,0,0,0,-4.9e+02,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.014,0.00048,0.033,-0.0091,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.4e-05,0,0,-0.013,0,0,0,0,0,0,0,0,-4.9e+02,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.01,-0.013,0.0004,0.023,-0.0062,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2e-05,0,0,-0.015,0,0,0,0,0,0,0,0,-4.9e+02,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.01,-0.013,0.00044,0.027,-0.0055,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,-4.9e+02,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.01,-0.013,0.00038,0.022,-0.0032,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.5e-05,0,0,-0.022,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.01,-0.013,0.00031,0.026,-0.005,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.5e-05,0,0,-0.025,0,0,0,0,0,0,0,0,-4.9e+02,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.01,-0.013,0.00033,0.02,-0.0039,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3e-05,0,0,-0.028,0,0,0,0,0,0,0,0,-4.9e+02,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.01,-0.013,0.00053,0.025,-0.0068,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3e-05,0,0,-0.031,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.01,-0.013,0.00058,0.02,-0.0065,-0.15,0,0,-4.9e+02,-0.0021,-0.0036,-3.5e-05,0,0,-0.032,0,0,0,0,0,0,0,0,-4.9e+02,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.01,-0.013,0.0006,0.023,-0.0067,-0.15,0,0,-4.9e+02,-0.0021,-0.0036,-3.4e-05,0,0,-0.034,0,0,0,0,0,0,0,0,-4.9e+02,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.0098,-0.013,0.00061,0.019,-0.0036,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.8e-05,0,0,-0.039,0,0,0,0,0,0,0,0,-4.9e+02,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.0097,-0.013,0.0006,0.025,-0.0023,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.8e-05,0,0,-0.044,0,0,0,0,0,0,0,0,-4.9e+02,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.0095,-0.012,0.00056,0.021,-0.0018,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.2e-05,0,0,-0.046,0,0,0,0,0,0,0,0,-4.9e+02,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.0095,-0.013,0.00054,0.024,-0.0011,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.2e-05,0,0,-0.051,0,0,0,0,0,0,0,0,-4.9e+02,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.0094,-0.012,0.00057,0.019,0.0033,-0.15,0,0,-4.9e+02,-0.0022,-0.0043,-4.7e-05,0,0,-0.054,0,0,0,0,0,0,0,0,-4.9e+02,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.0094,-0.013,0.00065,0.021,0.0045,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.058,0,0,0,0,0,0,0,0,-4.9e+02,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.0094,-0.013,0.00072,0.026,0.0042,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.063,0,0,0,0,0,0,0,0,-4.9e+02,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.0093,-0.012,0.00078,0.022,0.0037,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.1e-05,0,0,-0.071,0,0,0,0,0,0,0,0,-4.9e+02,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.0094,-0.012,0.00075,0.024,0.0034,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.1e-05,0,0,-0.074,0,0,0,0,0,0,0,0,-4.9e+02,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.0095,-0.012,0.00076,0.021,0.0033,-0.13,0,0,-4.9e+02,-0.0022,-0.0046,-5.6e-05,0,0,-0.076,0,0,0,0,0,0,0,0,-4.9e+02,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.0094,-0.012,0.00072,0.025,0.0018,-0.11,0,0,-4.9e+02,-0.0022,-0.0046,-5.6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,-4.9e+02,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.0094,-0.012,0.00078,0.021,0.0036,-0.11,0,0,-4.9e+02,-0.0022,-0.0048,-6.1e-05,0,0,-0.086,0,0,0,0,0,0,0,0,-4.9e+02,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.0094,-0.012,0.00085,0.023,0.0024,-0.11,0,0,-4.9e+02,-0.0022,-0.0048,-6.1e-05,0,0,-0.088,0,0,0,0,0,0,0,0,-4.9e+02,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.0094,-0.012,0.00079,0.017,0.0027,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,-4.9e+02,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.0093,-0.012,0.00089,0.015,0.0048,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,-4.9e+02,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.0093,-0.012,0.00093,0.01,0.0024,-0.093,0,0,-4.9e+02,-0.0021,-0.0051,-6.9e-05,0,0,-0.099,0,0,0,0,0,0,0,0,-4.9e+02,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.0092,-0.012,0.00091,0.013,0.0031,-0.085,0,0,-4.9e+02,-0.0021,-0.0051,-6.9e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.0091,-0.011,0.00099,0.01,0.0034,-0.083,0,0,-4.9e+02,-0.0021,-0.0052,-7.2e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.0089,-0.012,0.001,0.0099,0.007,-0.08,0,0,-4.9e+02,-0.0021,-0.0052,-7.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0071,-0.069,0,0,-4.9e+02,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0,0,-4.9e+02,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.061,0,0,-4.9e+02,-0.002,-0.0054,-7.7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0088,-0.012,0.001,0.0083,0.016,-0.054,0,0,-4.9e+02,-0.002,-0.0054,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0089,-0.011,0.00093,0.0077,0.016,-0.053,0,0,-4.9e+02,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0088,-0.011,0.00088,0.0089,0.018,-0.05,0,0,-4.9e+02,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0088,-0.011,0.00092,0.0095,0.015,-0.048,0,0,-4.9e+02,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0088,-0.012,0.00089,0.011,0.017,-0.042,0,0,-4.9e+02,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0088,-0.011,0.00071,0.011,0.018,-0.039,0,0,-4.9e+02,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0089,-0.011,0.00072,0.0087,0.017,-0.038,0,0,-4.9e+02,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0089,-0.011,0.00075,0.008,0.019,-0.041,0,0,-4.9e+02,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0089,-0.011,0.00076,0.0082,0.016,-0.042,0,0,-4.9e+02,-0.0018,-0.0056,-8.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0089,-0.011,0.00066,0.0057,0.016,-0.04,0,0,-4.9e+02,-0.0018,-0.0056,-8.7e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0089,-0.011,0.00059,0.0039,0.015,-0.042,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0088,-0.011,0.00055,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,1,-0.0089,-0.011,0.00052,0.0029,0.015,-0.043,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6890000,1,-0.0087,-0.011,0.00043,0.0023,0.015,-0.039,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 +6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 +7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 +7190000,0.98,-0.0065,-0.012,0.18,-6.1e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.7e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 +7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.2e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00094,-0.032,0,0,-4.9e+02,-0.0017,-0.0057,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.8e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7490000,0.98,-0.0063,-0.012,0.18,0.00097,0.0035,-0.026,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.9e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.006,-0.023,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00034,-0.00039,-9.5e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7690000,0.98,-0.0064,-0.013,0.18,0.0021,0.0093,-0.022,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-3.4e-05,0.43,-0.00031,-0.0004,2.4e-06,0,0,-4.9e+02,0.0016,0.0015,0.042,25,25,0.025,52,52,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00017,0.0013,0.0014,0.0016,0.0013,1,1,2 +7790000,0.98,-0.0064,-0.013,0.18,0.0056,0.01,-0.025,0,0,-4.9e+02,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.9e-05,0.43,-0.00021,-0.00039,-1.5e-06,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.024,35,35,0.061,6.3e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00015,0.0013,0.0014,0.0016,0.0013,1,1,2 +7890000,0.98,-0.0065,-0.013,0.18,0.0046,0.014,-0.025,0,0,-4.9e+02,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.6e-05,0.43,-0.00019,-0.0004,4.4e-05,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.023,36,36,0.06,6.3e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00013,0.0013,0.0014,0.0016,0.0013,1,1,2 +7990000,0.98,-0.0063,-0.013,0.18,0.0032,0.017,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-2.5e-05,0.43,-0.0002,-0.00042,7.4e-05,0,0,-4.9e+02,0.0017,0.0016,0.042,24,24,0.022,28,28,0.059,6.2e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00012,0.0013,0.0014,0.0016,0.0013,1,1,2 +8090000,0.98,-0.0063,-0.013,0.18,0.0043,0.019,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-2.2e-05,0.43,-0.00017,-0.00042,9.9e-05,0,0,-4.9e+02,0.0017,0.0017,0.042,24,24,0.022,30,30,0.059,6.2e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,0.00011,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8190000,0.98,-0.0064,-0.012,0.18,0.0052,0.022,-0.018,0,0,-4.9e+02,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.44,-0.00014,-0.00041,0.00014,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.021,24,24,0.058,6.1e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0013,0.0001,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8290000,0.98,-0.0062,-0.012,0.18,0.002,0.028,-0.017,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.43,-0.00014,-0.00044,6.7e-05,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.02,27,27,0.057,6.1e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0013,9.6e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8390000,0.98,-0.0062,-0.012,0.18,-0.0023,0.028,-0.016,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00044,2.1e-05,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.02,23,23,0.057,6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0013,9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8490000,0.98,-0.0059,-0.012,0.18,-0.0061,0.035,-0.017,0,0,-4.9e+02,-0.0017,-0.0059,-9.6e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00048,-7.4e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.019,25,25,0.056,5.9e-05,5.6e-05,2.2e-06,0.04,0.04,0.0011,0.0013,8.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8590000,0.98,-0.006,-0.012,0.18,-0.00035,0.034,-0.012,0,0,-4.9e+02,-0.0016,-0.0057,-9.6e-05,-0.00036,0.00036,-0.14,0.2,-1.7e-05,0.43,-0.00018,-0.00043,3.3e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,19,19,0.018,22,22,0.055,5.8e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0013,7.9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8690000,0.98,-0.006,-0.012,0.18,-0.0022,0.037,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-1.6e-05,0.43,-0.00022,-0.00045,-8.7e-05,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,24,24,0.055,5.8e-05,5.3e-05,2.2e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8790000,0.98,-0.006,-0.012,0.18,-0.005,0.039,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00019,-0.00045,-0.00013,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,27,27,0.055,5.7e-05,5.2e-05,2.2e-06,0.04,0.04,0.00099,0.0013,7.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8890000,0.98,-0.006,-0.012,0.18,0.00053,0.041,-0.0094,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00026,-0.00043,-0.00011,0,0,-4.9e+02,0.002,0.0018,0.041,17,17,0.017,24,24,0.054,5.6e-05,5e-05,2.2e-06,0.04,0.04,0.00094,0.0013,6.7e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +8990000,0.98,-0.0059,-0.013,0.18,0.01,0.047,-0.0086,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00037,-0.00039,-6.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,17,17,0.017,27,27,0.054,5.5e-05,4.9e-05,2.2e-06,0.04,0.04,0.00091,0.0013,6.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +9090000,0.98,-0.0055,-0.012,0.18,-0.00033,0.056,-0.0096,0,0,-4.9e+02,-0.0018,-0.0057,-8.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00028,-0.00049,-8.8e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,24,24,0.053,5.3e-05,4.7e-05,2.2e-06,0.04,0.04,0.00087,0.0013,6.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +9190000,0.98,-0.0053,-0.013,0.18,0.0029,0.064,-0.009,0,0,-4.9e+02,-0.0018,-0.0057,-8.6e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00031,-0.00053,-0.00011,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,27,27,0.052,5.2e-05,4.5e-05,2.2e-06,0.04,0.04,0.00083,0.0013,5.9e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.3 +9290000,0.98,-0.0053,-0.013,0.18,0.013,0.062,-0.0074,0,0,-4.9e+02,-0.0018,-0.0056,-8.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.0004,-0.00049,-8.5e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,24,24,0.052,5.1e-05,4.4e-05,2.2e-06,0.04,0.04,0.00079,0.0013,5.6e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 +9390000,0.98,-0.0054,-0.013,0.18,0.014,0.06,-0.0063,0,0,-4.9e+02,-0.0017,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00035,-0.00043,-3.8e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,26,26,0.052,5e-05,4.2e-05,2.2e-06,0.04,0.04,0.00077,0.0013,5.4e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 +9490000,0.98,-0.0053,-0.013,0.18,0.0087,0.061,-0.0046,0,0,-4.9e+02,-0.0018,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00033,-0.00048,-7.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,12,12,0.015,23,23,0.051,4.8e-05,4e-05,2.2e-06,0.04,0.04,0.00074,0.0013,5.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 +9590000,0.98,-0.0057,-0.013,0.18,0.0078,0.052,-0.0045,0,0,-4.9e+02,-0.0016,-0.0057,-9.3e-05,-0.00036,0.00036,-0.14,0.2,-1e-05,0.43,-0.00031,-0.00041,-0.00012,0,0,-4.9e+02,0.0022,0.0018,0.041,12,12,0.014,26,26,0.05,4.7e-05,3.9e-05,2.2e-06,0.04,0.04,0.00071,0.0013,5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 +9690000,0.98,-0.0059,-0.013,0.18,0.0096,0.047,-0.0016,0,0,-4.9e+02,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-9.2e-06,0.43,-0.00031,-0.00038,-9.9e-05,0,0,-4.9e+02,0.0022,0.0018,0.041,10,10,0.014,23,23,0.05,4.6e-05,3.7e-05,2.2e-06,0.04,0.04,0.00068,0.0013,4.8e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9790000,0.98,-0.0061,-0.012,0.18,0.001,0.041,-0.003,0,0,-4.9e+02,-0.0015,-0.0058,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.7e-06,0.43,-0.00019,-0.00036,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,10,10,0.014,25,25,0.05,4.4e-05,3.5e-05,2.2e-06,0.04,0.04,0.00066,0.0013,4.7e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9890000,0.98,-0.0061,-0.012,0.18,0.0086,0.04,-0.0017,0,0,-4.9e+02,-0.0015,-0.0057,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.6e-06,0.43,-0.00023,-0.00034,-9.4e-05,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,22,22,0.049,4.3e-05,3.4e-05,2.2e-06,0.04,0.04,0.00063,0.0013,4.5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9990000,0.98,-0.0063,-0.012,0.18,0.0025,0.034,-0.001,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-6.4e-06,0.43,-0.00014,-0.00033,-0.00013,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,24,24,0.049,4.2e-05,3.2e-05,2.2e-06,0.04,0.04,0.00061,0.0013,4.4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +10090000,0.98,-0.0067,-0.012,0.18,0.00072,0.019,0.00016,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-4.9e-06,0.43,-9.8e-05,-0.00025,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,7.4,7.5,0.013,21,21,0.048,4e-05,3.1e-05,2.2e-06,0.04,0.04,0.00059,0.0013,4.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10190000,0.98,-0.0072,-0.012,0.18,0.0053,0.0047,0.001,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00036,0.00036,-0.14,0.2,-3.3e-06,0.43,-9.1e-05,-0.00012,-0.00015,0,0,-4.9e+02,0.0022,0.0016,0.041,7.4,7.6,0.012,23,23,0.048,3.9e-05,2.9e-05,2.2e-06,0.04,0.04,0.00057,0.0013,4.1e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10290000,0.98,-0.0071,-0.012,0.18,0.012,0.0085,-3.9e-05,0,0,-4.9e+02,-0.0012,-0.0057,-0.00011,-0.00036,0.00036,-0.14,0.2,-3.7e-06,0.43,-0.00016,-0.00011,-0.00012,0,0,-4.9e+02,0.0022,0.0016,0.041,6.4,6.5,0.012,20,20,0.048,3.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10390000,0.98,-0.0071,-0.012,0.18,0.0076,0.0026,-0.0025,0,0,-4.9e+02,-0.0012,-0.0056,-0.00011,-0.0003,0.00041,-0.14,0.2,-3.9e-06,0.43,-0.0002,-0.00011,-6.5e-05,0,0,-4.9e+02,0.0022,0.0016,0.041,0.25,0.25,0.56,0.5,0.5,0.048,3.6e-05,2.7e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10490000,0.98,-0.0073,-0.012,0.18,0.0096,0.00082,0.007,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.00033,0.00027,-0.14,0.2,-3e-06,0.43,-0.00016,-5.9e-05,-8.7e-05,0,0,-4.9e+02,0.0021,0.0016,0.041,0.25,0.26,0.55,0.51,0.51,0.057,3.5e-05,2.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10590000,0.98,-0.007,-0.012,0.18,0.00031,-0.00018,0.013,0,0,-4.9e+02,-0.0012,-0.0056,-0.00011,-0.0003,0.00025,-0.14,0.2,-3.6e-06,0.43,-0.00018,-9.8e-05,-7.3e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.13,0.27,0.17,0.17,0.055,3.3e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10690000,0.98,-0.0071,-0.013,0.18,0.0031,-0.0028,0.016,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00027,0.00018,-0.14,0.2,-3.3e-06,0.43,-0.00021,-4.9e-05,-7e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.14,0.26,0.17,0.18,0.065,3.2e-05,2.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10790000,0.98,-0.0073,-0.013,0.18,0.0057,-0.006,0.014,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00016,7.3e-05,-0.14,0.2,-3.1e-06,0.43,-0.00026,1.3e-05,-9e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.09,0.095,0.17,0.11,0.11,0.061,3e-05,2.1e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10890000,0.98,-0.0069,-0.013,0.18,0.008,-0.0029,0.01,0,0,-4.9e+02,-0.0012,-0.0055,-0.00011,-5.4e-05,0.0002,-0.14,0.2,-4.1e-06,0.43,-0.00033,-3e-05,-7.2e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.097,0.1,0.16,0.11,0.11,0.068,2.9e-05,2e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +10990000,0.98,-0.0069,-0.013,0.18,0.0054,0.0032,0.016,0,0,-4.9e+02,-0.0012,-0.0056,-0.00011,-0.00021,0.00015,-0.14,0.2,-4.1e-06,0.43,-0.00026,-5.9e-05,-0.00012,0,0,-4.9e+02,0.0019,0.0014,0.041,0.074,0.081,0.12,0.079,0.079,0.065,2.6e-05,1.9e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11090000,0.98,-0.0075,-0.013,0.18,0.0093,0.0017,0.02,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.0003,-5.8e-05,-0.14,0.2,-2.9e-06,0.43,-0.00024,2.5e-05,-0.0001,0,0,-4.9e+02,0.0019,0.0013,0.041,0.082,0.093,0.11,0.084,0.085,0.069,2.6e-05,1.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11190000,0.98,-0.0077,-0.013,0.18,0.0083,0.0023,0.026,0,0,-4.9e+02,-0.001,-0.0056,-0.00012,-0.00041,-1.3e-05,-0.14,0.2,-2.4e-06,0.43,-0.00017,-1.2e-06,-0.00012,0,0,-4.9e+02,0.0017,0.0013,0.04,0.066,0.076,0.084,0.065,0.066,0.066,2.2e-05,1.6e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11290000,0.98,-0.0077,-0.012,0.18,0.0065,-0.0003,0.025,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00052,0.00011,-0.14,0.2,-2.2e-06,0.43,-0.0001,-4e-05,-0.00015,0,0,-4.9e+02,0.0017,0.0012,0.04,0.074,0.088,0.078,0.071,0.072,0.069,2.2e-05,1.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11390000,0.98,-0.0076,-0.012,0.18,0.0045,0.00081,0.016,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00054,1.8e-05,-0.14,0.2,-2.3e-06,0.43,-7.8e-05,-5.1e-05,-0.00018,0,0,-4.9e+02,0.0015,0.0012,0.04,0.062,0.074,0.064,0.058,0.058,0.066,1.9e-05,1.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11490000,0.98,-0.0075,-0.012,0.18,0.0052,-0.00071,0.02,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00052,-6.5e-05,-0.14,0.2,-2.3e-06,0.43,-9.9e-05,-3e-05,-0.00017,0,0,-4.9e+02,0.0015,0.0011,0.04,0.071,0.086,0.058,0.063,0.064,0.067,1.8e-05,1.3e-05,2.2e-06,0.04,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11590000,0.98,-0.0076,-0.012,0.18,0.0047,-0.00064,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00061,2.5e-05,-0.14,0.2,-2.3e-06,0.43,-5e-05,-6.9e-05,-0.00019,0,0,-4.9e+02,0.0014,0.0011,0.04,0.06,0.072,0.049,0.053,0.054,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11690000,0.98,-0.0075,-0.012,0.18,0.0042,0.0018,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00057,0.00015,-0.14,0.2,-2.5e-06,0.43,-4.4e-05,-8.6e-05,-0.00022,0,0,-4.9e+02,0.0014,0.001,0.04,0.068,0.084,0.046,0.059,0.06,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11790000,0.98,-0.0075,-0.012,0.18,0.0028,0.0026,0.019,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-4.6e-05,0.00011,-0.14,0.2,-2.6e-06,0.43,-5.7e-05,-9.1e-05,-0.00022,0,0,-4.9e+02,0.0012,0.00096,0.039,0.058,0.07,0.039,0.05,0.051,0.062,1.3e-05,9.4e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11890000,0.98,-0.0077,-0.012,0.18,0.0039,0.0012,0.017,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00025,0.00027,-0.14,0.2,-2.4e-06,0.43,-2.6e-05,-9.1e-05,-0.00026,0,0,-4.9e+02,0.0012,0.00096,0.039,0.066,0.082,0.036,0.056,0.058,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11990000,0.98,-0.0077,-0.012,0.18,0.0067,0.0034,0.014,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00032,0.00042,-0.14,0.2,-2.7e-06,0.43,-4.4e-05,-9.8e-05,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.057,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +12090000,0.98,-0.0077,-0.012,0.18,0.0099,0.001,0.017,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00051,0.00017,-0.14,0.2,-2.4e-06,0.43,-4.3e-05,-6.6e-05,-0.00025,0,0,-4.9e+02,0.0011,0.00088,0.039,0.064,0.079,0.029,0.055,0.056,0.06,1e-05,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12190000,0.98,-0.0077,-0.012,0.18,0.011,-0.00015,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.00048,-0.14,0.2,-2.3e-06,0.43,-6.9e-06,-3e-05,-0.00028,0,0,-4.9e+02,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12290000,0.98,-0.0078,-0.012,0.18,0.0081,-0.0036,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0015,-0.00031,-0.14,0.2,-2.1e-06,0.43,2e-05,-3.8e-05,-0.00028,0,0,-4.9e+02,0.00095,0.00081,0.039,0.062,0.075,0.024,0.054,0.056,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12390000,0.98,-0.0079,-0.012,0.18,0.008,-0.0049,0.013,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0018,-0.00078,-0.14,0.2,-2e-06,0.43,4.7e-05,-1.7e-05,-0.0003,0,0,-4.9e+02,0.00085,0.00075,0.039,0.053,0.062,0.022,0.047,0.048,0.056,6.9e-06,5.6e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12490000,0.98,-0.008,-0.012,0.18,0.0096,-0.0064,0.017,0,0,-4.9e+02,-0.00099,-0.0058,-0.00013,-0.0021,-0.001,-0.14,0.2,-1.9e-06,0.43,5.5e-05,1.3e-05,-0.00031,0,0,-4.9e+02,0.00085,0.00075,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.8e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12590000,0.98,-0.0081,-0.012,0.18,0.012,-0.01,0.018,0,0,-4.9e+02,-0.00098,-0.0058,-0.00013,-0.0022,-0.0009,-0.14,0.2,-1.8e-06,0.43,6.9e-05,1.3e-05,-0.00032,0,0,-4.9e+02,0.00077,0.0007,0.039,0.051,0.059,0.019,0.046,0.047,0.054,5.7e-06,4.7e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12690000,0.98,-0.008,-0.012,0.18,0.012,-0.014,0.018,0,0,-4.9e+02,-0.00097,-0.0058,-0.00013,-0.0024,-0.00085,-0.14,0.2,-1.8e-06,0.43,7.9e-05,1.7e-05,-0.00033,0,0,-4.9e+02,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.055,0.054,5.7e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.02,0,0,-4.9e+02,-0.00098,-0.0058,-0.00013,-0.0017,-0.0013,-0.14,0.2,-1.8e-06,0.43,4.4e-05,4.1e-05,-0.00031,0,0,-4.9e+02,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4.1e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12890000,0.98,-0.008,-0.012,0.18,0.016,-0.013,0.021,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.0014,-0.0015,-0.14,0.2,-1.9e-06,0.43,2.4e-05,4.5e-05,-0.00029,0,0,-4.9e+02,0.00071,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0013,-0.0016,-0.14,0.2,-2.4e-06,0.43,3.8e-05,2e-05,-0.00032,0,0,-4.9e+02,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00075,-0.0018,-0.14,0.2,-2.4e-06,0.43,1.5e-05,2.6e-05,-0.0003,0,0,-4.9e+02,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13190000,0.98,-0.0079,-0.012,0.18,0.0097,-0.0078,0.017,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00053,-0.0023,-0.14,0.2,-2.6e-06,0.43,3.9e-05,2.4e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00076,-0.0028,-0.14,0.2,-2.4e-06,0.43,4.8e-05,4.4e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.6e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13390000,0.98,-0.0079,-0.012,0.18,0.0091,-0.0081,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0014,-0.0028,-0.14,0.2,-2.7e-06,0.43,8.6e-05,4.2e-05,-0.00035,0,0,-4.9e+02,0.00057,0.00056,0.038,0.042,0.047,0.012,0.045,0.046,0.047,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13490000,0.98,-0.0079,-0.012,0.18,0.011,-0.0068,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0015,-0.0033,-0.14,0.2,-2.6e-06,0.43,8.9e-05,6.4e-05,-0.00035,0,0,-4.9e+02,0.00058,0.00056,0.038,0.047,0.052,0.011,0.052,0.053,0.046,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13590000,0.98,-0.0079,-0.012,0.18,0.014,-0.0071,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.002,-0.0034,-0.14,0.2,-2.6e-06,0.43,9.8e-05,5.7e-05,-0.00035,0,0,-4.9e+02,0.00054,0.00053,0.038,0.04,0.044,0.011,0.045,0.046,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13690000,0.98,-0.0078,-0.012,0.18,0.014,-0.0082,0.015,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0016,-0.0031,-0.14,0.2,-2.6e-06,0.43,8.5e-05,3.7e-05,-0.00033,0,0,-4.9e+02,0.00055,0.00053,0.038,0.044,0.049,0.011,0.052,0.053,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13790000,0.98,-0.0077,-0.012,0.18,0.019,-0.0044,0.015,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0022,-0.0026,-0.14,0.2,-2.8e-06,0.43,8.3e-05,1.6e-05,-0.00034,0,0,-4.9e+02,0.00052,0.00051,0.038,0.038,0.042,0.01,0.045,0.046,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13890000,0.98,-0.0076,-0.012,0.18,0.019,-0.0029,0.016,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0015,-0.002,-0.14,0.2,-3e-06,0.43,6.2e-05,3.3e-07,-0.00034,0,0,-4.9e+02,0.00052,0.00052,0.038,0.042,0.046,0.01,0.051,0.053,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13990000,0.98,-0.0076,-0.012,0.18,0.019,-0.001,0.014,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.001,-0.0025,-0.14,0.2,-2.9e-06,0.43,7e-05,-9.3e-07,-0.00031,0,0,-4.9e+02,0.0005,0.0005,0.038,0.036,0.039,0.0097,0.045,0.046,0.043,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +14090000,0.98,-0.0077,-0.011,0.18,0.017,-0.0082,0.015,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0028,-0.0023,-0.14,0.2,-2.8e-06,0.43,0.00012,7.3e-06,-0.00035,0,0,-4.9e+02,0.0005,0.0005,0.038,0.04,0.043,0.0096,0.051,0.053,0.042,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14190000,0.98,-0.0078,-0.011,0.18,0.016,-0.0076,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0039,-0.0034,-0.14,0.2,-2.6e-06,0.43,0.00018,3.3e-05,-0.00037,0,0,-4.9e+02,0.00048,0.00049,0.038,0.034,0.037,0.0094,0.045,0.046,0.042,2.1e-06,1.9e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14290000,0.98,-0.0077,-0.011,0.18,0.018,-0.0081,0.013,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.004,-0.0037,-0.14,0.2,-2.6e-06,0.43,0.00018,3.6e-05,-0.00036,0,0,-4.9e+02,0.00048,0.00049,0.038,0.038,0.041,0.0092,0.051,0.052,0.041,2.1e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14390000,0.98,-0.0078,-0.011,0.18,0.018,-0.0095,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0038,-0.0044,-0.14,0.2,-2.4e-06,0.43,0.00019,4.1e-05,-0.00035,0,0,-4.9e+02,0.00047,0.00047,0.038,0.033,0.035,0.009,0.045,0.046,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14490000,0.98,-0.008,-0.011,0.18,0.018,-0.011,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0049,-0.0045,-0.14,0.2,-2.3e-06,0.43,0.00022,5e-05,-0.00037,0,0,-4.9e+02,0.00047,0.00047,0.038,0.036,0.038,0.009,0.051,0.052,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14590000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00015,-0.0054,-0.0058,-0.14,0.2,-2.1e-06,0.43,0.00027,6.9e-05,-0.00037,0,0,-4.9e+02,0.00046,0.00046,0.038,0.031,0.033,0.0088,0.045,0.045,0.04,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14690000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0053,-0.0062,-0.14,0.2,-2e-06,0.43,0.00027,7.6e-05,-0.00037,0,0,-4.9e+02,0.00046,0.00046,0.038,0.034,0.036,0.0088,0.05,0.051,0.039,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14790000,0.98,-0.0082,-0.011,0.18,0.019,-0.0034,0.016,0,0,-4.9e+02,-0.0011,-0.0058,-0.00014,-0.0047,-0.0076,-0.14,0.2,-2.2e-06,0.43,0.00027,7.4e-05,-0.00035,0,0,-4.9e+02,0.00044,0.00045,0.038,0.03,0.031,0.0086,0.044,0.045,0.039,1.7e-06,1.5e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14890000,0.98,-0.0082,-0.011,0.18,0.021,-0.0055,0.02,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.0093,-0.14,0.2,-1.8e-06,0.43,0.00027,9.5e-05,-0.00031,0,0,-4.9e+02,0.00045,0.00045,0.038,0.032,0.034,0.0087,0.05,0.051,0.039,1.6e-06,1.5e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +14990000,0.98,-0.0083,-0.011,0.18,0.02,-0.0057,0.022,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0041,-0.011,-0.14,0.2,-1.6e-06,0.43,0.0003,0.00012,-0.00029,0,0,-4.9e+02,0.00044,0.00045,0.038,0.028,0.03,0.0085,0.044,0.045,0.038,1.6e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15090000,0.98,-0.0083,-0.011,0.18,0.021,-0.0049,0.026,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.011,-0.14,0.2,-1.5e-06,0.43,0.0003,0.00013,-0.00029,0,0,-4.9e+02,0.00044,0.00045,0.038,0.031,0.032,0.0085,0.05,0.051,0.038,1.5e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15190000,0.98,-0.0085,-0.011,0.18,0.019,-0.0059,0.027,0,0,-4.9e+02,-0.0011,-0.0058,-0.00014,-0.0048,-0.012,-0.14,0.2,-1.4e-06,0.43,0.00034,0.00014,-0.00029,0,0,-4.9e+02,0.00043,0.00044,0.038,0.027,0.028,0.0085,0.044,0.045,0.038,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15290000,0.98,-0.0086,-0.012,0.18,0.022,-0.0065,0.026,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.2e-06,0.43,0.00033,0.00017,-0.00027,0,0,-4.9e+02,0.00043,0.00044,0.038,0.029,0.031,0.0085,0.049,0.05,0.037,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15390000,0.98,-0.0088,-0.011,0.18,0.021,-0.004,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0045,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,-4.9e+02,0.00042,0.00043,0.038,0.026,0.027,0.0084,0.044,0.044,0.037,1.4e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15490000,0.98,-0.0088,-0.011,0.18,0.022,-0.0067,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0046,-0.014,-0.14,0.2,-1.4e-06,0.43,0.00037,0.00019,-0.00029,0,0,-4.9e+02,0.00042,0.00043,0.038,0.028,0.029,0.0085,0.049,0.05,0.037,1.4e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15590000,0.98,-0.0088,-0.011,0.18,0.023,-0.0081,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00036,0.00019,-0.00028,0,0,-4.9e+02,0.00041,0.00043,0.038,0.024,0.026,0.0084,0.043,0.044,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15690000,0.98,-0.0086,-0.011,0.18,0.024,-0.01,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0032,-0.012,-0.14,0.2,-1.8e-06,0.43,0.00031,0.00013,-0.00026,0,0,-4.9e+02,0.00041,0.00043,0.038,0.026,0.028,0.0085,0.049,0.049,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15790000,0.98,-0.0086,-0.011,0.18,0.019,-0.0094,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0021,-0.012,-0.14,0.2,-2.2e-06,0.43,0.0003,0.00012,-0.00026,0,0,-4.9e+02,0.00041,0.00042,0.038,0.023,0.025,0.0084,0.043,0.044,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15890000,0.98,-0.0087,-0.011,0.18,0.02,-0.0085,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0031,-0.012,-0.14,0.2,-1.9e-06,0.43,0.00034,0.00015,-0.00027,0,0,-4.9e+02,0.00041,0.00042,0.038,0.025,0.027,0.0085,0.048,0.049,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15990000,0.98,-0.0086,-0.011,0.18,0.018,-0.0074,0.022,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0034,-0.014,-0.14,0.2,-2.1e-06,0.43,0.00038,0.00019,-0.00029,0,0,-4.9e+02,0.0004,0.00042,0.038,0.022,0.024,0.0084,0.043,0.043,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +16090000,0.98,-0.0087,-0.011,0.18,0.019,-0.0092,0.02,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.004,-0.014,-0.14,0.2,-2e-06,0.43,0.00041,0.00021,-0.00031,0,0,-4.9e+02,0.0004,0.00042,0.038,0.024,0.025,0.0085,0.048,0.049,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16190000,0.98,-0.0086,-0.011,0.18,0.015,-0.0076,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0047,-0.015,-0.13,0.2,-2e-06,0.43,0.00044,0.00021,-0.00033,0,0,-4.9e+02,0.0004,0.00041,0.038,0.021,0.023,0.0084,0.043,0.043,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16290000,0.98,-0.0087,-0.011,0.18,0.016,-0.009,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0044,-0.014,-0.13,0.2,-2e-06,0.43,0.00043,0.0002,-0.00032,0,0,-4.9e+02,0.0004,0.00041,0.038,0.023,0.024,0.0085,0.047,0.048,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16390000,0.98,-0.0087,-0.011,0.18,0.019,-0.0081,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.004,-0.017,-0.13,0.2,-1.7e-06,0.43,0.00045,0.00024,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.022,0.0084,0.042,0.043,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16490000,0.98,-0.0089,-0.011,0.18,0.022,-0.01,0.021,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0044,-0.017,-0.13,0.2,-1.5e-06,0.43,0.00046,0.00025,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.022,0.023,0.0085,0.047,0.048,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16590000,0.98,-0.0089,-0.011,0.18,0.025,-0.01,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0043,-0.018,-0.13,0.2,-1.7e-06,0.43,0.00048,0.00025,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.02,0.021,0.0084,0.042,0.043,0.036,1.1e-06,9.7e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16690000,0.98,-0.0089,-0.011,0.18,0.028,-0.015,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0037,-0.017,-0.13,0.2,-1.9e-06,0.43,0.00046,0.00024,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.023,0.0085,0.047,0.047,0.036,1.1e-06,9.6e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16790000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0031,-0.017,-0.13,0.2,-2.2e-06,0.43,0.00046,0.00024,-0.00029,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.042,0.042,0.036,1e-06,9.3e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16890000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0021,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00043,0.00023,-0.00028,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.022,0.0085,0.046,0.047,0.036,1e-06,9.2e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +16990000,0.98,-0.0088,-0.011,0.18,0.026,-0.013,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.0018,-0.018,-0.13,0.2,-2.3e-06,0.43,0.00044,0.00025,-0.00028,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,1e-06,9e-07,2e-06,0.035,0.032,0.00042,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17090000,0.98,-0.0089,-0.011,0.18,0.03,-0.017,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0022,-0.019,-0.13,0.2,-2.4e-06,0.43,0.00048,0.00032,-0.00029,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.021,0.0085,0.046,0.047,0.035,9.9e-07,8.9e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17190000,0.98,-0.009,-0.011,0.18,0.028,-0.021,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0031,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00053,0.00033,-0.00033,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,9.6e-07,8.7e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17290000,0.98,-0.009,-0.011,0.18,0.03,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0038,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00055,0.00034,-0.00035,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.045,0.046,0.036,9.6e-07,8.6e-07,2e-06,0.035,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17390000,0.98,-0.0089,-0.011,0.18,0.025,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0025,-0.018,-0.13,0.2,-3.2e-06,0.43,0.00053,0.00034,-0.00034,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.041,0.041,0.035,9.3e-07,8.4e-07,2e-06,0.034,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17490000,0.98,-0.009,-0.011,0.18,0.024,-0.025,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0028,-0.019,-0.13,0.2,-3.2e-06,0.43,0.00054,0.00034,-0.00035,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.02,0.0085,0.045,0.046,0.036,9.3e-07,8.3e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17590000,0.98,-0.0089,-0.011,0.18,0.021,-0.022,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0024,-0.019,-0.13,0.2,-3.8e-06,0.43,0.00055,0.00034,-0.00035,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.04,0.041,0.035,9e-07,8.1e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17690000,0.98,-0.009,-0.011,0.18,0.022,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0021,-0.019,-0.13,0.2,-3.5e-06,0.43,0.00053,0.00032,-0.00033,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.019,0.0084,0.045,0.045,0.035,9e-07,8e-07,2e-06,0.034,0.031,0.00038,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17790000,0.98,-0.0091,-0.011,0.18,0.022,-0.022,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0013,-0.019,-0.13,0.2,-3.5e-06,0.43,0.00051,0.00029,-0.00031,0,0,-4.9e+02,0.00037,0.00039,0.038,0.016,0.017,0.0084,0.04,0.041,0.036,8.8e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17890000,0.98,-0.0089,-0.011,0.18,0.025,-0.024,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00075,-0.018,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.0003,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.019,0.0084,0.044,0.045,0.036,8.7e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17990000,0.98,-0.0089,-0.011,0.18,0.025,-0.02,0.024,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,4.1e-05,-0.02,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.00029,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0083,0.04,0.041,0.035,8.5e-07,7.6e-07,2e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +18090000,0.98,-0.009,-0.011,0.18,0.026,-0.021,0.024,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,8.7e-05,-0.02,-0.13,0.2,-3.3e-06,0.43,0.00048,0.00027,-0.00028,0,0,-4.9e+02,0.00036,0.00038,0.038,0.017,0.018,0.0083,0.044,0.045,0.036,8.5e-07,7.5e-07,1.9e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18190000,0.98,-0.0091,-0.011,0.18,0.025,-0.019,0.024,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0017,-0.021,-0.13,0.2,-3.6e-06,0.43,0.00045,0.00029,-0.00025,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0082,0.04,0.04,0.035,8.2e-07,7.4e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18290000,0.98,-0.0092,-0.011,0.18,0.028,-0.02,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0018,-0.022,-0.13,0.2,-3.4e-06,0.43,0.00045,0.0003,-0.00024,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.018,0.0082,0.044,0.044,0.036,8.2e-07,7.3e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18390000,0.98,-0.0091,-0.011,0.18,0.027,-0.019,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.002,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00046,0.00031,-0.00025,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18490000,0.98,-0.0091,-0.011,0.18,0.031,-0.019,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0027,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00045,0.00031,-0.00024,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0082,0.043,0.044,0.036,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18590000,0.98,-0.0089,-0.011,0.18,0.028,-0.019,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0029,-0.021,-0.13,0.2,-4.2e-06,0.43,0.00044,0.00028,-0.00024,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,7.8e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18690000,0.98,-0.0088,-0.011,0.18,0.028,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0039,-0.02,-0.13,0.2,-4.2e-06,0.43,0.00041,0.00025,-0.00022,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0081,0.043,0.044,0.035,7.7e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18790000,0.98,-0.0087,-0.011,0.18,0.026,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0043,-0.02,-0.13,0.2,-4.6e-06,0.43,0.00041,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.008,0.039,0.04,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00032,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18890000,0.98,-0.0088,-0.011,0.18,0.026,-0.018,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0041,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00043,0.00028,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.017,0.008,0.043,0.044,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +18990000,0.98,-0.0087,-0.011,0.18,0.023,-0.018,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0049,-0.019,-0.13,0.2,-5.3e-06,0.43,0.00042,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19090000,0.98,-0.0087,-0.011,0.18,0.023,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0054,-0.019,-0.13,0.2,-5.2e-06,0.43,0.00041,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19190000,0.98,-0.0086,-0.011,0.18,0.02,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0052,-0.019,-0.13,0.2,-6e-06,0.43,0.00043,0.00028,-0.00026,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19290000,0.98,-0.0086,-0.011,0.18,0.021,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0052,-0.019,-0.13,0.2,-5.9e-06,0.43,0.00044,0.00029,-0.00026,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.016,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0058,-0.019,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00028,-0.00026,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19490000,0.98,-0.0088,-0.011,0.18,0.02,-0.017,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0052,-0.02,-0.13,0.2,-6e-06,0.43,0.00043,0.00026,-0.00025,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19590000,0.98,-0.0087,-0.011,0.18,0.018,-0.019,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0054,-0.02,-0.13,0.2,-6.2e-06,0.43,0.00043,0.00026,-0.00025,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.03,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19690000,0.98,-0.0088,-0.011,0.18,0.018,-0.017,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0059,-0.021,-0.13,0.2,-5.9e-06,0.43,0.00043,0.00026,-0.00024,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.016,0.0076,0.042,0.043,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19790000,0.98,-0.0089,-0.011,0.18,0.015,-0.016,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0059,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0076,0.038,0.039,0.035,6.6e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19890000,0.98,-0.0089,-0.011,0.18,0.017,-0.016,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0059,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.5e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19990000,0.98,-0.0089,-0.012,0.18,0.015,-0.016,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0068,-0.021,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.038,0.038,0.035,6.4e-07,5.7e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +20090000,0.98,-0.0089,-0.012,0.18,0.017,-0.019,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.007,-0.021,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00028,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.4e-07,5.6e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5.1 +20190000,0.98,-0.0089,-0.012,0.18,0.017,-0.016,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00013,0.007,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00043,0.0003,-0.00027,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.037,0.038,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20290000,0.98,-0.009,-0.012,0.18,0.015,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.007,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00043,0.0003,-0.00028,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20390000,0.98,-0.0089,-0.012,0.18,0.013,-0.016,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.008,-0.022,-0.13,0.2,-7.5e-06,0.43,0.00041,0.00028,-0.00026,0,0,-4.9e+02,0.00034,0.00035,0.038,0.013,0.014,0.0073,0.037,0.038,0.035,6.1e-07,5.4e-07,1.8e-06,0.031,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20490000,0.98,-0.0089,-0.012,0.18,0.0094,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0081,-0.022,-0.13,0.2,-7.3e-06,0.43,0.00041,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00035,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6e-07,5.3e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20590000,0.98,-0.0088,-0.012,0.18,0.0087,-0.017,0.02,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0093,-0.022,-0.13,0.2,-7.2e-06,0.43,0.00039,0.00025,-0.00025,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0072,0.037,0.038,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20690000,0.98,-0.0088,-0.012,0.18,0.0076,-0.017,0.021,0,0,-4.9e+02,-0.0013,-0.0058,-0.00012,0.009,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00039,0.00025,-0.00025,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0072,0.041,0.042,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20790000,0.98,-0.0081,-0.012,0.18,0.0037,-0.013,0.006,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0098,-0.022,-0.13,0.2,-7.8e-06,0.43,0.00038,0.00025,-0.00025,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0071,0.037,0.038,0.035,5.8e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20890000,0.98,0.00097,-0.0077,0.18,-0.00018,-0.002,-0.11,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00038,-0.00019,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0071,0.04,0.041,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20990000,0.98,0.0044,-0.0043,0.18,-0.012,0.017,-0.25,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00036,-0.00015,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.007,0.037,0.038,0.034,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21090000,0.98,0.0027,-0.0047,0.18,-0.024,0.032,-0.37,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-6.9e-06,0.43,0.00043,0.00016,-0.00019,0,0,-4.9e+02,0.00033,0.00035,0.038,0.014,0.015,0.007,0.04,0.041,0.035,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21190000,0.98,-6.9e-05,-0.0063,0.18,-0.03,0.039,-0.5,0,0,-4.9e+02,-0.0013,-0.0058,-0.0001,0.0099,-0.024,-0.13,0.2,-6.2e-06,0.43,0.00044,0.00019,-0.00018,0,0,-4.9e+02,0.00033,0.00034,0.038,0.013,0.014,0.0069,0.037,0.038,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21290000,0.98,-0.0023,-0.0076,0.18,-0.029,0.041,-0.63,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0095,-0.024,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00023,-0.00011,0,0,-4.9e+02,0.00033,0.00034,0.037,0.014,0.015,0.0069,0.04,0.041,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21390000,0.98,-0.0037,-0.0082,0.18,-0.026,0.038,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0096,-0.024,-0.13,0.2,-6.3e-06,0.43,0.00044,0.00015,-0.00012,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.014,0.0068,0.037,0.038,0.034,5.3e-07,4.7e-07,1.7e-06,0.031,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21490000,0.98,-0.0045,-0.0086,0.18,-0.021,0.035,-0.89,0,0,-4.9e+02,-0.0013,-0.0058,-0.0001,0.01,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00044,0.00018,-0.00017,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0068,0.04,0.041,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21590000,0.98,-0.0049,-0.0086,0.18,-0.015,0.032,-1,0,0,-4.9e+02,-0.0013,-0.0058,-9.5e-05,0.01,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00042,0.00023,-0.00018,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.015,0.0067,0.037,0.038,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21690000,0.98,-0.0052,-0.0085,0.18,-0.012,0.027,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,-8.7e-05,0.011,-0.024,-0.13,0.2,-6.3e-06,0.43,0.00039,0.00027,-0.00017,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0067,0.04,0.041,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21790000,0.98,-0.0055,-0.0086,0.18,-0.0066,0.022,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-7.8e-05,0.012,-0.023,-0.13,0.2,-6.3e-06,0.43,0.00039,0.00027,-0.00022,0,0,-4.9e+02,0.00032,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21890000,0.98,-0.0058,-0.0088,0.18,-0.003,0.017,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-8.1e-05,0.012,-0.023,-0.13,0.2,-6.7e-06,0.43,0.00037,0.00029,-0.00021,0,0,-4.9e+02,0.00032,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21990000,0.98,-0.0064,-0.009,0.18,-0.00094,0.013,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-7.5e-05,0.012,-0.022,-0.13,0.2,-6.4e-06,0.43,0.0004,0.0003,-0.00023,0,0,-4.9e+02,0.00031,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22090000,0.98,-0.0071,-0.0099,0.18,0.0011,0.0095,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.6e-05,0.012,-0.022,-0.13,0.2,-6.3e-06,0.43,0.00039,0.00032,-0.00024,0,0,-4.9e+02,0.00031,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22190000,0.98,-0.0075,-0.01,0.18,0.0069,0.0051,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.8e-05,0.013,-0.02,-0.13,0.2,-6.5e-06,0.43,0.0004,0.00033,-0.00024,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0065,0.037,0.038,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22290000,0.98,-0.0082,-0.01,0.18,0.013,-0.00055,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6e-05,0.013,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00033,-0.00023,0,0,-4.9e+02,0.00031,0.00032,0.037,0.014,0.015,0.0065,0.04,0.041,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22390000,0.98,-0.0085,-0.01,0.18,0.017,-0.0091,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.2e-05,0.012,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00041,0.00033,-0.00026,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0064,0.037,0.038,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22490000,0.98,-0.0087,-0.011,0.18,0.021,-0.016,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.6e-05,0.011,-0.02,-0.13,0.2,-6.9e-06,0.43,0.00041,0.00035,-0.00028,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22590000,0.98,-0.0086,-0.012,0.18,0.029,-0.024,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.8e-05,0.012,-0.02,-0.13,0.2,-6.4e-06,0.43,0.00042,0.00037,-0.00027,0,0,-4.9e+02,0.0003,0.00032,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22690000,0.98,-0.0085,-0.012,0.18,0.032,-0.028,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6e-05,0.011,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00042,0.00037,-0.00029,0,0,-4.9e+02,0.0003,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22790000,0.98,-0.0085,-0.012,0.18,0.038,-0.036,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.4e-05,0.011,-0.021,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00034,-0.00031,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22890000,0.98,-0.0087,-0.012,0.18,0.042,-0.041,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.5e-05,0.011,-0.021,-0.13,0.2,-6.2e-06,0.43,0.00041,0.00035,-0.00029,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.015,0.0063,0.04,0.041,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22990000,0.98,-0.0086,-0.013,0.18,0.046,-0.046,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.6e-05,0.012,-0.02,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00032,-0.00027,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0062,0.036,0.038,0.033,4.3e-07,3.9e-07,1.6e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23090000,0.98,-0.0086,-0.013,0.18,0.051,-0.05,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.012,-0.02,-0.13,0.2,-6.6e-06,0.43,0.00039,0.00033,-0.00025,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.014,0.0062,0.04,0.041,0.033,4.3e-07,3.9e-07,1.5e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23190000,0.98,-0.0086,-0.013,0.18,0.057,-0.052,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.9e-05,0.012,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00037,0.0003,-0.00025,0,0,-4.9e+02,0.00029,0.00031,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23290000,0.98,-0.0091,-0.014,0.18,0.062,-0.057,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.012,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00034,0.00034,-0.00025,0,0,-4.9e+02,0.00029,0.00031,0.037,0.013,0.014,0.0062,0.039,0.041,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23390000,0.98,-0.009,-0.014,0.18,0.066,-0.06,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.9e-05,0.012,-0.019,-0.13,0.2,-7.7e-06,0.43,0.00035,0.00032,-0.00023,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23490000,0.98,-0.0091,-0.014,0.18,0.072,-0.062,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.012,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00034,0.00037,-0.00028,0,0,-4.9e+02,0.00029,0.0003,0.037,0.013,0.014,0.0061,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23590000,0.98,-0.0093,-0.014,0.18,0.075,-0.064,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.7e-05,0.013,-0.019,-0.13,0.2,-8.4e-06,0.43,0.00029,0.00034,-0.00024,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23690000,0.98,-0.01,-0.015,0.18,0.074,-0.066,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-3.9e-05,0.013,-0.02,-0.13,0.2,-8.2e-06,0.43,0.00027,0.00036,-0.00024,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.014,0.006,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23790000,0.98,-0.012,-0.017,0.18,0.068,-0.062,-0.95,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.014,-0.02,-0.13,0.2,-7.9e-06,0.43,0.00026,0.00039,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.011,0.013,0.006,0.036,0.037,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23890000,0.98,-0.015,-0.021,0.18,0.064,-0.062,-0.52,0,0,-4.9e+02,-0.0013,-0.0058,-3.5e-05,0.014,-0.02,-0.13,0.2,-7.9e-06,0.43,0.00024,0.00042,-0.00024,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.039,0.041,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23990000,0.98,-0.017,-0.024,0.18,0.063,-0.061,-0.13,0,0,-4.9e+02,-0.0013,-0.0058,-4.2e-05,0.015,-0.02,-0.13,0.2,-8e-06,0.43,0.00021,0.00043,3.6e-06,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.033,3.9e-07,3.5e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24090000,0.98,-0.017,-0.023,0.18,0.07,-0.069,0.099,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.015,-0.019,-0.13,0.2,-8e-06,0.43,0.00022,0.0004,3.6e-06,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.9e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24190000,0.98,-0.014,-0.019,0.18,0.08,-0.074,0.089,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.017,-0.02,-0.13,0.2,-7.6e-06,0.43,0.00022,0.00037,0.0001,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.032,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24290000,0.98,-0.012,-0.016,0.18,0.084,-0.078,0.067,0,0,-4.9e+02,-0.0013,-0.0058,-4.3e-05,0.017,-0.02,-0.13,0.2,-7.2e-06,0.43,0.00026,0.00032,9.8e-05,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24390000,0.98,-0.011,-0.015,0.18,0.077,-0.073,0.083,0,0,-4.9e+02,-0.0013,-0.0058,-4.1e-05,0.018,-0.02,-0.13,0.2,-5.9e-06,0.43,0.00025,0.00037,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24490000,0.98,-0.011,-0.015,0.18,0.073,-0.069,0.081,0,0,-4.9e+02,-0.0013,-0.0058,-2.8e-05,0.019,-0.021,-0.13,0.2,-5.9e-06,0.43,0.00017,0.00047,0.00021,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24590000,0.98,-0.012,-0.015,0.18,0.069,-0.066,0.077,0,0,-4.9e+02,-0.0013,-0.0058,-3.9e-05,0.02,-0.021,-0.13,0.2,-4.6e-06,0.43,0.00021,0.00044,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24690000,0.98,-0.012,-0.015,0.18,0.067,-0.065,0.076,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.02,-0.021,-0.13,0.2,-4.8e-06,0.43,0.0002,0.00047,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24790000,0.98,-0.012,-0.014,0.18,0.064,-0.063,0.068,0,0,-4.9e+02,-0.0014,-0.0058,-4.5e-05,0.022,-0.022,-0.13,0.2,-4.2e-06,0.43,0.00019,0.00048,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24890000,0.98,-0.012,-0.014,0.18,0.063,-0.067,0.057,0,0,-4.9e+02,-0.0014,-0.0058,-4e-05,0.022,-0.022,-0.13,0.2,-4e-06,0.43,0.00019,0.00049,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24990000,0.98,-0.012,-0.014,0.18,0.055,-0.063,0.05,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.023,-0.023,-0.13,0.2,-4.2e-06,0.43,0.00015,0.00059,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25090000,0.98,-0.012,-0.014,0.18,0.051,-0.063,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.024,-0.023,-0.13,0.2,-4.8e-06,0.43,0.00014,0.00067,0.00029,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25190000,0.98,-0.012,-0.014,0.18,0.045,-0.057,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-7.2e-05,0.025,-0.024,-0.13,0.2,-4.8e-06,0.43,0.00012,0.0007,0.00028,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25290000,0.98,-0.012,-0.013,0.18,0.041,-0.059,0.043,0,0,-4.9e+02,-0.0014,-0.0058,-7.9e-05,0.025,-0.024,-0.13,0.2,-5.4e-06,0.43,0.00011,0.00072,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25390000,0.98,-0.012,-0.013,0.18,0.032,-0.052,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9.4e-05,0.027,-0.025,-0.13,0.2,-6.2e-06,0.43,7.1e-05,0.00078,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25490000,0.98,-0.013,-0.013,0.18,0.028,-0.053,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9.6e-05,0.026,-0.025,-0.13,0.2,-5.9e-06,0.43,6.5e-05,0.00073,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25590000,0.98,-0.013,-0.012,0.18,0.023,-0.049,0.042,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-6.7e-06,0.43,3.4e-05,0.00076,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25690000,0.98,-0.012,-0.012,0.18,0.023,-0.048,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-6.8e-06,0.43,4e-05,0.00079,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25790000,0.98,-0.012,-0.012,0.18,0.011,-0.04,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.029,-0.026,-0.13,0.2,-7.3e-06,0.43,-2.1e-05,0.00072,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25890000,0.98,-0.012,-0.012,0.18,0.0059,-0.039,0.033,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.029,-0.026,-0.13,0.2,-7.5e-06,0.43,-4e-05,0.00069,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25990000,0.98,-0.012,-0.012,0.18,-0.0031,-0.032,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.031,-0.026,-0.13,0.2,-8.9e-06,0.43,-9.5e-05,0.00067,0.00014,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26090000,0.98,-0.012,-0.012,0.18,-0.0077,-0.032,0.025,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.031,-0.027,-0.13,0.2,-8.4e-06,0.43,-9.2e-05,0.00066,0.00017,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.025,0.021,0,0,-4.9e+02,-0.0015,-0.0058,-0.00014,0.032,-0.026,-0.13,0.2,-8.7e-06,0.43,-0.00012,0.00066,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.024,0.015,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,0.032,-0.027,-0.13,0.2,-9.3e-06,0.43,-0.00012,0.00067,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26390000,0.98,-0.011,-0.013,0.18,-0.021,-0.017,0.019,0,0,-4.9e+02,-0.0015,-0.0058,-0.00016,0.033,-0.027,-0.13,0.2,-1e-05,0.43,-0.00016,0.00065,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26490000,0.98,-0.011,-0.013,0.18,-0.023,-0.015,0.028,0,0,-4.9e+02,-0.0015,-0.0058,-0.00016,0.033,-0.028,-0.13,0.2,-1.1e-05,0.43,-0.00017,0.00068,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26590000,0.98,-0.01,-0.013,0.18,-0.026,-0.0064,0.029,0,0,-4.9e+02,-0.0015,-0.0058,-0.00017,0.033,-0.028,-0.13,0.2,-1.2e-05,0.43,-0.00019,0.0007,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26690000,0.98,-0.01,-0.012,0.18,-0.027,-0.0023,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.033,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00021,0.00071,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26790000,0.98,-0.0099,-0.012,0.18,-0.035,0.0019,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00023,0.00069,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26890000,0.98,-0.0092,-0.012,0.18,-0.04,0.0048,0.022,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.2e-05,0.43,-0.00023,0.00067,0.00011,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26990000,0.98,-0.0087,-0.013,0.18,-0.048,0.012,0.021,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00025,0.00065,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27090000,0.98,-0.0086,-0.013,0.18,-0.05,0.018,0.025,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.035,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00025,0.00065,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27190000,0.98,-0.0086,-0.013,0.18,-0.056,0.025,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00026,0.00064,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27290000,0.98,-0.0088,-0.014,0.18,-0.063,0.03,0.14,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00027,0.00064,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0054,0.038,0.039,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27390000,0.98,-0.01,-0.016,0.18,-0.07,0.037,0.46,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00033,0.00057,0.00018,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27490000,0.98,-0.012,-0.018,0.18,-0.074,0.043,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.13,0.2,-1.6e-05,0.43,-0.0003,0.00053,0.00018,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27590000,0.98,-0.011,-0.017,0.18,-0.069,0.046,0.87,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00048,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27690000,0.98,-0.01,-0.014,0.18,-0.065,0.042,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.5e-05,0.43,-0.00027,0.00047,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27790000,0.98,-0.0088,-0.012,0.18,-0.065,0.041,0.77,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00043,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27890000,0.98,-0.0085,-0.013,0.18,-0.071,0.047,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00043,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27990000,0.98,-0.0089,-0.013,0.18,-0.072,0.049,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00037,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28090000,0.98,-0.0092,-0.013,0.18,-0.076,0.05,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00039,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28190000,0.98,-0.0086,-0.013,0.18,-0.077,0.047,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.033,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00026,0.00038,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28290000,0.98,-0.0081,-0.013,0.18,-0.081,0.051,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.034,-0.029,-0.12,0.2,-1.5e-05,0.43,-0.00027,0.0004,0.00019,0,0,-4.9e+02,0.00029,0.00029,0.037,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28390000,0.98,-0.0081,-0.014,0.18,-0.082,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.033,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00031,0.00031,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28490000,0.98,-0.0084,-0.015,0.18,-0.084,0.058,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.033,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00028,0.0003,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28590000,0.98,-0.0085,-0.014,0.18,-0.078,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00025,0.00028,0.00022,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28690000,0.98,-0.0082,-0.014,0.18,-0.078,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00022,0.00028,0.00022,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.032,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00025,0.00018,0.00022,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28890000,0.98,-0.0074,-0.013,0.18,-0.08,0.058,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.032,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00025,0.00021,0.0002,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28990000,0.98,-0.0072,-0.013,0.18,-0.077,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.031,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00028,6.7e-05,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29090000,0.98,-0.007,-0.013,0.18,-0.08,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.031,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.0003,4.9e-05,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.03,-0.029,-0.12,0.2,-2e-05,0.43,-0.00033,-8.1e-05,0.00024,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.03,-0.028,-0.12,0.2,-2e-05,0.43,-0.00032,-0.0001,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29390000,0.98,-0.0076,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.029,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00036,-0.00023,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29490000,0.98,-0.0076,-0.013,0.18,-0.08,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,0.029,-0.028,-0.12,0.2,-2.1e-05,0.43,-0.00038,-0.0002,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-8.6e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00043,-0.00031,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-7.9e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00045,-0.00027,0.00021,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-6.3e-05,0.027,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.0005,-0.00039,0.00021,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29890000,0.98,-0.0068,-0.013,0.18,-0.08,0.056,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.7e-05,0.028,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00052,-0.00036,0.00019,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29990000,0.98,-0.007,-0.013,0.18,-0.075,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-4.5e-05,0.026,-0.029,-0.12,0.2,-2.6e-05,0.43,-0.00051,-0.00041,0.00021,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30090000,0.98,-0.0071,-0.013,0.18,-0.077,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.6e-05,0.026,-0.029,-0.12,0.2,-2.7e-05,0.43,-0.00046,-0.00048,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30190000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.8e-05,0.025,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00046,-0.00069,0.00032,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30290000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.7e-05,0.026,-0.029,-0.12,0.2,-2.8e-05,0.43,-0.0005,-0.00074,0.00032,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30390000,0.98,-0.0071,-0.013,0.18,-0.065,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-3.9e-05,0.025,-0.029,-0.12,0.2,-3e-05,0.43,-0.00064,-0.00095,0.00032,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30490000,0.98,-0.0071,-0.013,0.18,-0.068,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-3.3e-05,0.025,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00068,-0.00094,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30590000,0.98,-0.0074,-0.014,0.18,-0.064,0.041,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.8e-05,0.024,-0.03,-0.12,0.2,-3e-05,0.43,-0.00079,-0.001,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30690000,0.98,-0.0078,-0.014,0.18,-0.062,0.04,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.7e-05,0.025,-0.03,-0.12,0.2,-3e-05,0.43,-0.00076,-0.00099,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30790000,0.98,-0.0075,-0.013,0.17,-0.056,0.034,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.3e-05,0.024,-0.031,-0.12,0.2,-3.1e-05,0.43,-0.00084,-0.0012,0.00033,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30890000,0.98,-0.0069,-0.013,0.17,-0.056,0.031,0.79,0,0,-4.9e+02,-0.0014,-0.0058,-2e-05,0.024,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00075,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30990000,0.98,-0.0071,-0.013,0.17,-0.049,0.025,0.8,0,0,-4.9e+02,-0.0014,-0.0057,-1.3e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00078,-0.0012,0.00038,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31090000,0.98,-0.0073,-0.013,0.17,-0.048,0.024,0.79,0,0,-4.9e+02,-0.0014,-0.0057,-1.7e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00073,-0.0012,0.00039,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31190000,0.98,-0.0075,-0.013,0.17,-0.043,0.02,0.8,0,0,-4.9e+02,-0.0013,-0.0057,-1.2e-06,0.023,-0.033,-0.12,0.2,-3.4e-05,0.43,-0.00083,-0.0012,0.00039,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.018,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3.9e-06,0.024,-0.033,-0.12,0.2,-3.3e-05,0.43,-0.00086,-0.0011,0.00037,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3.2e-06,0.023,-0.033,-0.12,0.2,-3.2e-05,0.43,-0.00095,-0.0015,0.00042,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0081,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.5e-06,0.023,-0.033,-0.12,0.2,-3.1e-05,0.43,-0.001,-0.0017,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31590000,0.98,-0.0071,-0.014,0.17,-0.033,0.0062,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.1e-05,0.023,-0.034,-0.12,0.2,-3.1e-05,0.43,-0.0011,-0.0018,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31690000,0.98,-0.0071,-0.014,0.17,-0.036,0.0056,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.9e-05,0.023,-0.033,-0.12,0.2,-3.1e-05,0.43,-0.0012,-0.0018,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31790000,0.98,-0.0074,-0.015,0.17,-0.026,0.003,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.023,-0.034,-0.12,0.2,-3e-05,0.43,-0.0013,-0.0019,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31890000,0.99,-0.0071,-0.015,0.17,-0.025,0.0008,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.8e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.002,0.00047,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31990000,0.99,-0.0074,-0.014,0.17,-0.017,-0.00029,0.79,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.0022,0.00053,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32090000,0.99,-0.0077,-0.014,0.17,-0.019,-0.0037,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.6e-05,0.024,-0.034,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0022,0.00054,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32190000,0.99,-0.008,-0.014,0.17,-0.013,-0.0073,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.1e-05,0.024,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0024,0.00058,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32290000,0.99,-0.0079,-0.014,0.17,-0.013,-0.0099,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.6e-05,0.024,-0.034,-0.12,0.2,-2.8e-05,0.43,-0.0015,-0.0025,0.00059,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32390000,0.99,-0.008,-0.015,0.17,-0.0063,-0.012,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.025,-0.035,-0.12,0.2,-2.6e-05,0.43,-0.0016,-0.0027,0.00065,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32490000,0.99,-0.011,-0.012,0.17,0.018,-0.077,-0.076,0,0,-4.9e+02,-0.0013,-0.0057,1.9e-05,0.025,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0015,-0.0026,0.00064,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32590000,0.99,-0.011,-0.013,0.17,0.021,-0.079,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,1.9e-05,0.025,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32690000,0.99,-0.011,-0.012,0.17,0.016,-0.085,-0.08,0,0,-4.9e+02,-0.0013,-0.0057,1.8e-05,0.025,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32790000,0.99,-0.011,-0.013,0.17,0.017,-0.084,-0.081,0,0,-4.9e+02,-0.0013,-0.0057,1.6e-05,0.025,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0022,0.00078,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0,0,-4.9e+02,-0.0013,-0.0057,2e-05,0.025,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0021,0.00079,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.025,-0.035,-0.12,0.2,-2.3e-05,0.43,-0.0015,-0.002,0.00085,0,0,-4.9e+02,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +33090000,0.98,-0.011,-0.013,0.17,0.013,-0.093,-0.079,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.025,-0.035,-0.12,0.2,-2.3e-05,0.43,-0.0015,-0.002,0.00085,0,0,-4.9e+02,0.00027,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,1.3e-05,0.026,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0015,-0.002,0.00088,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33290000,0.98,-0.01,-0.013,0.17,0.0073,-0.096,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,2.3e-05,0.026,-0.034,-0.12,0.2,-2e-05,0.43,-0.0016,-0.002,0.00092,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33390000,0.98,-0.01,-0.013,0.17,0.0051,-0.09,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,1.6e-05,0.028,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0016,-0.002,0.00096,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 +33490000,0.98,-0.01,-0.013,0.17,-0.00023,-0.093,-0.075,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.028,-0.032,-0.12,0.2,-1.6e-05,0.43,-0.0017,-0.0021,0.00099,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 +33590000,0.98,-0.01,-0.013,0.17,-0.003,-0.092,-0.072,0,0,-4.9e+02,-0.0014,-0.0057,2e-05,0.029,-0.032,-0.12,0.2,-1.3e-05,0.43,-0.0018,-0.0022,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.083 +33690000,0.98,-0.01,-0.013,0.17,-0.0063,-0.095,-0.073,0,0,-4.9e+02,-0.0014,-0.0057,2.4e-05,0.029,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.0018,-0.002,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.11 +33790000,0.98,-0.01,-0.013,0.17,-0.0082,-0.092,-0.068,0,0,-4.9e+02,-0.0014,-0.0057,1.4e-05,0.031,-0.032,-0.12,0.2,-1.2e-05,0.43,-0.0017,-0.0018,0.0011,0,0,-4.9e+02,0.00027,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.13 +33890000,0.98,-0.01,-0.013,0.17,-0.012,-0.093,-0.067,0,0,-4.9e+02,-0.0014,-0.0057,2.4e-05,0.031,-0.032,-0.12,0.2,-1.1e-05,0.43,-0.0018,-0.0018,0.0011,0,0,-4.9e+02,0.00027,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.4e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.16 +33990000,0.98,-0.0099,-0.013,0.17,-0.013,-0.088,-0.064,0,0,-4.9e+02,-0.0014,-0.0057,1.2e-05,0.033,-0.031,-0.12,0.2,-8.4e-06,0.43,-0.0017,-0.0018,0.0011,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.18 +34090000,0.98,-0.0098,-0.013,0.17,-0.017,-0.093,-0.062,0,0,-4.9e+02,-0.0014,-0.0057,1.5e-05,0.033,-0.031,-0.12,0.2,-8.8e-06,0.43,-0.0017,-0.0017,0.0011,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.21 +34190000,0.98,-0.0098,-0.013,0.17,-0.018,-0.088,-0.06,0,0,-4.9e+02,-0.0014,-0.0057,1e-05,0.035,-0.031,-0.12,0.2,-6.4e-06,0.43,-0.0016,-0.0015,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.23 +34290000,0.98,-0.0096,-0.013,0.17,-0.019,-0.092,-0.058,0,0,-4.9e+02,-0.0014,-0.0057,1.7e-05,0.035,-0.031,-0.12,0.2,-5.7e-06,0.43,-0.0017,-0.0016,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.26 +34390000,0.98,-0.0095,-0.013,0.17,-0.021,-0.085,-0.054,0,0,-4.9e+02,-0.0014,-0.0056,9.9e-06,0.036,-0.03,-0.12,0.2,-3.6e-06,0.43,-0.0016,-0.0015,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.28 +34490000,0.98,-0.0095,-0.013,0.17,-0.024,-0.089,-0.052,0,0,-4.9e+02,-0.0014,-0.0056,1.8e-05,0.036,-0.03,-0.12,0.2,-3.3e-06,0.43,-0.0017,-0.0014,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.31 +34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.081,0.74,0,0,-4.9e+02,-0.0014,-0.0056,1.1e-05,0.038,-0.03,-0.12,0.2,-8.1e-07,0.43,-0.0016,-0.0014,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 +34690000,0.98,-0.0094,-0.013,0.17,-0.03,-0.079,1.7,0,0,-4.9e+02,-0.0014,-0.0056,1.4e-05,0.038,-0.03,-0.12,0.2,-5.2e-07,0.43,-0.0017,-0.0014,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 +34790000,0.98,-0.0093,-0.013,0.17,-0.033,-0.07,2.7,0,0,-4.9e+02,-0.0015,-0.0056,7.9e-06,0.04,-0.031,-0.12,0.2,2.1e-06,0.43,-0.0017,-0.0013,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 +34890000,0.98,-0.0092,-0.013,0.17,-0.038,-0.069,3.7,0,0,-4.9e+02,-0.0015,-0.0056,1.1e-05,0.04,-0.031,-0.12,0.2,2.2e-06,0.43,-0.0017,-0.0013,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 From 842212df6c68d6f90c2a3b63b8b028e71c0f86c4 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2024 09:20:19 +0100 Subject: [PATCH 050/211] ekf2: update earth rate also without GNSS --- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 15 --------------- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 6 ++++-- src/modules/ekf2/EKF/ekf.cpp | 5 +++++ src/modules/ekf2/EKF/ekf.h | 2 -- 4 files changed, 9 insertions(+), 19 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index c1b47c224fa7..e21ee82ff9f2 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -55,21 +55,6 @@ #define MASK_GPS_VSPD (1<<8) #define MASK_GPS_SPOOFED (1<<9) -void Ekf::collect_gps(const gnssSample &gps) -{ - if (_filter_initialised && !_local_origin_lat_lon.isInitialized() && _gps_checks_passed) { - _information_events.flags.gps_checks_passed = true; - - } else { - // a rough 2D fix is sufficient to lookup earth spin rate - const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - - if (gps_rough_2d_fix && (_gps_checks_passed || !_local_origin_lat_lon.isInitialized())) { - _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); - } - } -} - bool Ekf::runGnssChecks(const gnssSample &gps) { _gps_check_fail_status.flags.spoofed = gps.spoofed; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index de8dad37b5e8..889af97a08ce 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -67,11 +67,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { // First time checks are passing, latching. + if (!_gps_checks_passed) { + _information_events.flags.gps_checks_passed = true; + } + _gps_checks_passed = true; } - collect_gps(gnss_sample); - } else { // Skip this sample _gps_data_ready = false; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index e1fb96784fd6..d883d83ff8be 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -230,6 +230,11 @@ bool Ekf::initialiseTilt() void Ekf::predictState(const imuSample &imu_delayed) { + if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) { + _earth_rate_lat_ref_rad = _gpos.latitude_rad(); + _earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad); + } + // apply imu bias corrections const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt; Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a76c0e351782..c2277606b108 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -374,8 +374,6 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - void collect_gps(const gnssSample &gps); - // 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; } From 814a2706f55af62c281cfb1f70e38c3db4d91e78 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2024 09:21:23 +0100 Subject: [PATCH 051/211] ekf2: compensate for coriolis and transport rate accelerations --- src/modules/ekf2/EKF/ekf.cpp | 7 +++++-- src/modules/ekf2/EKF/ekf.h | 3 ++- src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp | 15 +++++++++++++++ 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index d883d83ff8be..c3d4434a714d 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -259,8 +259,11 @@ void Ekf::predictState(const imuSample &imu_delayed) // calculate the increment in velocity using the current orientation _state.vel += corrected_delta_vel_ef; - // compensate for acceleration due to gravity - _state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt; + // compensate for acceleration due to gravity, Coriolis and transport rate + const Vector3f gravity_acceleration(0.f, 0.f, CONSTANTS_ONE_G); // simplistic model + const Vector3f coriolis_acceleration = -2.f * _earth_rate_NED.cross(vel_last); + const Vector3f transport_rate = -_gpos.computeAngularRateNavFrame(vel_last).cross(vel_last); + _state.vel += (gravity_acceleration + coriolis_acceleration + transport_rate) * imu_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity _gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c2277606b108..47c6e10f94b6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -487,7 +487,8 @@ class Ekf final : public EstimatorInterface LatLonAlt _last_known_gpos{}; - Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s + Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s + double _earth_rate_lat_ref_rad{0.0}; ///< latitude at which the earth rate was evaluated (radians) Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp index 0fa7115df77a..bb6a384d6ac6 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -120,6 +120,21 @@ class LatLonAlt -delta_alt); } + /* + * Compute the angular rate of the local navigation frame at the current latitude and height + * with respect to an inertial frame and resolved in the navigation frame + */ + matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) + { + double r_n; + double r_e; + computeRadiiOfCurvature(_latitude_rad, r_n, r_e); + return matrix::Vector3f( + v_ned(1) / (static_cast(r_e) + _altitude), + -v_ned(0) / (static_cast(r_n) + _altitude), + -v_ned(1) * tanf(_latitude_rad) / (static_cast(r_e) + _altitude)); + } + private: // Convert between curvilinear and cartesian errors static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude) From 30d98885b70b410e8a5fca712abdb1c6a511e423 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Nov 2024 15:15:19 +0100 Subject: [PATCH 052/211] update change indicator --- .../test/change_indication/ekf_gsf_reset.csv | 748 +++++++++--------- .../ekf2/test/change_indication/iris_gps.csv | 664 ++++++++-------- 2 files changed, 706 insertions(+), 706 deletions(-) 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 fcf346c1e89d..fbc6d2b054a5 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -14,378 +14,378 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0,0,-1.2e+02,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00051,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0,0,-1.2e+02,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.00038,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0,0,-1.2e+02,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00043,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 -1490000,1,-0.01,-0.014,0.00016,0.024,0.0029,-0.16,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1490000,1,-0.01,-0.014,0.00015,0.024,0.0029,-0.16,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.03,0.03,0.00037,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 -1690000,1,-0.011,-0.014,0.00013,0.028,-0.00014,-0.19,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 -1790000,1,-0.011,-0.014,9.8e-05,0.035,-0.002,-0.2,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 -1890000,1,-0.011,-0.015,7.8e-05,0.043,-0.0033,-0.22,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 -1990000,1,-0.011,-0.014,8.9e-05,0.036,-0.0048,-0.23,0,0,-1.2e+02,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 -2090000,1,-0.011,-0.014,5.1e-05,0.041,-0.0073,-0.24,0,0,-1.2e+02,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 -2190000,1,-0.011,-0.014,6.4e-05,0.033,-0.007,-0.26,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 -2290000,1,-0.011,-0.014,5.1e-05,0.039,-0.0095,-0.27,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 -2390000,1,-0.011,-0.013,6.9e-05,0.03,-0.0089,-0.29,0,0,-1.2e+02,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 -2490000,1,-0.011,-0.013,5.1e-05,0.035,-0.011,-0.3,0,0,-1.2e+02,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 -2590000,1,-0.011,-0.013,6.6e-05,0.026,-0.0093,-0.31,0,0,-1.2e+02,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 -2690000,1,-0.011,-0.013,6.3e-05,0.03,-0.011,-0.33,0,0,-1.2e+02,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 -2790000,1,-0.011,-0.013,5.8e-05,0.023,-0.0096,-0.34,0,0,-1.2e+02,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 -2890000,1,-0.011,-0.013,8.3e-06,0.027,-0.012,-0.35,0,0,-1.2e+02,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 -2990000,1,-0.011,-0.013,5.6e-05,0.022,-0.0098,-0.36,0,0,-1.2e+02,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 -3090000,1,-0.011,-0.013,6e-05,0.025,-0.011,-0.38,0,0,-1.2e+02,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 -3190000,1,-0.011,-0.013,2.9e-06,0.02,-0.009,-0.39,0,0,-1.2e+02,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 -3290000,1,-0.011,-0.013,4.4e-05,0.023,-0.011,-0.4,0,0,-1.2e+02,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 -3390000,1,-0.011,-0.012,1.5e-05,0.018,-0.0095,-0.42,0,0,-1.2e+02,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 -3490000,1,-0.011,-0.013,8.2e-06,0.022,-0.012,-0.43,0,0,-1.2e+02,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 -3590000,1,-0.011,-0.012,3.2e-05,0.017,-0.011,-0.44,0,0,-1.2e+02,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 -3690000,1,-0.011,-0.012,0.00015,0.019,-0.014,-0.46,0,0,-1.2e+02,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 -3790000,1,-0.011,-0.012,0.0002,0.016,-0.014,-0.47,0,0,-1.2e+02,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 -3890000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.48,0,0,-1.2e+02,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -3990000,1,-0.011,-0.012,0.00017,0.02,-0.017,-0.5,0,0,-1.2e+02,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -4090000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.51,0,0,-1.2e+02,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4190000,1,-0.011,-0.012,0.00014,0.02,-0.017,-0.53,0,0,-1.2e+02,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4290000,1,-0.01,-0.012,9.8e-05,0.017,-0.012,-0.54,0,0,-1.2e+02,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4390000,1,-0.01,-0.012,0.00012,0.018,-0.013,-0.55,0,0,-1.2e+02,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4490000,1,-0.01,-0.012,0.00018,0.014,-0.01,-0.57,0,0,-1.2e+02,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4590000,1,-0.01,-0.012,0.00021,0.017,-0.012,-0.58,0,0,-1.2e+02,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4690000,1,-0.01,-0.012,0.00021,0.014,-0.01,-0.6,0,0,-1.2e+02,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4790000,1,-0.01,-0.012,0.0002,0.015,-0.012,-0.61,0,0,-1.2e+02,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4890000,1,-0.01,-0.011,0.00019,0.012,-0.01,-0.63,0,0,-1.2e+02,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -4990000,1,-0.01,-0.012,0.00017,0.015,-0.011,-0.64,0,0,-1.2e+02,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5090000,1,-0.01,-0.011,0.00022,0.011,-0.0085,-0.66,0,0,-1.2e+02,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5190000,1,-0.01,-0.011,0.00024,0.013,-0.0098,-0.67,0,0,-1.2e+02,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5290000,1,-0.01,-0.011,0.00023,0.0086,-0.0073,-0.68,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5390000,1,-0.0099,-0.011,0.00029,0.0081,-0.0081,-0.7,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5490000,1,-0.0098,-0.011,0.0003,0.0055,-0.0062,-0.71,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5590000,1,-0.0097,-0.011,0.00028,0.0061,-0.0066,-0.73,0,0,-1.2e+02,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5690000,1,-0.0096,-0.011,0.00036,0.0041,-0.0038,-0.74,0,0,-1.2e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5790000,1,-0.0095,-0.011,0.00035,0.0044,-0.0028,-0.75,0,0,-1.2e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5890000,1,-0.0095,-0.011,0.00033,0.0038,-0.00092,0.0028,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5990000,1,-0.0094,-0.011,0.00036,0.0041,0.00056,0.015,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -6090000,1,-0.0094,-0.011,0.00034,0.0051,0.0017,-0.011,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6190000,1,-0.0094,-0.011,0.00027,0.0038,0.0042,-0.005,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6290000,1,-0.0094,-0.011,0.00024,0.005,0.0043,-0.012,0,0,-4.9e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6390000,1,-0.0093,-0.011,0.00026,0.0043,0.0053,-0.05,0,0,-4.9e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6490000,1,-0.0093,-0.011,0.00026,0.0049,0.0055,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6590000,1,-0.0093,-0.011,0.00019,0.0037,0.0055,-0.099,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6690000,1,-0.0093,-0.011,0.00012,0.0046,0.0051,-0.076,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0051,-0.11,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,-6.9e-05,0.21,8e-05,0.43,3.6e-05,0.00032,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 -6890000,0.71,0.0013,-0.014,0.7,0.00053,0.0064,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,0,0,-9.7e-05,0.21,1.3e-05,0.43,1.3e-06,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 -6990000,0.71,0.0013,-0.014,0.71,-0.00023,0.0046,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 -7090000,0.71,0.0012,-0.014,0.71,-0.00079,0.0011,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00037,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 -7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0019,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.7e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 -7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.0099,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00021,-0.0002,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00039,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00028,-3.8e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00032,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.011,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00018,-7.3e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00078,-0.00043,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7590000,0.71,0.0016,-0.014,0.71,-0.0034,0.02,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-6.8e-05,-0.00024,0.00021,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7690000,0.71,0.0016,-0.014,0.71,-0.0046,0.017,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00032,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 -7790000,0.71,0.0017,-0.014,0.71,-0.011,0.016,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00036,6.2e-05,-0.0071,0.21,-1.6e-05,0.43,-0.00034,0.00078,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 -7890000,0.71,0.0017,-0.014,0.71,-0.011,0.021,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00038,0.0002,-0.0096,0.21,-1.5e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 -7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00037,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 -8090000,0.71,0.0017,-0.014,0.71,-0.005,0.024,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.7e-05,-0.0003,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00033,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8190000,0.71,0.0017,-0.014,0.71,-0.014,0.028,-0.18,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00043,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8290000,0.71,0.0017,-0.014,0.71,-0.017,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00056,0.00031,-0.017,0.21,-1e-05,0.43,-0.00031,0.00079,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00041,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.021,0.21,-9.3e-06,0.43,-0.0003,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8490000,0.71,0.0017,-0.014,0.71,-0.003,0.0023,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.025,0.21,-9e-06,0.43,-0.00031,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8590000,0.71,0.0021,-0.014,0.71,-0.00044,0.00093,-0.17,0,0,-4.9e+02,-0.0018,-0.0057,-6.8e-05,-0.00051,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00042,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8690000,0.71,0.002,-0.014,0.71,-0.0034,0.0028,-0.16,0,0,-4.9e+02,-0.0017,-0.0056,-6.6e-05,-0.00051,0.00039,-0.035,0.21,-1e-05,0.43,-0.00039,0.00084,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.088,5.8e-05,6.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8790000,0.71,0.0019,-0.014,0.71,-0.0057,0.0049,-0.15,0,0,-4.9e+02,-0.0017,-0.0057,-6.9e-05,-0.0005,0.00043,-0.041,0.21,-9.1e-06,0.43,-0.00037,0.0008,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.087,5.6e-05,6.6e-05,2.4e-06,0.04,0.04,0.032,0.0013,6.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8890000,0.71,0.0019,-0.014,0.71,-0.0079,0.0055,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00063,0.00042,-0.045,0.21,-7.8e-06,0.43,-0.00034,0.00078,-0.00033,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.086,5.3e-05,6.4e-05,2.4e-06,0.04,0.04,0.03,0.0013,6.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -8990000,0.71,0.0018,-0.014,0.71,-0.01,0.0049,-0.14,0,0,-4.9e+02,-0.0015,-0.0058,-7.5e-05,-0.00087,0.00044,-0.051,0.21,-6.6e-06,0.43,-0.0003,0.00071,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.087,5.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.029,0.0013,6.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9090000,0.71,0.002,-0.014,0.71,-0.014,0.006,-0.14,0,0,-4.9e+02,-0.0016,-0.0058,-7.6e-05,-0.00098,0.00054,-0.053,0.21,-6.9e-06,0.43,-0.00033,0.00068,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1.1e+02,1.1e+02,0.086,4.9e-05,6.2e-05,2.4e-06,0.04,0.04,0.028,0.0013,5.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9190000,0.71,0.0019,-0.014,0.71,-0.012,0.0087,-0.14,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.00086,0.00059,-0.057,0.21,-6.2e-06,0.43,-0.00031,0.00081,-0.00025,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.094,1.1e+02,1.1e+02,0.085,4.6e-05,6e-05,2.4e-06,0.04,0.04,0.027,0.0013,5.6e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9290000,0.71,0.0017,-0.014,0.71,-0.011,0.0086,-0.14,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00096,0.00059,-0.061,0.21,-5.1e-06,0.43,-0.00026,0.00082,-0.00024,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.1e+02,1.1e+02,0.085,4.4e-05,5.8e-05,2.4e-06,0.04,0.04,0.025,0.0013,5.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9390000,0.71,0.0015,-0.014,0.71,-0.011,0.0092,-0.14,0,0,-4.9e+02,-0.0014,-0.0056,-7.3e-05,-0.001,0.00058,-0.065,0.21,-4.3e-06,0.43,-0.00022,0.00082,-0.00022,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.2e+02,1.2e+02,0.086,4.2e-05,5.7e-05,2.4e-06,0.04,0.04,0.024,0.0013,5.2e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9490000,0.71,0.0014,-0.014,0.71,-0.0078,0.011,-0.13,0,0,-4.9e+02,-0.0014,-0.0055,-6.9e-05,-0.001,0.0006,-0.068,0.21,-3.7e-06,0.43,-0.00018,0.0009,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.091,1.2e+02,1.2e+02,0.085,4e-05,5.5e-05,2.4e-06,0.04,0.04,0.023,0.0013,5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9590000,0.71,0.0016,-0.014,0.71,-0.01,0.016,-0.13,0,0,-4.9e+02,-0.0015,-0.0054,-6.6e-05,-0.0011,0.00079,-0.072,0.21,-4.5e-06,0.43,-0.00024,0.00091,-0.00012,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.09,1.3e+02,1.3e+02,0.085,3.8e-05,5.4e-05,2.4e-06,0.04,0.04,0.021,0.0013,4.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9690000,0.71,0.0018,-0.014,0.71,-0.017,0.017,-0.12,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.0013,0.00092,-0.077,0.21,-4.8e-06,0.43,-0.00027,0.0008,-0.00017,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.089,1.3e+02,1.3e+02,0.086,3.6e-05,5.2e-05,2.4e-06,0.04,0.04,0.02,0.0013,4.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9790000,0.71,0.0017,-0.014,0.71,-0.013,0.02,-0.11,0,0,-4.9e+02,-0.0015,-0.0055,-6.8e-05,-0.0014,0.001,-0.082,0.21,-4.4e-06,0.43,-0.00024,0.00085,-9.7e-05,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.087,1.4e+02,1.4e+02,0.085,3.4e-05,5e-05,2.4e-06,0.04,0.04,0.019,0.0013,4.5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9890000,0.71,0.0018,-0.014,0.71,-0.016,0.023,-0.11,0,0,-4.9e+02,-0.0015,-0.0055,-6.8e-05,-0.0015,0.0011,-0.085,0.21,-4.6e-06,0.43,-0.00026,0.00083,-0.0001,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.084,1.4e+02,1.4e+02,0.085,3.2e-05,4.9e-05,2.4e-06,0.04,0.04,0.018,0.0013,4.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9990000,0.71,0.0019,-0.014,0.71,-0.02,0.021,-0.1,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.0016,0.0011,-0.089,0.21,-4.2e-06,0.43,-0.00025,0.00078,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.083,1.5e+02,1.5e+02,0.086,3e-05,4.7e-05,2.4e-06,0.04,0.04,0.017,0.0013,4.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -10090000,0.71,0.002,-0.014,0.71,-0.022,0.025,-0.096,0,0,-4.9e+02,-0.0015,-0.0056,-6.9e-05,-0.0017,0.0013,-0.091,0.21,-4.6e-06,0.43,-0.00029,0.00079,-0.00012,0,0,-4.9e+02,0.0013,0.0012,0.039,25,25,0.08,1.6e+02,1.6e+02,0.085,2.9e-05,4.6e-05,2.4e-06,0.04,0.04,0.016,0.0013,4.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10190000,0.71,0.002,-0.013,0.71,-0.033,0.022,-0.096,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.0019,0.0013,-0.093,0.21,-4.5e-06,0.43,-0.00027,0.00065,-0.00015,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.078,1.7e+02,1.7e+02,0.084,2.7e-05,4.4e-05,2.4e-06,0.04,0.04,0.014,0.0013,4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10290000,0.71,0.002,-0.013,0.71,-0.04,0.017,-0.084,0,0,-4.9e+02,-0.0015,-0.0058,-7.8e-05,-0.0021,0.0014,-0.098,0.21,-4.2e-06,0.43,-0.00026,0.00058,-0.00017,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.076,1.8e+02,1.8e+02,0.085,2.6e-05,4.2e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10390000,0.71,0.0018,-0.013,0.71,0.0091,-0.02,0.0093,0,0,-4.9e+02,-0.0015,-0.0058,-7.9e-05,-0.0021,0.0013,-0.098,0.21,-3.7e-06,0.43,-0.00023,0.0006,-0.00014,0,0,-4.9e+02,0.0012,0.0012,0.039,0.25,0.25,0.56,0.5,0.5,0.077,2.4e-05,4.1e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10490000,0.71,0.0018,-0.013,0.71,0.0071,-0.019,0.022,0,0,-4.9e+02,-0.0014,-0.0059,-8.3e-05,-0.0022,0.0013,-0.098,0.21,-3.3e-06,0.43,-0.00021,0.00053,-0.00016,0,0,-4.9e+02,0.0012,0.0012,0.039,0.26,0.26,0.55,0.51,0.51,0.079,2.3e-05,3.9e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10590000,0.71,0.0022,-0.013,0.71,0.006,-0.0082,0.024,0,0,-4.9e+02,-0.0016,-0.0059,-7.9e-05,-0.0024,0.0018,-0.098,0.21,-4.5e-06,0.43,-0.00029,0.00053,-0.00017,0,0,-4.9e+02,0.0012,0.0011,0.039,0.13,0.13,0.27,0.17,0.17,0.072,2.2e-05,3.8e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10690000,0.71,0.0021,-0.013,0.71,0.0033,-0.009,0.027,0,0,-4.9e+02,-0.0015,-0.0059,-8.1e-05,-0.0024,0.0018,-0.098,0.21,-4.1e-06,0.43,-0.00027,0.00052,-0.00017,0,0,-4.9e+02,0.0012,0.0011,0.039,0.14,0.14,0.26,0.18,0.18,0.077,2.1e-05,3.6e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10790000,0.71,0.002,-0.013,0.71,0.0034,-0.0064,0.021,0,0,-4.9e+02,-0.0015,-0.0059,-8e-05,-0.0025,0.0021,-0.098,0.21,-4e-06,0.43,-0.00026,0.00056,-0.00016,0,0,-4.9e+02,0.0012,0.0011,0.038,0.093,0.094,0.17,0.11,0.11,0.071,1.9e-05,3.4e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10890000,0.71,0.0019,-0.013,0.71,0.0021,-0.0066,0.016,0,0,-4.9e+02,-0.0015,-0.0058,-8.1e-05,-0.0025,0.0021,-0.098,0.21,-3.7e-06,0.43,-0.00025,0.00057,-0.00015,0,0,-4.9e+02,0.0011,0.0011,0.038,0.1,0.1,0.16,0.11,0.11,0.074,1.8e-05,3.3e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -10990000,0.71,0.0017,-0.014,0.71,0.0068,-0.0015,0.011,0,0,-4.9e+02,-0.0013,-0.0057,-7.9e-05,-0.0025,0.0031,-0.098,0.21,-3.2e-06,0.43,-0.00022,0.00071,-0.00011,0,0,-4.9e+02,0.0011,0.001,0.038,0.079,0.08,0.12,0.079,0.079,0.071,1.7e-05,3.1e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11090000,0.71,0.0017,-0.014,0.71,0.0068,0.0018,0.014,0,0,-4.9e+02,-0.0014,-0.0056,-7.5e-05,-0.0023,0.0029,-0.098,0.21,-3.3e-06,0.43,-0.00023,0.00078,-6.8e-05,0,0,-4.9e+02,0.0011,0.00099,0.038,0.09,0.091,0.12,0.085,0.085,0.073,1.6e-05,2.9e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11190000,0.71,0.0017,-0.014,0.71,0.01,0.0034,0.0027,0,0,-4.9e+02,-0.0013,-0.0056,-7.8e-05,-0.0023,0.004,-0.098,0.21,-3.4e-06,0.43,-0.00024,0.00079,-8.6e-05,0,0,-4.9e+02,0.00098,0.00092,0.038,0.074,0.075,0.091,0.066,0.066,0.068,1.5e-05,2.7e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11290000,0.71,0.0016,-0.014,0.71,0.0094,0.0018,0.0014,0,0,-4.9e+02,-0.0013,-0.0057,-8.3e-05,-0.0026,0.0043,-0.098,0.21,-3e-06,0.43,-0.00022,0.00073,-0.00011,0,0,-4.9e+02,0.00098,0.00091,0.038,0.086,0.087,0.087,0.072,0.072,0.071,1.4e-05,2.6e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11390000,0.71,0.0016,-0.013,0.71,0.0049,0.00087,-0.0044,0,0,-4.9e+02,-0.0013,-0.0058,-8.6e-05,-0.0033,0.0041,-0.098,0.21,-3e-06,0.43,-0.00021,0.00066,-0.00016,0,0,-4.9e+02,0.00087,0.00082,0.038,0.072,0.073,0.072,0.058,0.058,0.067,1.3e-05,2.4e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11490000,0.71,0.0014,-0.013,0.71,0.0011,-0.002,-0.0044,0,0,-4.9e+02,-0.0012,-0.0059,-9.3e-05,-0.0039,0.0049,-0.098,0.21,-2.7e-06,0.43,-0.00018,0.00056,-0.00022,0,0,-4.9e+02,0.00086,0.00081,0.038,0.083,0.086,0.069,0.064,0.064,0.068,1.3e-05,2.3e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11590000,0.71,0.0013,-0.013,0.71,-0.002,-0.0015,-0.01,0,0,-4.9e+02,-0.0012,-0.0059,-9.4e-05,-0.0045,0.005,-0.098,0.21,-2.5e-06,0.43,-0.00016,0.00054,-0.00023,0,0,-4.9e+02,0.00075,0.00072,0.038,0.07,0.072,0.059,0.054,0.054,0.066,1.2e-05,2.1e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11690000,0.71,0.0011,-0.013,0.71,-0.0035,-0.0027,-0.016,0,0,-4.9e+02,-0.0011,-0.006,-9.8e-05,-0.0053,0.0052,-0.098,0.21,-2e-06,0.43,-0.0001,0.00053,-0.00026,0,0,-4.9e+02,0.00075,0.00071,0.038,0.082,0.084,0.057,0.06,0.06,0.066,1.1e-05,2e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11790000,0.71,0.0011,-0.013,0.71,-0.0075,-0.00084,-0.018,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.0071,0.0053,-0.098,0.21,-2e-06,0.43,-8.6e-05,0.0005,-0.00027,0,0,-4.9e+02,0.00065,0.00063,0.038,0.068,0.07,0.051,0.051,0.051,0.063,1e-05,1.8e-05,2.3e-06,0.028,0.03,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11890000,0.71,0.00095,-0.013,0.71,-0.0085,-0.0037,-0.02,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0078,0.0059,-0.098,0.21,-1.9e-06,0.43,-5.4e-05,0.00047,-0.00032,0,0,-4.9e+02,0.00065,0.00062,0.038,0.08,0.082,0.049,0.058,0.058,0.063,9.8e-06,1.8e-05,2.3e-06,0.028,0.029,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11990000,0.71,0.0013,-0.013,0.71,-0.012,0.0012,-0.026,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.0074,0.0064,-0.097,0.21,-2.9e-06,0.43,-0.00013,0.00048,-0.00032,0,0,-4.9e+02,0.00056,0.00055,0.037,0.066,0.068,0.045,0.049,0.049,0.062,9.2e-06,1.6e-05,2.3e-06,0.026,0.028,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -12090000,0.71,0.0014,-0.013,0.71,-0.013,0.0044,-0.032,0,0,-4.9e+02,-0.0012,-0.0059,-9.5e-05,-0.0063,0.0056,-0.097,0.21,-3e-06,0.43,-0.00016,0.00053,-0.00028,0,0,-4.9e+02,0.00056,0.00055,0.037,0.077,0.079,0.045,0.056,0.057,0.062,8.8e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12190000,0.71,0.0012,-0.013,0.71,-0.0068,0.004,-0.026,0,0,-4.9e+02,-0.0011,-0.0059,-9.6e-05,-0.0062,0.006,-0.099,0.21,-2.3e-06,0.43,-0.00011,0.0006,-0.00026,0,0,-4.9e+02,0.00048,0.00048,0.037,0.064,0.066,0.041,0.048,0.048,0.059,8.2e-06,1.4e-05,2.3e-06,0.023,0.026,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12290000,0.71,0.001,-0.013,0.71,-0.0081,0.0044,-0.026,0,0,-4.9e+02,-0.0011,-0.0059,-9.6e-05,-0.0067,0.0056,-0.099,0.21,-2e-06,0.43,-8.1e-05,0.0006,-0.00026,0,0,-4.9e+02,0.00048,0.00048,0.037,0.073,0.076,0.042,0.056,0.056,0.06,7.9e-06,1.4e-05,2.3e-06,0.023,0.025,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12390000,0.71,0.001,-0.013,0.71,-0.0053,0.0032,-0.023,0,0,-4.9e+02,-0.0011,-0.0059,-9.9e-05,-0.0053,0.0073,-0.1,0.21,-2.5e-06,0.43,-0.00012,0.00061,-0.00025,0,0,-4.9e+02,0.00042,0.00043,0.037,0.061,0.063,0.039,0.048,0.048,0.058,7.4e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12490000,0.71,0.00093,-0.013,0.71,-0.007,0.0028,-0.027,0,0,-4.9e+02,-0.0011,-0.0059,-0.0001,-0.0058,0.0085,-0.1,0.21,-2.7e-06,0.43,-0.00015,0.00054,-0.00025,0,0,-4.9e+02,0.00042,0.00042,0.037,0.069,0.072,0.04,0.055,0.055,0.058,7.1e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12590000,0.71,0.0011,-0.013,0.71,-0.014,0.0037,-0.033,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0051,0.0078,-0.1,0.21,-3e-06,0.43,-0.00019,0.00051,-0.00024,0,0,-4.9e+02,0.00037,0.00038,0.037,0.058,0.059,0.038,0.047,0.048,0.057,6.8e-06,1.2e-05,2.3e-06,0.019,0.022,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12690000,0.71,0.0014,-0.013,0.71,-0.017,0.0046,-0.037,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0034,0.0091,-0.1,0.21,-3.9e-06,0.43,-0.00026,0.0005,-0.00025,0,0,-4.9e+02,0.00037,0.00038,0.037,0.065,0.067,0.039,0.055,0.055,0.057,6.5e-06,1.1e-05,2.3e-06,0.019,0.022,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12790000,0.71,0.0012,-0.013,0.71,-0.02,0.0027,-0.041,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0049,0.008,-0.1,0.21,-3.3e-06,0.43,-0.0002,0.00048,-0.00027,0,0,-4.9e+02,0.00033,0.00034,0.037,0.054,0.056,0.037,0.047,0.047,0.056,6.2e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12890000,0.71,0.0011,-0.013,0.71,-0.019,0.0014,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0063,0.0076,-0.1,0.21,-2.8e-06,0.43,-0.00017,0.00047,-0.00026,0,0,-4.9e+02,0.00033,0.00034,0.037,0.061,0.063,0.038,0.054,0.055,0.057,6e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -12990000,0.71,0.001,-0.013,0.71,-0.009,0.0019,-0.039,0,0,-4.9e+02,-0.0011,-0.0059,-9.9e-05,-0.0039,0.008,-0.1,0.21,-2.7e-06,0.43,-0.00018,0.00058,-0.0002,0,0,-4.9e+02,0.0003,0.00031,0.037,0.051,0.053,0.037,0.047,0.047,0.055,5.7e-06,9.9e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13090000,0.71,0.00094,-0.013,0.71,-0.0097,-0.00016,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.005,0.0096,-0.1,0.21,-3e-06,0.43,-0.00019,0.00052,-0.00023,0,0,-4.9e+02,0.0003,0.00031,0.037,0.057,0.059,0.038,0.054,0.054,0.056,5.5e-06,9.6e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13190000,0.71,0.00094,-0.013,0.71,-0.0023,0.0008,-0.034,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0032,0.011,-0.11,0.21,-3.4e-06,0.43,-0.00023,0.00056,-0.00021,0,0,-4.9e+02,0.00027,0.00029,0.037,0.048,0.05,0.037,0.047,0.047,0.054,5.2e-06,9.1e-06,2.3e-06,0.015,0.019,0.0098,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13290000,0.71,0.00079,-0.013,0.71,-0.00082,0.0015,-0.029,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0047,0.0096,-0.11,0.21,-2.5e-06,0.43,-0.00017,0.00058,-0.0002,0,0,-4.9e+02,0.00027,0.00028,0.037,0.054,0.055,0.038,0.054,0.054,0.055,5.1e-06,8.9e-06,2.3e-06,0.015,0.019,0.0097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13390000,0.71,0.0007,-0.013,0.71,0.00021,0.0023,-0.024,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0042,0.0088,-0.11,0.21,-2.3e-06,0.43,-0.00016,0.00063,-0.00019,0,0,-4.9e+02,0.00025,0.00026,0.037,0.045,0.047,0.037,0.047,0.047,0.054,4.8e-06,8.5e-06,2.3e-06,0.014,0.018,0.0091,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13490000,0.71,0.0007,-0.013,0.71,0.00018,0.0024,-0.022,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0042,0.0081,-0.11,0.21,-1.9e-06,0.43,-0.00014,0.00063,-0.00017,0,0,-4.9e+02,0.00025,0.00026,0.037,0.05,0.052,0.038,0.054,0.054,0.055,4.7e-06,8.2e-06,2.3e-06,0.014,0.018,0.009,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13590000,0.71,0.00072,-0.013,0.71,-5.1e-05,0.0028,-0.024,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.004,0.0093,-0.11,0.21,-2.4e-06,0.43,-0.00017,0.00062,-0.00019,0,0,-4.9e+02,0.00023,0.00025,0.037,0.042,0.044,0.037,0.046,0.047,0.054,4.5e-06,7.9e-06,2.3e-06,0.013,0.017,0.0085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13690000,0.71,0.0007,-0.013,0.71,0.00087,0.0053,-0.029,0,0,-4.9e+02,-0.001,-0.0059,-9.9e-05,-0.0035,0.0081,-0.11,0.21,-2e-06,0.43,-0.00016,0.00064,-0.00016,0,0,-4.9e+02,0.00023,0.00024,0.037,0.047,0.049,0.038,0.053,0.054,0.055,4.3e-06,7.7e-06,2.3e-06,0.013,0.017,0.0083,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13790000,0.71,0.00074,-0.013,0.71,0.00057,0.0021,-0.03,0,0,-4.9e+02,-0.0011,-0.0059,-9.9e-05,-0.002,0.0088,-0.11,0.21,-2.4e-06,0.43,-0.00019,0.00065,-0.00015,0,0,-4.9e+02,0.00022,0.00023,0.037,0.04,0.041,0.036,0.046,0.046,0.053,4.2e-06,7.3e-06,2.3e-06,0.013,0.016,0.0078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13890000,0.71,0.00059,-0.013,0.71,0.0024,0.0021,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.0034,0.0077,-0.11,0.21,-1.7e-06,0.43,-0.00015,0.00065,-0.00015,0,0,-4.9e+02,0.00022,0.00023,0.037,0.044,0.046,0.037,0.053,0.053,0.055,4e-06,7.1e-06,2.3e-06,0.012,0.016,0.0076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13990000,0.71,0.00065,-0.013,0.71,0.0018,0.0005,-0.034,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.0021,0.0083,-0.11,0.21,-2e-06,0.43,-0.00018,0.00065,-0.00013,0,0,-4.9e+02,0.00021,0.00022,0.037,0.037,0.039,0.036,0.046,0.046,0.054,3.9e-06,6.8e-06,2.3e-06,0.012,0.015,0.0071,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -14090000,0.71,0.00072,-0.013,0.71,0.0014,0.0019,-0.035,0,0,-4.9e+02,-0.0011,-0.0059,-9.4e-05,-0.00064,0.0072,-0.11,0.21,-1.8e-06,0.43,-0.00018,0.00068,-9.7e-05,0,0,-4.9e+02,0.00021,0.00022,0.037,0.041,0.043,0.036,0.053,0.053,0.054,3.8e-06,6.7e-06,2.3e-06,0.012,0.015,0.007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14190000,0.71,0.00068,-0.013,0.71,0.0044,0.0018,-0.037,0,0,-4.9e+02,-0.0011,-0.0059,-9.3e-05,-0.00018,0.007,-0.11,0.21,-1.6e-06,0.43,-0.00017,0.0007,-8e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.035,0.037,0.035,0.046,0.046,0.054,3.6e-06,6.4e-06,2.3e-06,0.011,0.015,0.0065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14290000,0.71,0.00075,-0.013,0.71,0.0046,0.0032,-0.035,0,0,-4.9e+02,-0.0011,-0.0058,-9.2e-05,0.00066,0.0069,-0.11,0.21,-1.7e-06,0.43,-0.00019,0.00071,-6.2e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.039,0.04,0.036,0.052,0.052,0.055,3.5e-06,6.2e-06,2.3e-06,0.011,0.014,0.0063,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14390000,0.71,0.00063,-0.014,0.71,0.007,0.0049,-0.037,0,0,-4.9e+02,-0.0011,-0.0058,-8.9e-05,0.00034,0.0054,-0.11,0.21,-9e-07,0.43,-0.00015,0.00074,-4.6e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14490000,0.71,0.00052,-0.013,0.71,0.008,0.0064,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,-0.00096,0.0049,-0.11,0.21,-4.1e-07,0.43,-0.00013,0.0007,-4.6e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14590000,0.71,0.00042,-0.013,0.71,0.006,0.0049,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,-0.0019,0.0047,-0.11,0.21,-3.2e-07,0.43,-0.00012,0.00067,-5.9e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14690000,0.71,0.00037,-0.013,0.71,0.0078,0.003,-0.038,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.0018,0.0038,-0.11,0.21,6.3e-08,0.43,-0.0001,0.00068,-4.6e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14790000,0.71,0.00035,-0.013,0.71,0.0055,0.0016,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.002,0.0036,-0.12,0.21,7.8e-09,0.43,-0.0001,0.00066,-4.7e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0098,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0033,-0.037,0,0,-4.9e+02,-0.001,-0.0058,-8.6e-05,-0.0022,0.0031,-0.12,0.21,2.7e-07,0.43,-9e-05,0.00067,-4.4e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0097,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -14990000,0.71,0.00027,-0.013,0.71,0.0067,0.0023,-0.032,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.0028,0.0036,-0.12,0.21,7.5e-08,0.43,-9.9e-05,0.00064,-5.6e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.013,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15090000,0.71,0.0002,-0.013,0.71,0.0075,0.0024,-0.035,0,0,-4.9e+02,-0.001,-0.0058,-8.8e-05,-0.0029,0.004,-0.12,0.21,-2.8e-08,0.43,-0.00011,0.00063,-5.7e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.031,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0093,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15190000,0.71,0.00016,-0.013,0.7,0.0072,0.003,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-9e-05,-0.0035,0.0043,-0.12,0.21,-2.3e-08,0.43,-0.00012,0.00059,-6.4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.0091,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15290000,0.71,0.00019,-0.013,0.7,0.0077,0.0041,-0.03,0,0,-4.9e+02,-0.001,-0.0058,-8.8e-05,-0.0026,0.0041,-0.12,0.21,2.1e-08,0.43,-0.00014,0.00059,-4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15390000,0.71,0.0002,-0.013,0.7,0.007,0.0053,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.3e-05,-0.0018,0.0027,-0.12,0.21,3.7e-07,0.43,-0.00012,0.00062,-1.7e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15490000,0.71,0.00021,-0.013,0.7,0.0085,0.0045,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.0022,0.004,-0.12,0.21,-8e-08,0.43,-0.00014,0.00059,-3.5e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15590000,0.71,0.00018,-0.013,0.71,0.0071,0.0035,-0.027,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.0028,0.0047,-0.12,0.21,-4.3e-07,0.43,-0.00014,0.00058,-6.1e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0036,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.0023,0.0051,-0.12,0.21,-7.3e-07,0.43,-0.00015,0.00058,-6.4e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15790000,0.71,0.00019,-0.013,0.7,0.008,0.002,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.0026,0.0053,-0.12,0.21,-8.4e-07,0.43,-0.00016,0.00057,-6.8e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0082,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15890000,0.71,0.0002,-0.013,0.71,0.0087,0.0021,-0.028,0,0,-4.9e+02,-0.0011,-0.0059,-8.8e-05,-0.0018,0.0056,-0.12,0.21,-1e-06,0.43,-0.00018,0.00057,-6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.1e-06,3.8e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15990000,0.71,0.00017,-0.013,0.71,0.0085,0.0022,-0.024,0,0,-4.9e+02,-0.0011,-0.0059,-8.4e-05,-0.0012,0.0049,-0.12,0.21,-9.4e-07,0.43,-0.00018,0.00058,-4.2e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -16090000,0.71,0.00021,-0.013,0.71,0.01,0.0038,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-8e-05,-0.00044,0.0036,-0.12,0.21,-6.7e-07,0.43,-0.00016,0.00063,-2.6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.049,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16190000,0.71,0.00026,-0.013,0.71,0.01,0.004,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.9e-05,0.0003,0.0038,-0.12,0.21,-9.9e-07,0.43,-0.00017,0.00063,-2.1e-05,0,0,-4.9e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16290000,0.71,0.00028,-0.014,0.71,0.012,0.0051,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.00062,0.0025,-0.12,0.21,-5.3e-07,0.43,-0.00015,0.00064,-5.9e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.048,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16390000,0.71,0.00033,-0.013,0.71,0.01,0.0036,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.6e-05,0.0017,0.0037,-0.12,0.21,-1.3e-06,0.43,-0.00018,0.00064,-2e-06,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.023,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16490000,0.71,0.00042,-0.013,0.71,0.0087,0.0049,-0.022,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.0026,0.0039,-0.12,0.21,-1.5e-06,0.43,-0.00019,0.00064,1.2e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16590000,0.71,0.00052,-0.013,0.71,0.0066,0.0058,-0.023,0,0,-4.9e+02,-0.0012,-0.0058,-7.6e-05,0.0027,0.0036,-0.12,0.21,-1.6e-06,0.43,-0.00017,0.00063,7.8e-06,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0096,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16690000,0.71,0.00049,-0.013,0.71,0.0075,0.0062,-0.019,0,0,-4.9e+02,-0.0012,-0.0058,-7.9e-05,0.0022,0.0042,-0.12,0.21,-1.7e-06,0.43,-0.00018,0.00062,-3.3e-06,0,0,-4.9e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16790000,0.71,0.00048,-0.013,0.71,0.0056,0.0068,-0.018,0,0,-4.9e+02,-0.0012,-0.0058,-8e-05,0.002,0.0039,-0.12,0.21,-1.7e-06,0.43,-0.00016,0.00063,-1.6e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0093,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16890000,0.71,0.00054,-0.013,0.71,0.0053,0.0078,-0.016,0,0,-4.9e+02,-0.0012,-0.0058,-8.2e-05,0.0023,0.0045,-0.13,0.21,-2e-06,0.43,-0.00017,0.00062,-1.2e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -16990000,0.71,0.00051,-0.013,0.71,0.0054,0.0055,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.2e-05,0.0022,0.0056,-0.13,0.21,-2.4e-06,0.43,-0.00019,0.00061,-2.2e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17090000,0.71,0.00055,-0.013,0.71,0.0057,0.007,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.0032,0.0059,-0.13,0.21,-2.7e-06,0.43,-0.0002,0.00061,-1e-05,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17190000,0.71,0.00061,-0.013,0.71,0.0058,0.0079,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-7.7e-05,0.0039,0.0058,-0.13,0.21,-3e-06,0.43,-0.0002,0.00062,-7.8e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0068,0.0088,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17290000,0.71,0.00063,-0.013,0.71,0.0076,0.0087,-0.011,0,0,-4.9e+02,-0.0012,-0.0059,-7.9e-05,0.0043,0.0068,-0.13,0.21,-3.4e-06,0.43,-0.00022,0.00061,-5.2e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17390000,0.71,0.00068,-0.013,0.71,0.0074,0.0091,-0.0095,0,0,-4.9e+02,-0.0012,-0.0059,-7.3e-05,0.0051,0.0065,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.5e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17490000,0.71,0.00063,-0.013,0.71,0.009,0.0093,-0.0078,0,0,-4.9e+02,-0.0012,-0.0059,-7.4e-05,0.0045,0.0063,-0.13,0.21,-3.3e-06,0.43,-0.00023,0.00061,7.8e-06,0,0,-4.9e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17590000,0.71,0.0006,-0.013,0.71,0.0098,0.0084,-0.0024,0,0,-4.9e+02,-0.0012,-0.0059,-7e-05,0.0048,0.0065,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00061,5.1e-06,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0065,0.0083,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17690000,0.71,0.00057,-0.013,0.71,0.011,0.01,-0.003,0,0,-4.9e+02,-0.0012,-0.0059,-6.9e-05,0.0049,0.0063,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.045,0.046,0.048,1.3e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17790000,0.71,0.00056,-0.013,0.71,0.012,0.011,-0.0042,0,0,-4.9e+02,-0.0012,-0.0058,-6e-05,0.0059,0.0052,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00064,2.3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.0081,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17890000,0.71,0.00055,-0.013,0.71,0.015,0.012,-0.0041,0,0,-4.9e+02,-0.0012,-0.0058,-5.8e-05,0.0057,0.0046,-0.13,0.21,-3.2e-06,0.43,-0.00023,0.00064,2.8e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17990000,0.71,0.00049,-0.013,0.71,0.016,0.009,-0.0027,0,0,-4.9e+02,-0.0012,-0.0058,-5.7e-05,0.0054,0.0049,-0.13,0.21,-3.3e-06,0.43,-0.00024,0.00064,2.5e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0079,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -18090000,0.71,0.00047,-0.013,0.71,0.017,0.0082,-0.00045,0,0,-4.9e+02,-0.0012,-0.0059,-6.2e-05,0.0049,0.0058,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00061,1.9e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18190000,0.71,0.00043,-0.013,0.71,0.017,0.0092,0.001,0,0,-4.9e+02,-0.0012,-0.0059,-5.8e-05,0.0051,0.0055,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00062,2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.041,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0077,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18290000,0.71,0.00035,-0.013,0.71,0.018,0.0088,0.0022,0,0,-4.9e+02,-0.0012,-0.0059,-6e-05,0.0047,0.0058,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00061,1.4e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18390000,0.71,0.00031,-0.013,0.71,0.02,0.011,0.0035,0,0,-4.9e+02,-0.0012,-0.0059,-5.4e-05,0.0044,0.005,-0.13,0.21,-3.3e-06,0.43,-0.00024,0.00062,1.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.006,0.0075,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18490000,0.71,0.00038,-0.013,0.71,0.021,0.012,0.0031,0,0,-4.9e+02,-0.0012,-0.0059,-5.3e-05,0.005,0.0051,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00063,2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18590000,0.71,0.00039,-0.013,0.71,0.02,0.013,0.0014,0,0,-4.9e+02,-0.0012,-0.0058,-4.5e-05,0.0058,0.0046,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00065,2.5e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18690000,0.71,0.00031,-0.013,0.71,0.022,0.012,-0.00039,0,0,-4.9e+02,-0.0012,-0.0059,-4.7e-05,0.0051,0.0046,-0.13,0.21,-3.5e-06,0.43,-0.00024,0.00064,1.9e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18790000,0.71,0.00032,-0.013,0.71,0.021,0.012,-0.00061,0,0,-4.9e+02,-0.0012,-0.0059,-4.6e-05,0.0053,0.0052,-0.13,0.21,-3.8e-06,0.43,-0.00025,0.00063,1.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00067,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18890000,0.71,0.00041,-0.013,0.71,0.021,0.014,1.9e-05,0,0,-4.9e+02,-0.0012,-0.0058,-4e-05,0.0062,0.0046,-0.13,0.21,-3.8e-06,0.43,-0.00026,0.00065,2.9e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -18990000,0.71,0.00046,-0.013,0.71,0.021,0.015,-0.0012,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.0069,0.0048,-0.13,0.21,-4.1e-06,0.43,-0.00027,0.00066,3e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.015,0.017,0.012,0.039,0.04,0.044,9.7e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.00062,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19090000,0.71,0.00051,-0.013,0.71,0.021,0.016,0.0018,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.0075,0.0051,-0.13,0.21,-4.4e-06,0.43,-0.00028,0.00066,3.4e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.016,0.019,0.012,0.042,0.044,0.044,9.6e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.0006,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19190000,0.71,0.00055,-0.013,0.71,0.02,0.016,0.0019,0,0,-4.9e+02,-0.0013,-0.0059,-2.7e-05,0.008,0.0053,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00067,3.8e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.012,0.038,0.04,0.043,9.3e-07,1.5e-06,2.1e-06,0.0055,0.0068,0.00058,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19290000,0.71,0.00058,-0.013,0.71,0.02,0.015,0.0046,0,0,-4.9e+02,-0.0013,-0.0059,-3.1e-05,0.0078,0.0057,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00066,4e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.019,0.012,0.042,0.044,0.043,9.2e-07,1.5e-06,2.1e-06,0.0055,0.0067,0.00056,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19390000,0.71,0.00054,-0.013,0.71,0.019,0.014,0.0084,0,0,-4.9e+02,-0.0013,-0.0059,-2.4e-05,0.0078,0.0055,-0.13,0.21,-4.7e-06,0.43,-0.00029,0.00065,3.6e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.011,0.038,0.04,0.043,8.9e-07,1.5e-06,2.1e-06,0.0055,0.0066,0.00054,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19490000,0.71,0.00055,-0.013,0.71,0.019,0.015,0.0049,0,0,-4.9e+02,-0.0013,-0.0058,-1.9e-05,0.0078,0.0048,-0.13,0.21,-4.5e-06,0.43,-0.00029,0.00066,3.9e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.018,0.011,0.042,0.044,0.043,8.8e-07,1.4e-06,2.1e-06,0.0054,0.0066,0.00052,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19590000,0.71,0.00063,-0.013,0.71,0.017,0.015,0.0043,0,0,-4.9e+02,-0.0013,-0.0058,-7.4e-06,0.0086,0.0046,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00068,4.7e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.014,0.017,0.011,0.038,0.039,0.042,8.5e-07,1.4e-06,2.1e-06,0.0054,0.0065,0.0005,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19690000,0.71,0.00067,-0.013,0.71,0.017,0.013,0.0059,0,0,-4.9e+02,-0.0013,-0.0058,-1.1e-05,0.0089,0.0052,-0.13,0.21,-4.9e-06,0.43,-0.0003,0.00068,3.9e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.015,0.018,0.011,0.042,0.043,0.042,8.4e-07,1.4e-06,2.1e-06,0.0053,0.0065,0.00049,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19790000,0.71,0.00074,-0.013,0.71,0.014,0.011,0.0064,0,0,-4.9e+02,-0.0013,-0.0059,-6.3e-06,0.0094,0.0055,-0.13,0.21,-5.2e-06,0.43,-0.00031,0.00068,3.8e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.042,8.2e-07,1.3e-06,2.1e-06,0.0053,0.0064,0.00047,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19890000,0.71,0.00066,-0.013,0.71,0.015,0.013,0.0076,0,0,-4.9e+02,-0.0013,-0.0058,1.8e-06,0.0092,0.0045,-0.13,0.21,-4.9e-06,0.43,-0.0003,0.00069,4.5e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.015,0.018,0.01,0.041,0.043,0.042,8.1e-07,1.3e-06,2.1e-06,0.0053,0.0063,0.00046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19990000,0.71,0.00063,-0.013,0.7,0.013,0.013,0.01,0,0,-4.9e+02,-0.0013,-0.0058,1.7e-05,0.0095,0.0037,-0.13,0.21,-4.8e-06,0.43,-0.0003,0.0007,5.1e-05,0,0,-4.9e+02,0.0001,9.4e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.041,7.9e-07,1.3e-06,2.1e-06,0.0052,0.0062,0.00044,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -20090000,0.71,0.00066,-0.013,0.7,0.013,0.013,0.011,0,0,-4.9e+02,-0.0013,-0.0058,2.7e-05,0.01,0.0029,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00072,6.2e-05,0,0,-4.9e+02,0.00011,9.5e-05,0.036,0.014,0.018,0.01,0.041,0.043,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20190000,0.71,0.00069,-0.013,0.7,0.012,0.012,0.013,0,0,-4.9e+02,-0.0013,-0.0058,3.6e-05,0.0099,0.0026,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00072,6.1e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.013,0.016,0.0098,0.038,0.039,0.041,7.6e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00042,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20290000,0.71,0.0007,-0.013,0.7,0.01,0.012,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4e-05,0.01,0.0025,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00073,6.3e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.014,0.018,0.0097,0.041,0.043,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.0004,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20390000,0.71,0.00065,-0.013,0.7,0.0085,0.0097,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.4e-05,0.01,0.0026,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00073,5.3e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.0051,0.006,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20490000,0.71,0.00071,-0.013,0.7,0.0086,0.0096,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.01,0.0029,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00072,5.1e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.006,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20590000,0.71,0.00074,-0.013,0.7,0.0077,0.0076,0.0099,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.01,0.0035,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00072,5.1e-05,0,0,-4.9e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0059,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20690000,0.71,0.00077,-0.013,0.7,0.0084,0.0076,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.01,0.0032,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00072,5.2e-05,0,0,-4.9e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.005,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20790000,0.71,0.0008,-0.013,0.7,0.0062,0.0071,0.012,0,0,-4.9e+02,-0.0013,-0.0058,5e-05,0.011,0.0034,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00073,4.7e-05,0,0,-4.9e+02,9.8e-05,8.8e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0058,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20890000,0.71,0.00081,-0.013,0.7,0.0061,0.0068,0.011,0,0,-4.9e+02,-0.0013,-0.0058,5.8e-05,0.011,0.003,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00074,5.2e-05,0,0,-4.9e+02,9.9e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -20990000,0.71,0.00083,-0.013,0.7,0.0044,0.0045,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.2e-05,0.011,0.0032,-0.13,0.21,-4.9e-06,0.43,-0.00033,0.00074,4.9e-05,0,0,-4.9e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0049,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21090000,0.71,0.00081,-0.013,0.7,0.0053,0.0037,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.011,0.0027,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00075,4.6e-05,0,0,-4.9e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21190000,0.71,0.00081,-0.013,0.7,0.0056,0.0028,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.011,0.0029,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00075,4.3e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.3e-07,9.5e-07,2e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21290000,0.71,0.0009,-0.013,0.7,0.0049,0.0029,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.8e-05,0.011,0.0025,-0.13,0.21,-4.7e-06,0.43,-0.00033,0.00077,4.7e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.5e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21390000,0.71,0.00088,-0.013,0.7,0.0039,0.00086,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.011,0.0026,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00076,4.8e-05,0,0,-4.9e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0055,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21490000,0.71,0.00088,-0.013,0.7,0.0044,0.0013,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.011,0.0023,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00076,5.3e-05,0,0,-4.9e+02,9.4e-05,8.4e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21590000,0.71,0.00087,-0.013,0.7,0.0033,0.0018,0.012,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.011,0.0023,-0.13,0.21,-5e-06,0.43,-0.00032,0.00076,5e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0054,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21690000,0.71,0.00084,-0.013,0.7,0.0049,0.0021,0.014,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.011,0.0019,-0.13,0.21,-4.9e-06,0.43,-0.0003,0.00076,5.1e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21790000,0.71,0.00083,-0.013,0.7,0.003,0.0042,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.011,0.0021,-0.13,0.21,-5.5e-06,0.43,-0.00032,0.00076,5.3e-05,0,0,-4.9e+02,9e-05,8.1e-05,0.036,0.012,0.015,0.0079,0.037,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0048,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.011,0.002,-0.13,0.21,-5.4e-06,0.43,-0.00032,0.00076,5.1e-05,0,0,-4.9e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21990000,0.71,0.00085,-0.013,0.7,0.0025,0.0054,0.014,0,0,-4.9e+02,-0.0013,-0.0058,6.9e-05,0.012,0.0019,-0.13,0.21,-5.8e-06,0.43,-0.00033,0.00076,5.2e-05,0,0,-4.9e+02,8.9e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.5e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22090000,0.71,0.00087,-0.013,0.7,0.0024,0.007,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.9e-05,0.012,0.0019,-0.13,0.21,-5.8e-06,0.43,-0.00033,0.00076,5.2e-05,0,0,-4.9e+02,8.9e-05,8e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22190000,0.71,0.00084,-0.013,0.7,0.0019,0.007,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.5e-05,0.012,0.002,-0.13,0.21,-5.6e-06,0.43,-0.00034,0.00077,4.2e-05,0,0,-4.9e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22290000,0.71,0.00087,-0.013,0.7,0.0013,0.0067,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.012,0.002,-0.13,0.21,-5.6e-06,0.43,-0.00034,0.00076,4.3e-05,0,0,-4.9e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22390000,0.71,0.00089,-0.013,0.7,-0.0011,0.0064,0.015,0,0,-4.9e+02,-0.0013,-0.0058,7.9e-05,0.012,0.0023,-0.13,0.21,-5.6e-06,0.43,-0.00034,0.00077,4.4e-05,0,0,-4.9e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22490000,0.71,0.00093,-0.013,0.7,-0.0022,0.0073,0.016,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.012,0.0024,-0.13,0.21,-5.5e-06,0.43,-0.00036,0.00078,4.1e-05,0,0,-4.9e+02,8.7e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22590000,0.71,0.00095,-0.013,0.7,-0.0038,0.0066,0.015,0,0,-4.9e+02,-0.0014,-0.0058,8.4e-05,0.013,0.0026,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00079,3.8e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22690000,0.71,0.001,-0.013,0.7,-0.0052,0.008,0.016,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.013,0.0025,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.0008,3.8e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22790000,0.71,0.001,-0.013,0.7,-0.0072,0.0069,0.017,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.013,0.0033,-0.13,0.21,-5.6e-06,0.43,-0.00038,0.00078,4.3e-05,0,0,-4.9e+02,8.3e-05,7.5e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.8e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22890000,0.71,0.00099,-0.013,0.7,-0.0076,0.0078,0.019,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.013,0.0031,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00077,3.9e-05,0,0,-4.9e+02,8.4e-05,7.5e-05,0.036,0.013,0.016,0.0071,0.039,0.042,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22990000,0.71,0.00097,-0.013,0.7,-0.0077,0.0069,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.8e-05,0.013,0.003,-0.13,0.21,-5.2e-06,0.43,-0.00037,0.00078,3.6e-05,0,0,-4.9e+02,8.2e-05,7.4e-05,0.036,0.012,0.015,0.007,0.036,0.038,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23090000,0.71,0.00092,-0.013,0.7,-0.0081,0.0068,0.02,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.012,0.0033,-0.13,0.21,-5.4e-06,0.43,-0.00037,0.00076,3.4e-05,0,0,-4.9e+02,8.3e-05,7.4e-05,0.036,0.013,0.016,0.007,0.039,0.042,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23190000,0.71,0.00097,-0.013,0.7,-0.0094,0.0048,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.2e-05,0.013,0.0035,-0.13,0.21,-5.4e-06,0.43,-0.00036,0.00076,2.6e-05,0,0,-4.9e+02,8.1e-05,7.3e-05,0.036,0.012,0.014,0.0069,0.036,0.038,0.035,4.6e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23290000,0.71,0.00089,-0.013,0.7,-0.0093,0.0042,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.4e-05,0.012,0.0033,-0.13,0.21,-5.3e-06,0.43,-0.00036,0.00076,2.5e-05,0,0,-4.9e+02,8.2e-05,7.3e-05,0.036,0.013,0.016,0.0069,0.039,0.042,0.036,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23390000,0.71,0.00095,-0.013,0.7,-0.0095,0.003,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.7e-05,0.012,0.0033,-0.13,0.21,-5.4e-06,0.43,-0.00034,0.00075,2.6e-05,0,0,-4.9e+02,8e-05,7.2e-05,0.036,0.012,0.014,0.0068,0.036,0.038,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23490000,0.71,0.0033,-0.01,0.7,-0.016,0.0033,-0.013,0,0,-4.9e+02,-0.0014,-0.0058,9.3e-05,0.012,0.0031,-0.13,0.21,-5.4e-06,0.43,-0.00033,0.00078,5.1e-05,0,0,-4.9e+02,8.1e-05,7.2e-05,0.036,0.013,0.015,0.0068,0.039,0.042,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 -23590000,0.71,0.0086,-0.0026,0.7,-0.027,0.0032,-0.045,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.012,0.0031,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00084,0.00012,0,0,-4.9e+02,7.9e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23690000,0.71,0.0082,0.0032,0.71,-0.058,-0.0046,-0.095,0,0,-4.9e+02,-0.0014,-0.0058,9e-05,0.012,0.0031,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00078,9e-05,0,0,-4.9e+02,8e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0047,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23790000,0.71,0.0052,-0.00012,0.71,-0.083,-0.016,-0.15,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.011,0.0026,-0.13,0.21,-4.5e-06,0.43,-0.00038,0.00078,0.00044,0,0,-4.9e+02,7.8e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23890000,0.71,0.0026,-0.0062,0.71,-0.1,-0.025,-0.2,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.012,0.0028,-0.13,0.21,-4.4e-06,0.43,-0.00041,0.00083,0.00035,0,0,-4.9e+02,7.9e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23990000,0.71,0.0012,-0.011,0.71,-0.1,-0.029,-0.26,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.012,0.0028,-0.13,0.21,-4e-06,0.43,-0.00038,0.00083,0.00032,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24090000,0.71,0.0025,-0.0095,0.71,-0.1,-0.028,-0.3,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.011,0.0025,-0.13,0.21,-3.6e-06,0.43,-0.00041,0.0008,0.00036,0,0,-4.9e+02,7.8e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24190000,0.71,0.0036,-0.0072,0.71,-0.11,-0.03,-0.35,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.011,0.0024,-0.13,0.21,-2.8e-06,0.43,-0.00041,0.00083,0.00036,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24290000,0.71,0.0041,-0.0064,0.71,-0.12,-0.033,-0.41,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.011,0.0023,-0.13,0.21,-2.5e-06,0.43,-0.00045,0.00088,0.00042,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24390000,0.71,0.0042,-0.0065,0.71,-0.13,-0.041,-0.46,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.011,0.003,-0.13,0.21,2.9e-07,0.43,-0.00033,0.00092,0.00042,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24490000,0.71,0.005,-0.0024,0.71,-0.14,-0.046,-0.51,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.011,0.003,-0.13,0.21,2.9e-07,0.43,-0.00033,0.00093,0.00041,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24590000,0.71,0.0055,0.0013,0.71,-0.16,-0.057,-0.56,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.003,-0.13,0.21,1.4e-06,0.43,2.7e-05,0.00058,0.00036,0,0,-4.9e+02,7.4e-05,6.7e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24690000,0.71,0.0056,0.0022,0.71,-0.18,-0.07,-0.64,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.0028,-0.13,0.21,2.4e-06,0.43,-1.8e-05,0.00062,0.00054,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24790000,0.71,0.0053,0.00097,0.71,-0.2,-0.083,-0.73,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.0029,-0.13,0.21,1.8e-06,0.43,1.5e-05,0.00059,0.0003,0,0,-4.9e+02,7.3e-05,6.6e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24890000,0.71,0.0071,0.0027,0.71,-0.22,-0.094,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.01,0.0029,-0.13,0.21,2.6e-06,0.43,-9.1e-05,0.00075,0.00032,0,0,-4.9e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24990000,0.71,0.0089,0.0044,0.71,-0.24,-0.1,-0.81,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.0099,0.0026,-0.13,0.21,2.1e-06,0.43,-0.00017,0.00085,-2.2e-05,0,0,-4.9e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25090000,0.71,0.0092,0.0038,0.71,-0.27,-0.11,-0.86,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.0098,0.0027,-0.13,0.21,1.7e-06,0.43,-0.00019,0.00086,-5.8e-05,0,0,-4.9e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25190000,0.71,0.0087,0.0024,0.71,-0.3,-0.13,-0.91,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.0092,0.003,-0.13,0.21,7.1e-06,0.43,7e-05,0.00083,7.7e-05,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25290000,0.71,0.011,0.0092,0.71,-0.33,-0.14,-0.96,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.0092,0.003,-0.13,0.21,6.9e-06,0.43,9.5e-05,0.00077,8.4e-05,0,0,-4.9e+02,7.2e-05,6.5e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.0085,0.0027,-0.13,0.21,1.1e-05,0.43,0.0005,0.00046,0.00012,0,0,-4.9e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,0.00014,0.0084,0.0026,-0.13,0.21,9.7e-06,0.43,0.00067,0.00013,0.00031,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0077,0.003,-0.13,0.21,1.6e-05,0.43,0.00097,0.00014,0.00032,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25690000,0.71,0.015,0.022,0.71,-0.49,-0.23,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0077,0.003,-0.13,0.21,1.6e-05,0.43,0.00096,0.00016,0.0004,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 -25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.007,0.0023,-0.13,0.21,2e-05,0.43,0.0013,-8.6e-05,-5.5e-05,0,0,-4.9e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25890000,0.71,0.018,0.029,0.71,-0.62,-0.29,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00017,0.0072,0.0022,-0.13,0.21,2.2e-05,0.43,0.0014,-1.8e-05,-0.00012,0,0,-4.9e+02,7.1e-05,6.3e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25990000,0.7,0.017,0.025,0.71,-0.67,-0.32,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00019,0.0063,0.0026,-0.13,0.21,2.9e-05,0.43,0.0023,-0.00058,-0.00058,0,0,-4.9e+02,7e-05,6.2e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 -26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0064,0.0027,-0.13,0.21,2.5e-05,0.43,0.0024,-0.0005,-0.0012,0,0,-4.9e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 -26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0053,0.0016,-0.13,0.21,3.9e-05,0.43,0.0023,0.0004,-0.0014,0,0,-4.9e+02,7.1e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 -26290000,0.7,0.025,0.047,0.71,-0.89,-0.43,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0053,0.0016,-0.13,0.21,3.8e-05,0.43,0.0023,0.00027,-0.0014,0,0,-4.9e+02,7.1e-05,6.2e-05,0.03,0.015,0.028,0.0059,0.039,0.043,0.033,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.00099,3.9e-05,0.00099,0.0013,0.001,0.00099,1,1,0.01 -26390000,0.7,0.024,0.044,0.71,-0.96,-0.49,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00021,0.0044,0.0024,-0.13,0.21,4.6e-05,0.44,0.0036,-0.00018,-0.0024,0,0,-4.9e+02,7.1e-05,6.1e-05,0.028,0.014,0.027,0.0058,0.036,0.039,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00096,3.9e-05,0.00095,0.0012,0.00096,0.00095,1,1,0.01 -26490000,0.7,0.031,0.06,0.71,-1.1,-0.53,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00021,0.0044,0.0024,-0.13,0.21,3.9e-05,0.44,0.004,-0.00099,-0.0027,0,0,-4.9e+02,7.2e-05,6.1e-05,0.028,0.016,0.031,0.0058,0.039,0.044,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00092,3.9e-05,0.00092,0.0012,0.00092,0.00091,1,1,0.01 -26590000,0.7,0.037,0.076,0.71,-1.2,-0.59,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.0028,0.0014,-0.13,0.21,3.9e-05,0.44,0.0042,-0.00064,-0.0048,0,0,-4.9e+02,7.2e-05,6e-05,0.025,0.015,0.031,0.0058,0.036,0.04,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0041,0.00013,0.00087,3.9e-05,0.00086,0.001,0.00087,0.00086,1,1,0.01 -26690000,0.7,0.039,0.079,0.71,-1.3,-0.65,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.0028,0.0013,-0.13,0.21,4.5e-05,0.44,0.004,-0.00013,-0.004,0,0,-4.9e+02,7.2e-05,6.1e-05,0.025,0.017,0.037,0.0058,0.04,0.045,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00081,3.9e-05,0.0008,0.001,0.00081,0.00079,1,1,0.01 -26790000,0.7,0.036,0.074,0.71,-1.4,-0.74,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00022,0.0012,0.0022,-0.13,0.21,8.4e-05,0.44,0.0055,0.00063,-0.0038,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.016,0.036,0.0057,0.036,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00076,3.9e-05,0.00075,0.00092,0.00076,0.00074,1,1,0.01 -26890000,0.7,0.045,0.096,0.71,-1.6,-0.8,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00023,0.0013,0.0021,-0.13,0.21,9e-05,0.44,0.0054,0.0013,-0.0041,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.018,0.043,0.0058,0.04,0.046,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00072,3.9e-05,0.0007,0.00092,0.00072,0.0007,1,1,0.01 -26990000,0.7,0.051,0.12,0.71,-1.7,-0.89,-1.3,0,0,-4.9e+02,-0.00099,-0.0059,0.00022,-0.00069,0.00024,-0.13,0.21,0.00013,0.44,0.0061,0.0034,-0.0056,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.017,0.042,0.0057,0.037,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00065,3.9e-05,0.00063,0.00079,0.00065,0.00062,1,1,0.01 -27090000,0.7,0.052,0.12,0.71,-1.9,-0.98,-1.3,0,0,-4.9e+02,-0.00098,-0.0059,0.00022,-0.00073,0.00025,-0.13,0.21,0.00013,0.44,0.0061,0.0035,-0.0052,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.02,0.052,0.0057,0.04,0.048,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00059,3.9e-05,0.00056,0.00078,0.0006,0.00056,1,1,0.01 -27190000,0.7,0.05,0.11,0.7,-2.1,-1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00022,-0.0018,6.1e-05,-0.13,0.21,4.8e-05,0.44,0.0021,0.0027,-0.0049,0,0,-4.9e+02,7.5e-05,6e-05,0.016,0.02,0.051,0.0057,0.043,0.05,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00055,3.9e-05,0.00051,0.00066,0.00055,0.00051,1,1,0.01 -27290000,0.71,0.044,0.095,0.7,-2.3,-1.1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00023,-0.0018,4.3e-05,-0.13,0.21,5.6e-05,0.44,0.002,0.0034,-0.0049,0,0,-4.9e+02,7.6e-05,6.1e-05,0.016,0.022,0.059,0.0057,0.047,0.057,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00052,3.9e-05,0.00048,0.00066,0.00052,0.00048,1,1,0.01 -27390000,0.71,0.038,0.079,0.7,-2.4,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0058,0.00022,-0.003,-0.0017,-0.13,0.21,1.3e-05,0.44,-0.00049,0.0032,-0.0064,0,0,-4.9e+02,7.6e-05,6e-05,0.013,0.021,0.052,0.0057,0.049,0.059,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00049,3.9e-05,0.00046,0.00055,0.00049,0.00046,1,1,0.01 -27490000,0.71,0.032,0.064,0.7,-2.5,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0058,0.00022,-0.003,-0.0016,-0.13,0.21,1.9e-05,0.44,-0.00057,0.0032,-0.0067,0,0,-4.9e+02,7.7e-05,6.1e-05,0.013,0.023,0.056,0.0057,0.054,0.067,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00048,3.9e-05,0.00045,0.00055,0.00048,0.00044,1,1,0.01 -27590000,0.72,0.028,0.051,0.69,-2.6,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00023,-0.0037,-0.001,-0.12,0.21,-3.7e-05,0.44,-0.0032,0.0027,-0.0066,0,0,-4.9e+02,7.7e-05,6.1e-05,0.011,0.021,0.047,0.0057,0.056,0.068,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00044,1,1,0.01 -27690000,0.72,0.027,0.05,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00023,-0.0036,-0.00089,-0.12,0.21,-3.3e-05,0.44,-0.0033,0.0026,-0.0068,0,0,-4.9e+02,7.8e-05,6.1e-05,0.011,0.022,0.049,0.0057,0.062,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00043,1,1,0.01 -27790000,0.72,0.027,0.051,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00022,-0.0042,-0.0007,-0.12,0.21,-6e-05,0.44,-0.005,0.0021,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.02,0.042,0.0056,0.064,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00045,3.9e-05,0.00043,0.00041,0.00045,0.00043,1,1,0.01 -27890000,0.72,0.027,0.049,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00023,-0.0042,-0.00071,-0.12,0.21,-6.1e-05,0.44,-0.005,0.002,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.021,0.043,0.0057,0.07,0.087,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00044,3.9e-05,0.00043,0.00041,0.00044,0.00042,1,1,0.01 -27990000,0.72,0.026,0.046,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00024,-0.004,0.00022,-0.12,0.21,-7.7e-05,0.44,-0.0064,0.0015,-0.0071,0,0,-4.9e+02,8e-05,6.1e-05,0.0087,0.02,0.038,0.0056,0.072,0.087,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00043,3.9e-05,0.00042,0.00037,0.00043,0.00042,1,1,0.01 -28090000,0.72,0.032,0.059,0.69,-2.8,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00023,-0.0042,0.00043,-0.12,0.21,-7.7e-05,0.44,-0.0065,0.0014,-0.0072,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0086,0.021,0.039,0.0057,0.078,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00042,3.9e-05,0.00042,0.00036,0.00042,0.00041,1,1,0.01 -28190000,0.72,0.037,0.072,0.69,-2.8,-1.2,-0.94,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0042,0.00087,-0.12,0.21,-9.5e-05,0.44,-0.0073,0.00091,-0.0071,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0079,0.02,0.034,0.0056,0.08,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00041,3.9e-05,0.00041,0.00033,0.00041,0.00041,1,1,0.01 -28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0043,0.00098,-0.12,0.21,-9.8e-05,0.44,-0.0073,0.0013,-0.0069,0,0,-4.9e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,0,0,-4.9e+02,-0.00094,-0.0059,0.00023,-0.0042,0.0013,-0.12,0.21,-8.5e-05,0.44,-0.0075,0.0014,-0.007,0,0,-4.9e+02,8.3e-05,6.2e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28490000,0.73,0.0028,0.006,0.69,-2.8,-1.2,1.1,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0039,0.0015,-0.12,0.21,-6.7e-05,0.44,-0.0076,0.0014,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28590000,0.73,0.00088,0.0025,0.69,-2.7,-1.2,0.97,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0039,0.0016,-0.12,0.21,-6.9e-05,0.44,-0.0075,0.0015,-0.007,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28690000,0.73,0.00018,0.0016,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00095,-0.0059,0.00024,-0.0036,0.0019,-0.12,0.21,-5.2e-05,0.44,-0.0076,0.0014,-0.0068,0,0,-4.9e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 -28790000,0.73,-1.6e-05,0.0015,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00098,-0.0059,0.00024,-0.0031,0.0022,-0.12,0.21,-9.3e-05,0.44,-0.0091,0.00063,-0.006,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 -28890000,0.73,-6.2e-06,0.0018,0.69,-2.5,-1.2,0.97,0,0,-4.9e+02,-0.00099,-0.0059,0.00024,-0.0029,0.0025,-0.12,0.21,-7.6e-05,0.44,-0.0091,0.00059,-0.0059,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 -28990000,0.73,0.00034,0.0024,0.68,-2.5,-1.1,0.97,0,0,-4.9e+02,-0.001,-0.0059,0.00025,-0.0016,0.0029,-0.12,0.21,-0.00011,0.44,-0.011,-0.00033,-0.0047,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.0003,0.00039,0.00039,1,1,0.01 -29090000,0.73,0.00051,0.0028,0.68,-2.4,-1.1,0.96,0,0,-4.9e+02,-0.001,-0.0059,0.00025,-0.0014,0.0033,-0.12,0.21,-9.3e-05,0.44,-0.011,-0.00039,-0.0046,0,0,-4.9e+02,8.8e-05,6.2e-05,0.0073,0.021,0.025,0.0057,0.14,0.17,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29190000,0.73,0.00076,0.0032,0.68,-2.4,-1.1,0.95,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00098,0.0034,-0.12,0.21,-0.00013,0.44,-0.011,-0.00062,-0.0042,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0072,0.02,0.023,0.0056,0.14,0.17,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29290000,0.73,0.0011,0.004,0.68,-2.3,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00095,0.0038,-0.12,0.21,-0.00012,0.44,-0.011,-0.00069,-0.0041,0,0,-4.9e+02,8.9e-05,6.2e-05,0.0072,0.021,0.024,0.0056,0.14,0.18,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29390000,0.73,0.0017,0.0055,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00022,0.0043,-0.12,0.21,-0.00015,0.44,-0.012,-0.0014,-0.0033,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.18,0.031,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 -29490000,0.73,0.0022,0.0066,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,-0.00021,0.0045,-0.12,0.21,-0.00015,0.44,-0.012,-0.0013,-0.0033,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.15,0.19,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 -29590000,0.73,0.0027,0.0076,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.00041,0.0045,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.19,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00038,1,1,0.01 -29690000,0.73,0.003,0.0082,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00025,0.00035,0.0049,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-4.9e+02,9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29790000,0.73,0.0034,0.0087,0.68,-2.2,-1.1,0.96,0,0,-4.9e+02,-0.0012,-0.0059,0.00025,0.0014,0.0047,-0.12,0.21,-0.00018,0.44,-0.012,-0.0019,-0.0023,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29890000,0.73,0.0034,0.0088,0.68,-2.1,-1.1,0.95,0,0,-4.9e+02,-0.0012,-0.0059,0.00024,0.0011,0.0052,-0.12,0.21,-0.00018,0.44,-0.013,-0.0019,-0.0023,0,0,-4.9e+02,9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.17,0.21,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29990000,0.73,0.0036,0.0089,0.68,-2.1,-1.1,0.94,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0016,0.0049,-0.12,0.21,-0.0002,0.44,-0.013,-0.0021,-0.002,0,0,-4.9e+02,8.9e-05,6e-05,0.0071,0.02,0.024,0.0056,0.17,0.21,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -30090000,0.73,0.0035,0.0088,0.68,-2.1,-1.1,0.92,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0013,0.0052,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.18,0.22,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -30190000,0.73,0.0036,0.0085,0.68,-2.1,-1.1,0.91,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0021,0.0047,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.02,0.025,0.0056,0.18,0.22,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00038,0.00028,0.00037,0.00038,1,1,0.01 -30290000,0.73,0.0034,0.0083,0.68,-2,-1.1,0.9,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0019,0.0049,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.9e-05,6.1e-05,0.007,0.021,0.026,0.0056,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30390000,0.73,0.0035,0.008,0.68,-2,-1.1,0.89,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0027,0.0047,-0.12,0.21,-0.00022,0.43,-0.012,-0.0025,-0.0014,0,0,-4.9e+02,8.7e-05,6e-05,0.007,0.021,0.025,0.0055,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30490000,0.73,0.0034,0.0077,0.68,-2,-1.1,0.87,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0026,0.0049,-0.12,0.21,-0.00022,0.43,-0.012,-0.0024,-0.0014,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.022,0.027,0.0056,0.2,0.24,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.8e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30590000,0.73,0.0034,0.0073,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0034,0.0045,-0.12,0.21,-0.00024,0.43,-0.012,-0.0025,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.021,0.026,0.0055,0.2,0.24,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30690000,0.73,0.0031,0.0069,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0032,0.0048,-0.12,0.21,-0.00024,0.43,-0.012,-0.0024,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.022,0.028,0.0055,0.21,0.25,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30790000,0.73,0.0031,0.0065,0.68,-1.9,-1,0.82,0,0,-4.9e+02,-0.0012,-0.0059,0.00014,0.004,0.0042,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00079,0,0,-4.9e+02,8.4e-05,6e-05,0.0068,0.021,0.027,0.0055,0.21,0.25,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30890000,0.73,0.0029,0.006,0.68,-1.9,-1,0.81,0,0,-4.9e+02,-0.0013,-0.0059,0.00015,0.004,0.0046,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00077,0,0,-4.9e+02,8.5e-05,6e-05,0.0068,0.022,0.029,0.0055,0.22,0.26,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30990000,0.73,0.003,0.0054,0.68,-1.8,-1,0.8,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0049,0.004,-0.12,0.21,-0.00025,0.43,-0.011,-0.0027,-0.00043,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.021,0.028,0.0055,0.22,0.26,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.5e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31090000,0.73,0.0027,0.0049,0.68,-1.8,-1,0.79,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0046,0.0044,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00043,0,0,-4.9e+02,8.4e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31190000,0.73,0.0026,0.0044,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.0049,0.0044,-0.12,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00026,0,0,-4.9e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31290000,0.73,0.0023,0.0039,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.9e-05,0.0047,0.0048,-0.11,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00029,0,0,-4.9e+02,8.2e-05,6e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00035,0.00037,1,1,0.01 -31390000,0.73,0.0022,0.0032,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.0051,0.0045,-0.11,0.21,-0.0003,0.43,-0.011,-0.0028,-3.3e-05,0,0,-4.9e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 -31490000,0.73,0.0019,0.0026,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.005,0.0051,-0.11,0.21,-0.0003,0.43,-0.011,-0.0029,3.9e-06,0,0,-4.9e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 -31590000,0.73,0.002,0.0021,0.68,-1.7,-0.97,0.77,0,0,-4.9e+02,-0.0013,-0.0058,4.6e-05,0.0059,0.0047,-0.11,0.21,-0.00029,0.43,-0.01,-0.0029,0.00024,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31690000,0.73,0.0016,0.0013,0.68,-1.6,-0.97,0.78,0,0,-4.9e+02,-0.0013,-0.0058,4.8e-05,0.0056,0.0051,-0.11,0.21,-0.0003,0.43,-0.01,-0.0029,0.00021,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31790000,0.73,0.0015,0.00056,0.69,-1.6,-0.95,0.78,0,0,-4.9e+02,-0.0013,-0.0058,2.3e-05,0.0067,0.0049,-0.11,0.21,-0.0003,0.43,-0.0097,-0.0029,0.00055,0,0,-4.9e+02,7.6e-05,5.9e-05,0.0062,0.021,0.029,0.0054,0.26,0.3,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31890000,0.73,0.0013,-0.00017,0.69,-1.6,-0.95,0.77,0,0,-4.9e+02,-0.0013,-0.0058,2.3e-05,0.0066,0.0054,-0.11,0.21,-0.00029,0.43,-0.0097,-0.0029,0.00058,0,0,-4.9e+02,7.7e-05,5.9e-05,0.0062,0.022,0.031,0.0054,0.27,0.31,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31990000,0.73,0.0012,-0.00078,0.69,-1.6,-0.93,0.77,0,0,-4.9e+02,-0.0013,-0.0058,-8e-06,0.0071,0.0053,-0.11,0.21,-0.00029,0.43,-0.0093,-0.003,0.00075,0,0,-4.9e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32090000,0.73,0.00083,-0.0015,0.69,-1.5,-0.93,0.78,0,0,-4.9e+02,-0.0013,-0.0058,-8.9e-06,0.0069,0.0058,-0.11,0.21,-0.00029,0.43,-0.0093,-0.003,0.00076,0,0,-4.9e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32190000,0.73,0.00062,-0.0025,0.69,-1.5,-0.91,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-4.3e-05,0.0074,0.0058,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.6e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32290000,0.73,0.00035,-0.0032,0.69,-1.5,-0.91,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-4.3e-05,0.0072,0.0064,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32390000,0.73,0.00025,-0.004,0.69,-1.5,-0.89,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-6.1e-05,0.0077,0.0063,-0.11,0.2,-0.00029,0.43,-0.0084,-0.0031,0.0011,0,0,-4.9e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32490000,0.72,0.0001,-0.0042,0.69,-1.4,-0.88,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-5.9e-05,0.0075,0.0067,-0.11,0.2,-0.0003,0.43,-0.0084,-0.0031,0.0011,0,0,-4.9e+02,7.2e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32590000,0.72,0.00015,-0.0046,0.69,-1.4,-0.87,0.78,0,0,-4.9e+02,-0.0014,-0.0057,-8e-05,0.0079,0.0067,-0.11,0.21,-0.0003,0.43,-0.008,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32690000,0.72,0.00011,-0.0046,0.69,-1.4,-0.86,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.0079,0.0071,-0.11,0.2,-0.0003,0.43,-0.008,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.022,0.032,0.0053,0.31,0.35,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00036,1,1,0.01 -32790000,0.72,0.00023,-0.0047,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.0001,0.0082,0.0071,-0.11,0.2,-0.0003,0.43,-0.0076,-0.0031,0.0015,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0054,0.022,0.03,0.0053,0.3,0.35,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -32890000,0.72,0.00031,-0.0046,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.0081,0.0077,-0.11,0.2,-0.0003,0.43,-0.0076,-0.0032,0.0016,0,0,-4.9e+02,6.9e-05,5.8e-05,0.0054,0.022,0.031,0.0053,0.32,0.36,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -32990000,0.72,0.00052,-0.0047,0.69,-1.3,-0.82,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.0086,0.0078,-0.11,0.2,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-4.9e+02,6.7e-05,5.7e-05,0.0052,0.021,0.029,0.0053,0.31,0.36,0.03,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -33090000,0.72,0.00049,-0.0048,0.69,-1.3,-0.82,0.76,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.0085,0.0081,-0.11,0.2,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0053,0.022,0.031,0.0053,0.33,0.37,0.031,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -33190000,0.72,0.0039,-0.0039,0.7,-1.2,-0.8,0.7,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.0086,0.008,-0.11,0.21,-0.0003,0.43,-0.0068,-0.0031,0.0017,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.029,0.0053,0.32,0.37,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 -33290000,0.67,0.016,-0.0033,0.74,-1.2,-0.78,0.68,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.0084,0.0083,-0.11,0.2,-0.00028,0.43,-0.007,-0.0032,0.0016,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.031,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.3e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 -33390000,0.56,0.014,-0.0037,0.83,-1.2,-0.77,0.88,0,0,-4.9e+02,-0.0014,-0.0057,-0.00013,0.0086,0.0086,-0.11,0.21,-0.00034,0.43,-0.0063,-0.0032,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 -33490000,0.43,0.007,-0.0011,0.9,-1.2,-0.76,0.89,0,0,-4.9e+02,-0.0014,-0.0057,-0.00015,0.0086,0.0087,-0.11,0.21,-0.00043,0.43,-0.0058,-0.002,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 -33590000,0.27,0.00094,-0.0036,0.96,-1.2,-0.75,0.86,0,0,-4.9e+02,-0.0014,-0.0057,-0.00018,0.0086,0.0087,-0.11,0.21,-0.00067,0.43,-0.0038,-0.0013,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 -33690000,0.098,-0.0027,-0.0066,1,-1.1,-0.74,0.87,0,0,-4.9e+02,-0.0014,-0.0057,-0.00019,0.0086,0.0087,-0.11,0.21,-0.00072,0.43,-0.0035,-0.001,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 -33790000,-0.074,-0.0045,-0.0084,1,-1.1,-0.72,0.85,0,0,-4.9e+02,-0.0014,-0.0057,-0.00021,0.0086,0.0087,-0.11,0.21,-0.00088,0.43,-0.0021,-0.001,0.0023,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 -33890000,-0.24,-0.0059,-0.009,0.97,-1,-0.69,0.83,0,0,-4.9e+02,-0.0014,-0.0057,-0.00022,0.0086,0.0087,-0.11,0.21,-0.001,0.43,-0.0012,-0.0011,0.0024,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 -33990000,-0.39,-0.0046,-0.012,0.92,-0.94,-0.64,0.8,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.0086,0.0088,-0.11,0.21,-0.00098,0.43,-0.0011,-0.00064,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 -34090000,-0.5,-0.0037,-0.014,0.87,-0.88,-0.59,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.0086,0.0091,-0.11,0.21,-0.00095,0.43,-0.0012,-0.00052,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0014,0.023,0.03,0.0052,0.37,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,3e-05,3.4e-05,0.00036,2.2e-05,2.3e-05,0.00036,1,1,0.01 -34190000,-0.57,-0.0037,-0.012,0.82,-0.85,-0.54,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0064,0.012,-0.11,0.21,-0.00094,0.43,-0.001,-0.00031,0.0027,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0013,0.023,0.029,0.0052,0.37,0.38,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.5e-05,3.4e-05,0.00036,1.8e-05,1.8e-05,0.00036,1,1,0.01 -34290000,-0.61,-0.0046,-0.0091,0.79,-0.8,-0.48,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0062,0.012,-0.11,0.21,-0.00096,0.43,-0.00092,-0.00018,0.0027,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0012,0.025,0.032,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.2e-05,3.4e-05,0.00036,1.5e-05,1.5e-05,0.00036,1,1,0.01 -34390000,-0.63,-0.0052,-0.0062,0.77,-0.77,-0.44,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0034,0.016,-0.11,0.21,-0.00092,0.43,-0.00091,1.7e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0012,0.025,0.031,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.2e-05,2e-05,3.3e-05,0.00036,1.3e-05,1.3e-05,0.00036,1,1,0.01 -34490000,-0.65,-0.0062,-0.004,0.76,-0.72,-0.39,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0032,0.016,-0.11,0.21,-0.00093,0.43,-0.00085,-2.4e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0011,0.027,0.035,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.6e-07,0.0034,0.0035,8.1e-05,1.8e-05,3.3e-05,0.00036,1.2e-05,1.2e-05,0.00036,1,1,0.01 -34590000,-0.66,-0.0062,-0.0026,0.75,-0.7,-0.37,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,-0.0018,0.023,-0.11,0.21,-0.00088,0.43,-0.00092,3.1e-05,0.0032,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.027,0.034,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,1.1e-05,1e-05,0.00036,1,1,0.01 -34690000,-0.67,-0.0066,-0.0018,0.75,-0.64,-0.32,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,-0.002,0.023,-0.11,0.21,-0.00091,0.43,-0.00078,0.00023,0.0031,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.03,0.037,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,9.9e-06,9.4e-06,0.00036,1,1,0.01 -34790000,-0.67,-0.006,-0.0013,0.74,-0.63,-0.3,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.008,0.03,-0.11,0.21,-0.0009,0.43,-0.00062,0.00032,0.0032,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.029,0.036,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,9.1e-06,8.6e-06,0.00036,1,1,0.01 -34890000,-0.67,-0.006,-0.0012,0.74,-0.57,-0.26,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.0081,0.03,-0.11,0.21,-0.00089,0.43,-0.00063,0.00028,0.0033,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.032,0.04,0.0052,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,8.4e-06,7.9e-06,0.00036,1,1,0.01 -34990000,-0.67,-0.013,-0.0037,0.74,0.47,0.35,-0.043,0,0,-4.9e+02,-0.0016,-0.0058,-6.8e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00051,0.00034,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.001,0.034,0.046,0.0054,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.3e-05,3.3e-05,0.00036,8e-06,7.4e-06,0.00036,1,1,0.01 -35090000,-0.67,-0.013,-0.0037,0.74,0.6,0.39,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-6.9e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00047,0.00028,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.037,0.051,0.0055,0.42,0.43,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.5e-06,6.9e-06,0.00036,1,1,0.01 -35190000,-0.67,-0.013,-0.0038,0.74,0.63,0.43,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-7e-05,-0.016,0.039,-0.11,0.21,-0.00088,0.43,-0.00041,0.00032,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.041,0.055,0.0055,0.43,0.44,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.1e-06,6.4e-06,0.00036,1,1,0.01 -35290000,-0.67,-0.013,-0.0038,0.74,0.66,0.47,-0.099,0,0,-4.9e+02,-0.0016,-0.0058,-7.1e-05,-0.016,0.039,-0.11,0.21,-0.00089,0.43,-0.00033,0.00032,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.044,0.06,0.0055,0.44,0.45,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.8e-06,6.1e-06,0.00036,1,1,0.01 -35390000,-0.67,-0.013,-0.0038,0.74,0.69,0.51,-0.096,0,0,-4.9e+02,-0.0016,-0.0058,-7.4e-05,-0.016,0.039,-0.11,0.21,-0.00091,0.43,-0.00023,0.0003,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.048,0.064,0.0055,0.46,0.47,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.5e-06,5.8e-06,0.00036,1,1,0.01 -35490000,-0.67,-0.013,-0.0038,0.74,0.73,0.56,-0.095,0,0,-4.9e+02,-0.0016,-0.0058,-7.6e-05,-0.016,0.039,-0.11,0.21,-0.00092,0.43,-0.00014,0.00025,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.053,0.069,0.0055,0.47,0.48,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.2e-06,5.5e-06,0.00036,1,1,0.01 -35590000,-0.67,-0.013,-0.0038,0.74,0.76,0.6,-0.094,0,0,-4.9e+02,-0.0016,-0.0058,-7.5e-05,-0.016,0.039,-0.11,0.21,-0.00092,0.43,-0.00017,0.00025,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.057,0.075,0.0055,0.49,0.5,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6e-06,5.2e-06,0.00036,1,1,0.01 -35690000,-0.67,-0.013,-0.0038,0.74,0.79,0.64,-0.092,0,0,-4.9e+02,-0.0016,-0.0058,-7.7e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-9.2e-05,0.00024,0.0036,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.062,0.08,0.0056,0.51,0.52,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0032,8e-05,1e-05,3.3e-05,0.00036,5.8e-06,5e-06,0.00036,1,1,0.01 -35790000,-0.67,-0.013,-0.0038,0.74,0.82,0.68,-0.089,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-6.8e-05,0.00024,0.0036,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.067,0.086,0.0056,0.53,0.54,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0032,8e-05,1e-05,3.3e-05,0.00036,5.6e-06,4.8e-06,0.00036,1,1,0.023 -35890000,-0.67,-0.013,-0.0039,0.74,0.85,0.72,-0.086,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00094,0.43,-5.3e-05,0.00022,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.072,0.092,0.0056,0.55,0.56,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.4e-06,4.6e-06,0.00036,1,1,0.048 -35990000,-0.67,-0.013,-0.0038,0.74,0.88,0.76,-0.083,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-7.8e-05,0.00021,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.078,0.099,0.0056,0.57,0.59,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,9.9e-06,3.3e-05,0.00036,5.3e-06,4.5e-06,0.00036,1,1,0.073 -36090000,-0.68,-0.013,-0.0038,0.74,0.91,0.81,-0.079,0,0,-4.9e+02,-0.0016,-0.0058,-7.9e-05,-0.016,0.039,-0.11,0.21,-0.00094,0.43,-4.3e-05,0.00019,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.083,0.11,0.0056,0.6,0.62,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.7e-06,3.3e-05,0.00036,5.2e-06,4.3e-06,0.00036,1,1,0.099 -36190000,-0.68,-0.013,-0.0038,0.74,0.94,0.85,-0.075,0,0,-4.9e+02,-0.0016,-0.0058,-8.4e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,5.5e-05,0.00018,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.089,0.11,0.0056,0.63,0.65,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.5e-06,3.3e-05,0.00036,5e-06,4.2e-06,0.00036,1,1,0.12 -36290000,-0.68,-0.013,-0.0038,0.74,0.97,0.89,-0.07,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.016,0.039,-0.11,0.21,-0.00096,0.43,7.5e-05,0.00018,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.096,0.12,0.0056,0.66,0.68,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.4e-06,3.3e-05,0.00036,4.9e-06,4.1e-06,0.00036,1,1,0.15 -36390000,-0.68,-0.013,-0.0038,0.74,1,0.93,-0.067,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.016,0.039,-0.11,0.21,-0.00096,0.43,7.2e-05,0.00022,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.1,0.13,0.0056,0.69,0.72,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.3e-06,3.3e-05,0.00036,4.8e-06,3.9e-06,0.00036,1,1,0.17 -36490000,-0.68,-0.013,-0.0038,0.74,1,0.97,-0.063,0,0,-4.9e+02,-0.0016,-0.0058,-8.3e-05,-0.015,0.039,-0.11,0.21,-0.00095,0.43,4.2e-05,0.00023,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.11,0.13,0.0056,0.72,0.76,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.2e-06,3.3e-05,0.00036,4.7e-06,3.8e-06,0.00036,1,1,0.2 -36590000,-0.68,-0.013,-0.0038,0.74,1.1,1,-0.057,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.015,0.039,-0.11,0.21,-0.00096,0.43,9e-05,0.00025,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.14,0.0056,0.76,0.8,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.1e-06,3.3e-05,0.00036,4.6e-06,3.7e-06,0.00036,1,1,0.23 -36690000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.053,0,0,-4.9e+02,-0.0016,-0.0058,-8.8e-05,-0.015,0.039,-0.11,0.21,-0.00097,0.43,0.00012,0.00027,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.15,0.0057,0.8,0.85,0.032,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9e-06,3.3e-05,0.00036,4.6e-06,3.6e-06,0.00036,1,1,0.25 -36790000,-0.68,-0.013,-0.0037,0.74,1.1,1.1,-0.047,0,0,-4.9e+02,-0.0016,-0.0058,-9.1e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00017,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.13,0.16,0.0057,0.84,0.9,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.9e-06,3.3e-05,0.00036,4.5e-06,3.5e-06,0.00036,1,1,0.28 -36890000,-0.68,-0.013,-0.0037,0.74,1.2,1.1,-0.042,0,0,-4.9e+02,-0.0016,-0.0058,-9.4e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00021,0.00022,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0056,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00036,1,1,0.3 -36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00023,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00036,1,1,0.33 -37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00036,1,1,0.35 -37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.6e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.38 -37290000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.02,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00024,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00035,1,1,0.41 -37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00027,0.00027,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00035,1,1,0.43 -37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0093,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.015,0.038,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00035,1,1,0.46 -37590000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0027,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00032,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.2,0.23,0.0057,1.3,1.5,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.3e-06,3.3e-05,0.00036,4e-06,3e-06,0.00035,1,1,0.48 -37690000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,0.0046,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00033,0.00029,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.21,0.24,0.0057,1.4,1.6,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.51 -37790000,-0.68,-0.013,-0.0036,0.74,1.5,1.5,0.012,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00034,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.22,0.26,0.0056,1.5,1.7,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.54 -37890000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.017,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0038,0,0,-4.9e+02,4.5e-05,4.5e-05,0.00096,0.23,0.27,0.0056,1.6,1.8,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.56 -37990000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.025,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.24,0.28,0.0056,1.7,1.9,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.59 -38090000,-0.68,-0.013,-0.0036,0.74,1.5,1.7,0.034,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00037,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.25,0.29,0.0056,1.8,2,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.61 -38190000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.04,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00029,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.26,0.3,0.0056,1.9,2.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.64 -38290000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.046,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00028,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.27,0.31,0.0056,2.1,2.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.67 -38390000,-0.68,-0.013,-0.0035,0.74,1.6,1.8,0.052,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.036,-0.11,0.21,-0.001,0.43,0.00037,0.00029,0.0037,0,0,-4.9e+02,4.6e-05,4.6e-05,0.00096,0.28,0.33,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.69 -38490000,-0.68,-0.013,-0.0035,0.74,1.7,1.8,0.058,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.036,-0.11,0.21,-0.001,0.43,0.00038,0.00031,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.72 -38590000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.063,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00035,1,1,0.75 -38690000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.069,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.77 -38790000,-0.68,-0.013,-0.0034,0.74,1.8,1.9,0.075,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0037,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 -38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.00031,0.0038,0,0,-4.9e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 +1690000,1,-0.011,-0.014,0.00012,0.028,-9.5e-05,-0.19,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.011,-0.014,9.5e-05,0.035,-0.0019,-0.2,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.011,-0.015,7.5e-05,0.043,-0.0032,-0.22,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,8.5e-05,0.036,-0.0046,-0.23,0,0,-1.2e+02,0.00022,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,4.7e-05,0.041,-0.0071,-0.24,0,0,-1.2e+02,0.00022,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,5.9e-05,0.033,-0.0068,-0.26,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,4.5e-05,0.039,-0.0093,-0.27,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,6.2e-05,0.03,-0.0087,-0.29,0,0,-1.2e+02,9e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.013,4.4e-05,0.035,-0.011,-0.3,0,0,-1.2e+02,9e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.011,-0.013,5.9e-05,0.026,-0.009,-0.31,0,0,-1.2e+02,-1.4e-05,-0.0029,-5.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.011,-0.013,5.5e-05,0.03,-0.01,-0.33,0,0,-1.2e+02,-1.4e-05,-0.0029,-5.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.011,-0.013,4.9e-05,0.023,-0.0093,-0.34,0,0,-1.2e+02,-0.00012,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.011,-0.013,-1.1e-06,0.027,-0.011,-0.35,0,0,-1.2e+02,-0.00012,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.011,-0.013,4.6e-05,0.022,-0.0095,-0.36,0,0,-1.2e+02,-0.00023,-0.0036,-6.5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.011,-0.013,4.9e-05,0.025,-0.011,-0.38,0,0,-1.2e+02,-0.00023,-0.0036,-6.5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.011,-0.013,-8e-06,0.02,-0.0086,-0.39,0,0,-1.2e+02,-0.00034,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.011,-0.013,3.2e-05,0.023,-0.01,-0.4,0,0,-1.2e+02,-0.00034,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.011,-0.012,3.2e-06,0.018,-0.0091,-0.42,0,0,-1.2e+02,-0.00044,-0.0041,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.011,-0.013,-4.5e-06,0.022,-0.012,-0.43,0,0,-1.2e+02,-0.00044,-0.0041,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.011,-0.012,1.9e-05,0.017,-0.011,-0.44,0,0,-1.2e+02,-0.00055,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.011,-0.012,0.00014,0.019,-0.014,-0.46,0,0,-1.2e+02,-0.00055,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.011,-0.012,0.00019,0.016,-0.013,-0.47,0,0,-1.2e+02,-0.00067,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.48,0,0,-1.2e+02,-0.00067,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.011,-0.012,0.00016,0.02,-0.016,-0.5,0,0,-1.2e+02,-0.00067,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.51,0,0,-1.2e+02,-0.00079,-0.0047,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.011,-0.012,0.00013,0.02,-0.016,-0.53,0,0,-1.2e+02,-0.00079,-0.0047,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.01,-0.012,8.1e-05,0.017,-0.012,-0.54,0,0,-1.2e+02,-0.00091,-0.0049,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.01,-0.012,0.0001,0.018,-0.013,-0.55,0,0,-1.2e+02,-0.00091,-0.0049,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.01,-0.012,0.00016,0.014,-0.0097,-0.57,0,0,-1.2e+02,-0.001,-0.005,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.01,-0.012,0.00019,0.017,-0.011,-0.58,0,0,-1.2e+02,-0.001,-0.005,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.01,-0.012,0.00019,0.014,-0.0096,-0.6,0,0,-1.2e+02,-0.0011,-0.0052,-7.4e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.01,-0.012,0.00018,0.015,-0.011,-0.61,0,0,-1.2e+02,-0.0011,-0.0052,-7.4e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.01,-0.011,0.00017,0.012,-0.0097,-0.63,0,0,-1.2e+02,-0.0012,-0.0053,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.01,-0.012,0.00015,0.015,-0.01,-0.64,0,0,-1.2e+02,-0.0012,-0.0053,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.01,-0.011,0.0002,0.011,-0.0081,-0.66,0,0,-1.2e+02,-0.0013,-0.0054,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.01,-0.011,0.00022,0.013,-0.0095,-0.67,0,0,-1.2e+02,-0.0013,-0.0054,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.0099,-0.011,0.00021,0.0086,-0.007,-0.68,0,0,-1.2e+02,-0.0013,-0.0055,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0099,-0.011,0.00027,0.0081,-0.0078,-0.7,0,0,-1.2e+02,-0.0013,-0.0055,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0098,-0.011,0.00028,0.0055,-0.0059,-0.71,0,0,-1.2e+02,-0.0014,-0.0055,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0097,-0.011,0.00026,0.0061,-0.0063,-0.73,0,0,-1.2e+02,-0.0014,-0.0055,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0096,-0.011,0.00033,0.0041,-0.0036,-0.74,0,0,-1.2e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0095,-0.011,0.00033,0.0044,-0.0025,-0.75,0,0,-1.2e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0094,-0.011,0.00031,0.0038,-0.0007,0.0028,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0094,-0.011,0.00033,0.0041,0.00081,0.015,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0094,-0.011,0.00032,0.0051,0.002,-0.011,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0094,-0.011,0.00024,0.0038,0.0044,-0.005,0,0,-4.9e+02,-0.0015,-0.0056,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0094,-0.011,0.00022,0.005,0.0045,-0.012,0,0,-4.9e+02,-0.0015,-0.0056,-7.5e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0093,-0.011,0.00023,0.0043,0.0055,-0.05,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0093,-0.011,0.00023,0.0049,0.0057,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0093,-0.011,0.00016,0.0037,0.0057,-0.099,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0093,-0.011,9.4e-05,0.0046,0.0053,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0052,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 +6890000,0.71,0.0013,-0.014,0.7,0.00052,0.0065,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 +6990000,0.71,0.0013,-0.014,0.71,-0.00025,0.0048,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 +7090000,0.71,0.0012,-0.014,0.71,-0.00081,0.0013,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 +7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0021,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 +7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.01,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.012,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7590000,0.71,0.0017,-0.014,0.71,-0.0034,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7690000,0.71,0.0017,-0.014,0.71,-0.0046,0.018,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 +7790000,0.71,0.0017,-0.014,0.71,-0.011,0.017,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 +7890000,0.71,0.0017,-0.014,0.71,-0.011,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 +7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.023,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 +8090000,0.71,0.0017,-0.014,0.71,-0.0051,0.025,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8190000,0.71,0.0018,-0.014,0.71,-0.014,0.029,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8290000,0.71,0.0017,-0.014,0.71,-0.018,0.023,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00048,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00052,0.00039,-0.021,0.21,-9.6e-06,0.43,-0.00031,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8490000,0.71,0.0017,-0.014,0.71,-0.0029,0.0026,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00052,0.00039,-0.025,0.21,-9.3e-06,0.43,-0.00032,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8590000,0.71,0.0022,-0.014,0.71,-0.00042,0.00097,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-7e-05,-0.00052,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00043,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8690000,0.71,0.0021,-0.014,0.71,-0.0034,0.003,-0.16,0,0,-4.9e+02,-0.0017,-0.0056,-6.7e-05,-0.00052,0.00039,-0.035,0.21,-1.1e-05,0.43,-0.00041,0.00085,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.088,5.8e-05,6.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8790000,0.71,0.002,-0.014,0.71,-0.0056,0.0053,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00052,0.00043,-0.041,0.21,-9.5e-06,0.43,-0.00038,0.0008,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.087,5.6e-05,6.6e-05,2.4e-06,0.04,0.04,0.032,0.0013,6.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8890000,0.71,0.0019,-0.014,0.71,-0.0077,0.0061,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00065,0.00043,-0.045,0.21,-8.2e-06,0.43,-0.00036,0.00078,-0.00033,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.086,5.3e-05,6.4e-05,2.4e-06,0.04,0.04,0.03,0.0013,6.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +8990000,0.71,0.0019,-0.014,0.71,-0.01,0.0057,-0.14,0,0,-4.9e+02,-0.0015,-0.0058,-7.7e-05,-0.00088,0.00044,-0.051,0.21,-7e-06,0.43,-0.00032,0.00071,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.087,5.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.029,0.0013,6.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9090000,0.71,0.002,-0.014,0.71,-0.014,0.0071,-0.14,0,0,-4.9e+02,-0.0015,-0.0058,-7.7e-05,-0.001,0.00055,-0.053,0.21,-7.3e-06,0.43,-0.00034,0.00067,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1.1e+02,1.1e+02,0.086,4.9e-05,6.2e-05,2.4e-06,0.04,0.04,0.028,0.0013,5.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9190000,0.71,0.0019,-0.014,0.71,-0.012,0.01,-0.14,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00088,0.0006,-0.057,0.21,-6.7e-06,0.43,-0.00033,0.00081,-0.00025,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.094,1.1e+02,1.1e+02,0.085,4.6e-05,6e-05,2.4e-06,0.04,0.04,0.027,0.0013,5.6e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9290000,0.71,0.0018,-0.014,0.71,-0.011,0.01,-0.14,0,0,-4.9e+02,-0.0014,-0.0056,-7.3e-05,-0.00098,0.0006,-0.061,0.21,-5.5e-06,0.43,-0.00028,0.00082,-0.00024,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.1e+02,1.1e+02,0.085,4.4e-05,5.8e-05,2.4e-06,0.04,0.04,0.025,0.0013,5.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9390000,0.71,0.0016,-0.014,0.71,-0.011,0.011,-0.14,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,-0.0011,0.0006,-0.065,0.21,-4.7e-06,0.43,-0.00024,0.00081,-0.00022,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.2e+02,1.2e+02,0.086,4.2e-05,5.7e-05,2.4e-06,0.04,0.04,0.024,0.0013,5.2e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9490000,0.71,0.0015,-0.014,0.71,-0.0079,0.013,-0.13,0,0,-4.9e+02,-0.0013,-0.0055,-7.1e-05,-0.001,0.00062,-0.068,0.21,-4.1e-06,0.43,-0.00021,0.00089,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.091,1.2e+02,1.2e+02,0.085,4e-05,5.5e-05,2.4e-06,0.04,0.04,0.023,0.0013,5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9590000,0.71,0.0017,-0.014,0.71,-0.01,0.019,-0.13,0,0,-4.9e+02,-0.0014,-0.0055,-6.8e-05,-0.0011,0.00082,-0.072,0.21,-4.9e-06,0.43,-0.00027,0.0009,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.09,1.3e+02,1.3e+02,0.085,3.8e-05,5.4e-05,2.4e-06,0.04,0.04,0.021,0.0013,4.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9690000,0.71,0.0019,-0.014,0.71,-0.018,0.019,-0.12,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.0014,0.00095,-0.077,0.21,-5.2e-06,0.43,-0.00029,0.00079,-0.00017,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.089,1.3e+02,1.3e+02,0.086,3.6e-05,5.2e-05,2.4e-06,0.04,0.04,0.02,0.0013,4.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9790000,0.71,0.0018,-0.014,0.71,-0.013,0.023,-0.11,0,0,-4.9e+02,-0.0014,-0.0055,-7e-05,-0.0014,0.001,-0.082,0.21,-4.9e-06,0.43,-0.00027,0.00084,-0.0001,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.087,1.4e+02,1.4e+02,0.085,3.4e-05,5e-05,2.4e-06,0.04,0.04,0.019,0.0013,4.5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9890000,0.71,0.0019,-0.014,0.71,-0.017,0.026,-0.11,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.0015,0.0011,-0.085,0.21,-5e-06,0.43,-0.00028,0.00081,-0.00011,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.084,1.4e+02,1.4e+02,0.085,3.2e-05,4.9e-05,2.4e-06,0.04,0.04,0.018,0.0013,4.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9990000,0.71,0.002,-0.014,0.71,-0.021,0.024,-0.1,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.0017,0.0012,-0.089,0.21,-4.7e-06,0.43,-0.00028,0.00077,-0.00014,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.083,1.5e+02,1.5e+02,0.086,3.1e-05,4.7e-05,2.4e-06,0.04,0.04,0.017,0.0013,4.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +10090000,0.71,0.0021,-0.014,0.71,-0.023,0.029,-0.096,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.0017,0.0013,-0.091,0.21,-5.1e-06,0.43,-0.00031,0.00077,-0.00013,0,0,-4.9e+02,0.0013,0.0012,0.039,25,25,0.08,1.6e+02,1.6e+02,0.085,2.9e-05,4.6e-05,2.4e-06,0.04,0.04,0.016,0.0013,4.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10190000,0.71,0.0022,-0.014,0.71,-0.034,0.026,-0.096,0,0,-4.9e+02,-0.0015,-0.0058,-7.6e-05,-0.0019,0.0013,-0.093,0.21,-4.9e-06,0.43,-0.0003,0.00064,-0.00016,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.078,1.7e+02,1.7e+02,0.084,2.7e-05,4.4e-05,2.4e-06,0.04,0.04,0.014,0.0013,4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10290000,0.71,0.0021,-0.013,0.71,-0.041,0.022,-0.084,0,0,-4.9e+02,-0.0015,-0.0059,-8e-05,-0.0021,0.0015,-0.098,0.21,-4.6e-06,0.43,-0.00029,0.00056,-0.00018,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.076,1.8e+02,1.8e+02,0.085,2.6e-05,4.2e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10390000,0.71,0.002,-0.013,0.71,0.009,-0.02,0.0093,0,0,-4.9e+02,-0.0015,-0.0059,-8.1e-05,-0.0021,0.0014,-0.098,0.21,-4.1e-06,0.43,-0.00026,0.00058,-0.00014,0,0,-4.9e+02,0.0012,0.0012,0.039,0.25,0.25,0.56,0.5,0.5,0.077,2.4e-05,4.1e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10490000,0.71,0.0019,-0.013,0.71,0.007,-0.019,0.022,0,0,-4.9e+02,-0.0014,-0.0059,-8.5e-05,-0.0022,0.0014,-0.098,0.21,-3.7e-06,0.43,-0.00024,0.00052,-0.00017,0,0,-4.9e+02,0.0012,0.0012,0.039,0.26,0.26,0.55,0.51,0.51,0.079,2.3e-05,3.9e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10590000,0.71,0.0023,-0.013,0.71,0.0059,-0.0077,0.024,0,0,-4.9e+02,-0.0015,-0.0059,-8.1e-05,-0.0024,0.0018,-0.098,0.21,-5e-06,0.43,-0.00032,0.00051,-0.00018,0,0,-4.9e+02,0.0012,0.0011,0.039,0.13,0.13,0.27,0.17,0.17,0.072,2.2e-05,3.8e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10690000,0.71,0.0022,-0.013,0.71,0.0031,-0.0083,0.027,0,0,-4.9e+02,-0.0015,-0.0059,-8.3e-05,-0.0024,0.0018,-0.098,0.21,-4.6e-06,0.43,-0.0003,0.0005,-0.00018,0,0,-4.9e+02,0.0012,0.0011,0.039,0.14,0.14,0.26,0.18,0.18,0.077,2.1e-05,3.6e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10790000,0.71,0.0021,-0.013,0.71,0.0032,-0.0057,0.021,0,0,-4.9e+02,-0.0015,-0.0059,-8.2e-05,-0.0025,0.0022,-0.098,0.21,-4.5e-06,0.43,-0.00029,0.00054,-0.00017,0,0,-4.9e+02,0.0012,0.0011,0.038,0.093,0.094,0.17,0.11,0.11,0.071,1.9e-05,3.4e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10890000,0.71,0.002,-0.013,0.71,0.0018,-0.0056,0.016,0,0,-4.9e+02,-0.0014,-0.0059,-8.3e-05,-0.0025,0.0021,-0.098,0.21,-4.2e-06,0.43,-0.00027,0.00055,-0.00016,0,0,-4.9e+02,0.0011,0.0011,0.038,0.1,0.1,0.16,0.11,0.11,0.074,1.8e-05,3.3e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +10990000,0.71,0.0018,-0.014,0.71,0.0065,-0.00059,0.011,0,0,-4.9e+02,-0.0013,-0.0057,-8.1e-05,-0.0025,0.0031,-0.098,0.21,-3.7e-06,0.43,-0.00025,0.00069,-0.00012,0,0,-4.9e+02,0.0011,0.001,0.038,0.079,0.08,0.12,0.079,0.079,0.071,1.7e-05,3.1e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11090000,0.71,0.0019,-0.014,0.71,0.0064,0.003,0.014,0,0,-4.9e+02,-0.0014,-0.0056,-7.7e-05,-0.0023,0.0029,-0.098,0.21,-3.8e-06,0.43,-0.00026,0.00076,-7.8e-05,0,0,-4.9e+02,0.0011,0.00099,0.038,0.09,0.091,0.12,0.085,0.085,0.073,1.6e-05,2.9e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11190000,0.71,0.0018,-0.014,0.71,0.0098,0.0044,0.0027,0,0,-4.9e+02,-0.0013,-0.0057,-8e-05,-0.0021,0.004,-0.098,0.21,-3.9e-06,0.43,-0.00027,0.00077,-9.5e-05,0,0,-4.9e+02,0.00098,0.00092,0.038,0.074,0.075,0.091,0.066,0.066,0.068,1.5e-05,2.7e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11290000,0.71,0.0017,-0.014,0.71,0.0089,0.003,0.0014,0,0,-4.9e+02,-0.0013,-0.0057,-8.5e-05,-0.0025,0.0043,-0.098,0.21,-3.5e-06,0.43,-0.00024,0.00071,-0.00012,0,0,-4.9e+02,0.00098,0.00091,0.038,0.086,0.087,0.087,0.072,0.072,0.071,1.4e-05,2.6e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11390000,0.71,0.0017,-0.013,0.71,0.0044,0.002,-0.0044,0,0,-4.9e+02,-0.0013,-0.0058,-8.7e-05,-0.003,0.004,-0.098,0.21,-3.4e-06,0.43,-0.00024,0.00064,-0.00017,0,0,-4.9e+02,0.00087,0.00082,0.038,0.072,0.073,0.072,0.058,0.058,0.067,1.3e-05,2.4e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11490000,0.71,0.0015,-0.013,0.71,0.00059,-0.00071,-0.0044,0,0,-4.9e+02,-0.0012,-0.006,-9.4e-05,-0.0036,0.0048,-0.098,0.21,-3.2e-06,0.43,-0.00021,0.00054,-0.00023,0,0,-4.9e+02,0.00086,0.00081,0.038,0.083,0.086,0.069,0.064,0.064,0.068,1.3e-05,2.3e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11590000,0.71,0.0014,-0.013,0.71,-0.0025,-0.00042,-0.01,0,0,-4.9e+02,-0.0012,-0.006,-9.6e-05,-0.0041,0.0049,-0.098,0.21,-2.9e-06,0.43,-0.00018,0.00052,-0.00024,0,0,-4.9e+02,0.00075,0.00072,0.038,0.07,0.072,0.059,0.054,0.054,0.066,1.2e-05,2.1e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11690000,0.71,0.0012,-0.013,0.71,-0.0041,-0.0014,-0.016,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.0049,0.005,-0.098,0.21,-2.5e-06,0.43,-0.00013,0.00051,-0.00027,0,0,-4.9e+02,0.00075,0.00071,0.038,0.082,0.084,0.057,0.06,0.06,0.066,1.1e-05,2e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11790000,0.71,0.0011,-0.013,0.71,-0.008,0.00022,-0.018,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0066,0.0052,-0.098,0.21,-2.4e-06,0.43,-0.00011,0.00049,-0.00028,0,0,-4.9e+02,0.00065,0.00063,0.038,0.068,0.07,0.051,0.051,0.051,0.063,1e-05,1.8e-05,2.3e-06,0.028,0.03,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11890000,0.71,0.001,-0.013,0.71,-0.009,-0.0025,-0.02,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0072,0.0057,-0.098,0.21,-2.4e-06,0.43,-7.7e-05,0.00045,-0.00033,0,0,-4.9e+02,0.00065,0.00062,0.038,0.08,0.082,0.049,0.058,0.058,0.063,9.8e-06,1.8e-05,2.3e-06,0.028,0.029,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11990000,0.71,0.0014,-0.013,0.71,-0.012,0.0022,-0.026,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0067,0.0062,-0.097,0.21,-3.3e-06,0.43,-0.00015,0.00047,-0.00032,0,0,-4.9e+02,0.00056,0.00055,0.037,0.066,0.068,0.045,0.049,0.049,0.062,9.2e-06,1.6e-05,2.3e-06,0.026,0.028,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +12090000,0.71,0.0015,-0.013,0.71,-0.014,0.0055,-0.032,0,0,-4.9e+02,-0.0012,-0.006,-9.6e-05,-0.0056,0.0054,-0.097,0.21,-3.4e-06,0.43,-0.00018,0.00052,-0.00028,0,0,-4.9e+02,0.00056,0.00055,0.037,0.077,0.079,0.045,0.056,0.057,0.062,8.8e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12190000,0.71,0.0012,-0.013,0.71,-0.0072,0.0049,-0.026,0,0,-4.9e+02,-0.0011,-0.0059,-9.8e-05,-0.0054,0.0057,-0.099,0.21,-2.7e-06,0.43,-0.00013,0.00059,-0.00027,0,0,-4.9e+02,0.00048,0.00048,0.037,0.064,0.066,0.041,0.048,0.048,0.059,8.2e-06,1.4e-05,2.3e-06,0.023,0.026,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12290000,0.71,0.0011,-0.013,0.71,-0.0085,0.0054,-0.026,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.0059,0.0053,-0.099,0.21,-2.4e-06,0.43,-0.0001,0.0006,-0.00026,0,0,-4.9e+02,0.00048,0.00048,0.037,0.073,0.076,0.042,0.056,0.056,0.06,7.9e-06,1.4e-05,2.3e-06,0.023,0.025,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12390000,0.71,0.001,-0.013,0.71,-0.0057,0.004,-0.023,0,0,-4.9e+02,-0.0011,-0.0059,-0.0001,-0.0045,0.007,-0.1,0.21,-2.8e-06,0.43,-0.00014,0.0006,-0.00025,0,0,-4.9e+02,0.00042,0.00043,0.037,0.061,0.063,0.039,0.048,0.048,0.058,7.4e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12490000,0.71,0.00098,-0.013,0.71,-0.0074,0.0036,-0.027,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0049,0.0082,-0.1,0.21,-3.1e-06,0.43,-0.00017,0.00054,-0.00025,0,0,-4.9e+02,0.00042,0.00042,0.037,0.069,0.072,0.04,0.055,0.055,0.058,7.1e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12590000,0.71,0.0012,-0.013,0.71,-0.015,0.0044,-0.033,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0042,0.0074,-0.1,0.21,-3.3e-06,0.43,-0.00021,0.00051,-0.00024,0,0,-4.9e+02,0.00037,0.00038,0.037,0.058,0.059,0.038,0.047,0.048,0.057,6.8e-06,1.2e-05,2.3e-06,0.019,0.022,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12690000,0.71,0.0014,-0.013,0.71,-0.018,0.0053,-0.037,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0024,0.0088,-0.1,0.21,-4.3e-06,0.43,-0.00028,0.00049,-0.00025,0,0,-4.9e+02,0.00037,0.00038,0.037,0.065,0.067,0.039,0.055,0.055,0.057,6.5e-06,1.1e-05,2.3e-06,0.019,0.022,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12790000,0.71,0.0013,-0.013,0.71,-0.02,0.0033,-0.041,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0039,0.0076,-0.1,0.21,-3.6e-06,0.43,-0.00023,0.00048,-0.00027,0,0,-4.9e+02,0.00033,0.00034,0.037,0.054,0.056,0.037,0.047,0.047,0.056,6.2e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12890000,0.71,0.0011,-0.013,0.71,-0.02,0.002,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0052,0.0072,-0.1,0.21,-3.2e-06,0.43,-0.00019,0.00047,-0.00026,0,0,-4.9e+02,0.00033,0.00034,0.037,0.061,0.063,0.038,0.054,0.055,0.057,6e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +12990000,0.71,0.001,-0.013,0.71,-0.0092,0.0023,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0027,0.0076,-0.1,0.21,-3e-06,0.43,-0.0002,0.00058,-0.0002,0,0,-4.9e+02,0.0003,0.00031,0.037,0.051,0.053,0.037,0.047,0.047,0.055,5.7e-06,9.9e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13090000,0.71,0.00097,-0.013,0.71,-0.0099,0.00034,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.00011,-0.0038,0.0092,-0.1,0.21,-3.4e-06,0.43,-0.00022,0.00052,-0.00023,0,0,-4.9e+02,0.0003,0.00031,0.037,0.057,0.059,0.038,0.054,0.054,0.056,5.5e-06,9.6e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13190000,0.71,0.00097,-0.013,0.71,-0.0025,0.0012,-0.034,0,0,-4.9e+02,-0.0011,-0.006,-0.00011,-0.002,0.011,-0.11,0.21,-3.8e-06,0.43,-0.00025,0.00057,-0.00021,0,0,-4.9e+02,0.00027,0.00029,0.037,0.048,0.05,0.037,0.047,0.047,0.054,5.2e-06,9.1e-06,2.3e-06,0.015,0.019,0.0098,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13290000,0.71,0.00081,-0.013,0.71,-0.00098,0.0019,-0.029,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0035,0.009,-0.11,0.21,-2.8e-06,0.43,-0.00019,0.00058,-0.00019,0,0,-4.9e+02,0.00027,0.00028,0.037,0.054,0.055,0.038,0.054,0.054,0.055,5.1e-06,8.9e-06,2.3e-06,0.015,0.019,0.0097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13390000,0.71,0.00072,-0.013,0.71,8e-05,0.0026,-0.024,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0029,0.0083,-0.11,0.21,-2.6e-06,0.43,-0.00017,0.00063,-0.00019,0,0,-4.9e+02,0.00025,0.00026,0.037,0.045,0.047,0.037,0.047,0.047,0.054,4.8e-06,8.5e-06,2.3e-06,0.014,0.018,0.0091,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13490000,0.71,0.00072,-0.013,0.71,5e-05,0.0027,-0.022,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0029,0.0075,-0.11,0.21,-2.2e-06,0.43,-0.00016,0.00064,-0.00017,0,0,-4.9e+02,0.00025,0.00026,0.037,0.05,0.052,0.038,0.054,0.054,0.055,4.7e-06,8.2e-06,2.3e-06,0.014,0.018,0.009,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13590000,0.71,0.00074,-0.013,0.71,-0.00015,0.003,-0.024,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0027,0.0087,-0.11,0.21,-2.7e-06,0.43,-0.00019,0.00063,-0.00019,0,0,-4.9e+02,0.00023,0.00025,0.037,0.042,0.044,0.037,0.046,0.047,0.054,4.5e-06,7.9e-06,2.3e-06,0.013,0.017,0.0085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13690000,0.71,0.00072,-0.013,0.71,0.00078,0.0055,-0.029,0,0,-4.9e+02,-0.001,-0.0059,-9.9e-05,-0.0021,0.0075,-0.11,0.21,-2.3e-06,0.43,-0.00017,0.00065,-0.00016,0,0,-4.9e+02,0.00023,0.00024,0.037,0.047,0.049,0.038,0.053,0.054,0.055,4.3e-06,7.7e-06,2.3e-06,0.013,0.017,0.0083,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13790000,0.71,0.00076,-0.013,0.71,0.0005,0.0022,-0.03,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.00065,0.0082,-0.11,0.21,-2.7e-06,0.43,-0.00021,0.00066,-0.00014,0,0,-4.9e+02,0.00022,0.00023,0.037,0.04,0.041,0.036,0.046,0.046,0.053,4.2e-06,7.3e-06,2.3e-06,0.013,0.016,0.0078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13890000,0.71,0.0006,-0.013,0.71,0.0023,0.0023,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-9.9e-05,-0.002,0.0071,-0.11,0.21,-2e-06,0.43,-0.00016,0.00066,-0.00015,0,0,-4.9e+02,0.00022,0.00023,0.037,0.044,0.046,0.037,0.053,0.053,0.055,4e-06,7.1e-06,2.3e-06,0.012,0.016,0.0076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13990000,0.71,0.00067,-0.013,0.71,0.0018,0.00057,-0.034,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.00073,0.0076,-0.11,0.21,-2.3e-06,0.43,-0.0002,0.00066,-0.00013,0,0,-4.9e+02,0.00021,0.00022,0.037,0.037,0.039,0.036,0.046,0.046,0.054,3.9e-06,6.8e-06,2.3e-06,0.012,0.015,0.0071,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +14090000,0.71,0.00073,-0.013,0.71,0.0013,0.0019,-0.035,0,0,-4.9e+02,-0.0011,-0.0059,-9.4e-05,0.00078,0.0065,-0.11,0.21,-2e-06,0.43,-0.0002,0.0007,-8.9e-05,0,0,-4.9e+02,0.00021,0.00022,0.037,0.041,0.043,0.036,0.053,0.053,0.054,3.8e-06,6.7e-06,2.3e-06,0.012,0.015,0.007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14190000,0.71,0.00069,-0.014,0.71,0.0044,0.0018,-0.037,0,0,-4.9e+02,-0.0011,-0.0059,-9.3e-05,0.0013,0.0062,-0.11,0.21,-1.8e-06,0.43,-0.00019,0.00072,-7.1e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.035,0.037,0.035,0.046,0.046,0.054,3.6e-06,6.4e-06,2.3e-06,0.011,0.015,0.0065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14290000,0.71,0.00076,-0.014,0.71,0.0046,0.0031,-0.035,0,0,-4.9e+02,-0.0011,-0.0059,-9.2e-05,0.0021,0.0062,-0.11,0.21,-1.9e-06,0.43,-0.0002,0.00072,-5.3e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.039,0.04,0.036,0.052,0.052,0.055,3.5e-06,6.2e-06,2.3e-06,0.011,0.014,0.0063,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14390000,0.71,0.00064,-0.014,0.71,0.007,0.0049,-0.037,0,0,-4.9e+02,-0.0011,-0.0058,-8.9e-05,0.0018,0.0047,-0.11,0.21,-1.1e-06,0.43,-0.00016,0.00075,-3.7e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14490000,0.71,0.00053,-0.014,0.71,0.008,0.0064,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,0.00054,0.0041,-0.11,0.21,-5.6e-07,0.43,-0.00014,0.00072,-3.7e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14590000,0.71,0.00043,-0.013,0.71,0.0061,0.0048,-0.041,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.00033,0.0039,-0.11,0.21,-4.7e-07,0.43,-0.00013,0.00069,-5.1e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14690000,0.71,0.00038,-0.013,0.71,0.0078,0.0029,-0.038,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.00028,0.003,-0.11,0.21,-7.5e-08,0.43,-0.00012,0.0007,-3.6e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14790000,0.71,0.00036,-0.013,0.71,0.0055,0.0014,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-8.6e-05,-0.00044,0.0028,-0.12,0.21,-1.3e-07,0.43,-0.00012,0.00068,-3.7e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0098,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0031,-0.037,0,0,-4.9e+02,-0.001,-0.0058,-8.5e-05,-0.00068,0.0022,-0.12,0.21,1.4e-07,0.43,-0.00011,0.00068,-3.4e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0097,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +14990000,0.71,0.00028,-0.013,0.71,0.0068,0.0021,-0.032,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.0012,0.0027,-0.12,0.21,-6.2e-08,0.43,-0.00012,0.00066,-4.6e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.013,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15090000,0.71,0.00021,-0.013,0.71,0.0075,0.0021,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.0013,0.0031,-0.12,0.21,-1.6e-07,0.43,-0.00013,0.00064,-4.7e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.031,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0093,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15190000,0.71,0.00017,-0.013,0.71,0.0073,0.0027,-0.033,0,0,-4.9e+02,-0.00099,-0.0059,-8.9e-05,-0.0019,0.0034,-0.12,0.21,-1.5e-07,0.43,-0.00014,0.00061,-5.4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.0091,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15290000,0.71,0.00019,-0.013,0.71,0.0077,0.0037,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.00097,0.0032,-0.12,0.21,-9e-08,0.43,-0.00015,0.00061,-2.9e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15390000,0.71,0.0002,-0.013,0.71,0.007,0.005,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.3e-05,-0.00013,0.0017,-0.12,0.21,2.8e-07,0.43,-0.00014,0.00064,-5.8e-06,0,0,-4.9e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15490000,0.71,0.00022,-0.013,0.71,0.0086,0.0041,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.6e-05,-0.00053,0.003,-0.12,0.21,-1.8e-07,0.43,-0.00016,0.00061,-2.4e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15590000,0.71,0.00018,-0.013,0.71,0.0072,0.0031,-0.027,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.0012,0.0037,-0.12,0.21,-5.4e-07,0.43,-0.00016,0.0006,-4.9e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0032,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.00068,0.0041,-0.12,0.21,-8.5e-07,0.43,-0.00017,0.0006,-5.2e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15790000,0.71,0.00019,-0.013,0.71,0.0081,0.0017,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.00088,0.0043,-0.12,0.21,-9.6e-07,0.43,-0.00018,0.00059,-5.6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0082,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15890000,0.71,0.0002,-0.013,0.71,0.0088,0.0016,-0.028,0,0,-4.9e+02,-0.0011,-0.0059,-8.8e-05,-0.00015,0.0046,-0.12,0.21,-1.2e-06,0.43,-0.00019,0.00059,-4.7e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.2e-06,3.8e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15990000,0.71,0.00018,-0.013,0.71,0.0085,0.0017,-0.024,0,0,-4.9e+02,-0.0011,-0.0059,-8.3e-05,0.00049,0.0038,-0.12,0.21,-1e-06,0.43,-0.0002,0.0006,-2.9e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +16090000,0.71,0.00021,-0.013,0.71,0.011,0.0034,-0.02,0,0,-4.9e+02,-0.0011,-0.0059,-7.9e-05,0.0013,0.0025,-0.12,0.21,-7.5e-07,0.43,-0.00017,0.00065,-1.3e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.049,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16190000,0.71,0.00026,-0.013,0.71,0.01,0.0036,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.002,0.0027,-0.12,0.21,-1.1e-06,0.43,-0.00018,0.00065,-6.5e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16290000,0.71,0.00028,-0.014,0.71,0.012,0.0046,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0024,0.0014,-0.12,0.21,-5.8e-07,0.43,-0.00017,0.00067,8.3e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.048,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16390000,0.71,0.00033,-0.014,0.71,0.01,0.0031,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.0035,0.0025,-0.12,0.21,-1.4e-06,0.43,-0.00019,0.00066,1.3e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.023,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16490000,0.71,0.00042,-0.014,0.71,0.0088,0.0044,-0.022,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0044,0.0027,-0.12,0.21,-1.6e-06,0.43,-0.0002,0.00066,2.7e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16590000,0.71,0.00052,-0.013,0.71,0.0067,0.0053,-0.023,0,0,-4.9e+02,-0.0012,-0.0058,-7.5e-05,0.0045,0.0024,-0.12,0.21,-1.6e-06,0.43,-0.00019,0.00065,2.3e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0095,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16690000,0.71,0.00049,-0.013,0.71,0.0076,0.0056,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.0039,0.0031,-0.12,0.21,-1.8e-06,0.43,-0.00019,0.00065,1.2e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16790000,0.71,0.00048,-0.013,0.71,0.0057,0.0063,-0.018,0,0,-4.9e+02,-0.0011,-0.0058,-7.9e-05,0.0037,0.0027,-0.12,0.21,-1.7e-06,0.43,-0.00017,0.00065,-6e-07,0,0,-4.9e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0093,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16890000,0.71,0.00054,-0.013,0.71,0.0055,0.0072,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.0041,0.0033,-0.13,0.21,-2e-06,0.43,-0.00018,0.00064,4e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +16990000,0.71,0.00051,-0.013,0.71,0.0055,0.0049,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.004,0.0043,-0.13,0.21,-2.5e-06,0.43,-0.0002,0.00063,-6.1e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17090000,0.71,0.00055,-0.013,0.71,0.0058,0.0064,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8e-05,0.005,0.0046,-0.13,0.21,-2.8e-06,0.43,-0.00022,0.00063,6.2e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17190000,0.71,0.0006,-0.013,0.71,0.0059,0.0074,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-7.6e-05,0.0057,0.0045,-0.13,0.21,-3.1e-06,0.43,-0.00022,0.00064,8.8e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0068,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17290000,0.71,0.00063,-0.013,0.71,0.0077,0.0081,-0.011,0,0,-4.9e+02,-0.0012,-0.0059,-7.8e-05,0.0061,0.0055,-0.13,0.21,-3.4e-06,0.43,-0.00023,0.00063,1.1e-05,0,0,-4.9e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0086,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17390000,0.71,0.00068,-0.013,0.71,0.0075,0.0085,-0.0095,0,0,-4.9e+02,-0.0012,-0.0059,-7.2e-05,0.0069,0.0052,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00063,2.9e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.5e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17490000,0.71,0.00063,-0.013,0.71,0.0091,0.0086,-0.0078,0,0,-4.9e+02,-0.0012,-0.0059,-7.2e-05,0.0063,0.005,-0.13,0.21,-3.4e-06,0.43,-0.00025,0.00063,2.4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17590000,0.71,0.0006,-0.013,0.71,0.0099,0.0079,-0.0024,0,0,-4.9e+02,-0.0012,-0.0059,-6.9e-05,0.0067,0.0052,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00064,2.2e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0065,0.0083,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17690000,0.71,0.00057,-0.013,0.71,0.011,0.0094,-0.003,0,0,-4.9e+02,-0.0012,-0.0059,-6.8e-05,0.0068,0.005,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00064,3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.045,0.046,0.048,1.3e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17790000,0.71,0.00056,-0.013,0.71,0.012,0.01,-0.0042,0,0,-4.9e+02,-0.0012,-0.0059,-5.9e-05,0.0077,0.0039,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00066,4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.0081,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17890000,0.71,0.00055,-0.013,0.71,0.015,0.011,-0.004,0,0,-4.9e+02,-0.0012,-0.0059,-5.7e-05,0.0076,0.0032,-0.13,0.21,-3.2e-06,0.43,-0.00025,0.00067,4.4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.02,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17990000,0.71,0.00048,-0.013,0.71,0.016,0.0084,-0.0027,0,0,-4.9e+02,-0.0012,-0.0059,-5.6e-05,0.0073,0.0035,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00066,4.1e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0078,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +18090000,0.71,0.00047,-0.013,0.71,0.017,0.0076,-0.00044,0,0,-4.9e+02,-0.0012,-0.0059,-6.1e-05,0.0068,0.0045,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.6e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18190000,0.71,0.00043,-0.013,0.71,0.018,0.0086,0.001,0,0,-4.9e+02,-0.0012,-0.0059,-5.7e-05,0.007,0.0041,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00065,3.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.041,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0077,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18290000,0.71,0.00035,-0.013,0.71,0.018,0.0081,0.0022,0,0,-4.9e+02,-0.0012,-0.0059,-5.9e-05,0.0066,0.0044,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18390000,0.71,0.00031,-0.013,0.71,0.02,0.01,0.0035,0,0,-4.9e+02,-0.0012,-0.0059,-5.3e-05,0.0063,0.0036,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00065,3.3e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.006,0.0075,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18490000,0.71,0.00037,-0.013,0.71,0.021,0.011,0.0031,0,0,-4.9e+02,-0.0012,-0.0059,-5.2e-05,0.0069,0.0037,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00066,3.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18590000,0.71,0.00039,-0.013,0.71,0.02,0.012,0.0014,0,0,-4.9e+02,-0.0012,-0.0059,-4.4e-05,0.0077,0.0031,-0.13,0.21,-3.6e-06,0.43,-0.00026,0.00067,4.2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18690000,0.71,0.0003,-0.013,0.71,0.022,0.012,-0.00039,0,0,-4.9e+02,-0.0012,-0.0059,-4.6e-05,0.007,0.0031,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00066,3.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18790000,0.71,0.00032,-0.013,0.71,0.021,0.011,-0.00061,0,0,-4.9e+02,-0.0012,-0.0059,-4.5e-05,0.0072,0.0037,-0.13,0.21,-3.7e-06,0.43,-0.00026,0.00065,3.3e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00067,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18890000,0.71,0.00041,-0.013,0.71,0.021,0.013,1.6e-05,0,0,-4.9e+02,-0.0012,-0.0059,-3.9e-05,0.0082,0.0032,-0.13,0.21,-3.8e-06,0.43,-0.00027,0.00067,4.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +18990000,0.71,0.00046,-0.013,0.71,0.021,0.014,-0.0012,0,0,-4.9e+02,-0.0013,-0.0059,-3.3e-05,0.0088,0.0033,-0.13,0.21,-4.1e-06,0.43,-0.00028,0.00068,4.8e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.015,0.017,0.012,0.039,0.04,0.044,9.7e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.00062,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19090000,0.71,0.0005,-0.013,0.71,0.021,0.015,0.0018,0,0,-4.9e+02,-0.0013,-0.0059,-3.3e-05,0.0094,0.0036,-0.13,0.21,-4.3e-06,0.43,-0.00029,0.00069,5.2e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.016,0.019,0.012,0.042,0.044,0.044,9.6e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.0006,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19190000,0.71,0.00055,-0.013,0.71,0.02,0.015,0.0019,0,0,-4.9e+02,-0.0013,-0.0059,-2.7e-05,0.0099,0.0037,-0.13,0.21,-4.5e-06,0.43,-0.00031,0.00069,5.6e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.012,0.038,0.04,0.043,9.3e-07,1.5e-06,2.1e-06,0.0055,0.0068,0.00058,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19290000,0.71,0.00058,-0.013,0.71,0.02,0.015,0.0046,0,0,-4.9e+02,-0.0013,-0.0059,-3e-05,0.0098,0.0041,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00068,5.8e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.019,0.012,0.042,0.044,0.043,9.2e-07,1.5e-06,2.1e-06,0.0055,0.0067,0.00056,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19390000,0.71,0.00054,-0.013,0.71,0.019,0.014,0.0084,0,0,-4.9e+02,-0.0013,-0.0059,-2.3e-05,0.0097,0.004,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00068,5.3e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.011,0.038,0.04,0.043,8.9e-07,1.5e-06,2.1e-06,0.0055,0.0066,0.00054,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19490000,0.71,0.00055,-0.013,0.71,0.019,0.015,0.0049,0,0,-4.9e+02,-0.0013,-0.0059,-1.8e-05,0.0098,0.0033,-0.13,0.21,-4.4e-06,0.43,-0.0003,0.00069,5.6e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.018,0.011,0.042,0.044,0.043,8.8e-07,1.4e-06,2.1e-06,0.0054,0.0066,0.00052,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19590000,0.71,0.00063,-0.013,0.71,0.017,0.014,0.0043,0,0,-4.9e+02,-0.0013,-0.0059,-6.7e-06,0.011,0.003,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.0007,6.4e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.014,0.017,0.011,0.038,0.039,0.042,8.6e-07,1.4e-06,2.1e-06,0.0054,0.0065,0.0005,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19690000,0.71,0.00067,-0.013,0.71,0.017,0.012,0.0058,0,0,-4.9e+02,-0.0013,-0.0059,-1e-05,0.011,0.0036,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.0007,5.7e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.015,0.018,0.011,0.042,0.043,0.042,8.4e-07,1.4e-06,2.1e-06,0.0053,0.0065,0.00049,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19790000,0.71,0.00074,-0.013,0.71,0.015,0.01,0.0064,0,0,-4.9e+02,-0.0013,-0.0059,-5.7e-06,0.011,0.004,-0.13,0.21,-5.1e-06,0.43,-0.00032,0.0007,5.5e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.042,8.2e-07,1.3e-06,2.1e-06,0.0053,0.0064,0.00047,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19890000,0.71,0.00066,-0.013,0.71,0.015,0.012,0.0075,0,0,-4.9e+02,-0.0013,-0.0058,2.4e-06,0.011,0.0029,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00071,6.2e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.015,0.018,0.01,0.041,0.043,0.042,8.1e-07,1.3e-06,2.1e-06,0.0053,0.0063,0.00046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19990000,0.71,0.00063,-0.013,0.71,0.013,0.012,0.01,0,0,-4.9e+02,-0.0013,-0.0058,1.8e-05,0.012,0.0021,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00073,6.7e-05,0,0,-4.9e+02,0.0001,9.4e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.041,7.9e-07,1.3e-06,2.1e-06,0.0052,0.0062,0.00044,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +20090000,0.71,0.00066,-0.013,0.71,0.013,0.013,0.011,0,0,-4.9e+02,-0.0013,-0.0058,2.8e-05,0.012,0.0013,-0.13,0.21,-4.5e-06,0.43,-0.00032,0.00075,7.7e-05,0,0,-4.9e+02,0.00011,9.5e-05,0.036,0.014,0.018,0.01,0.041,0.043,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20190000,0.71,0.00069,-0.013,0.71,0.012,0.011,0.013,0,0,-4.9e+02,-0.0013,-0.0058,3.7e-05,0.012,0.001,-0.13,0.21,-4.5e-06,0.43,-0.00031,0.00075,7.6e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.013,0.016,0.0098,0.038,0.039,0.041,7.6e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00042,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20290000,0.71,0.0007,-0.013,0.71,0.01,0.011,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4e-05,0.012,0.00092,-0.13,0.21,-4.5e-06,0.43,-0.00032,0.00076,7.8e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.014,0.018,0.0097,0.041,0.043,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20390000,0.71,0.00066,-0.013,0.7,0.0086,0.0091,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.4e-05,0.012,0.001,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00076,6.8e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.0051,0.006,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20490000,0.71,0.00071,-0.013,0.7,0.0087,0.009,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.012,0.0013,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00075,6.6e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.006,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20590000,0.71,0.00074,-0.013,0.7,0.0078,0.007,0.0099,0,0,-4.9e+02,-0.0013,-0.0058,4.2e-05,0.012,0.0018,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00074,6.6e-05,0,0,-4.9e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0059,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20690000,0.71,0.00077,-0.013,0.7,0.0085,0.007,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.012,0.0016,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00075,6.7e-05,0,0,-4.9e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.005,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20790000,0.71,0.00081,-0.013,0.7,0.0063,0.0066,0.012,0,0,-4.9e+02,-0.0013,-0.0058,5e-05,0.013,0.0017,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00075,6.2e-05,0,0,-4.9e+02,9.8e-05,8.8e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0058,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20890000,0.71,0.00081,-0.013,0.7,0.0062,0.0062,0.011,0,0,-4.9e+02,-0.0013,-0.0058,5.8e-05,0.013,0.0013,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00077,6.7e-05,0,0,-4.9e+02,9.9e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +20990000,0.71,0.00083,-0.013,0.7,0.0045,0.0039,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.2e-05,0.013,0.0015,-0.13,0.21,-4.8e-06,0.43,-0.00034,0.00077,6.4e-05,0,0,-4.9e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0049,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21090000,0.71,0.00081,-0.013,0.7,0.0054,0.0032,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.013,0.0011,-0.13,0.21,-4.6e-06,0.43,-0.00033,0.00078,6.1e-05,0,0,-4.9e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21190000,0.71,0.00082,-0.013,0.7,0.0057,0.0023,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.013,0.0012,-0.13,0.21,-4.6e-06,0.43,-0.00033,0.00077,5.8e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.3e-07,9.5e-07,2e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21290000,0.71,0.0009,-0.013,0.7,0.005,0.0023,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.8e-05,0.013,0.00077,-0.13,0.21,-4.6e-06,0.43,-0.00034,0.0008,6.2e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.5e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21390000,0.71,0.00088,-0.013,0.7,0.004,0.00034,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.013,0.00093,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00079,6.2e-05,0,0,-4.9e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0055,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21490000,0.71,0.00088,-0.013,0.7,0.0045,0.00077,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.013,0.00058,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00079,6.7e-05,0,0,-4.9e+02,9.4e-05,8.3e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21590000,0.71,0.00087,-0.013,0.7,0.0034,0.0013,0.012,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.013,0.00061,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00079,6.4e-05,0,0,-4.9e+02,9.1e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0054,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21690000,0.71,0.00084,-0.013,0.7,0.005,0.0016,0.014,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.013,0.0002,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00079,6.4e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21790000,0.71,0.00084,-0.013,0.7,0.0031,0.0037,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7e-05,0.013,0.00038,-0.13,0.21,-5.4e-06,0.43,-0.00033,0.00079,6.6e-05,0,0,-4.9e+02,9e-05,8.1e-05,0.036,0.012,0.015,0.0079,0.037,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0043,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.013,0.00029,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00079,6.4e-05,0,0,-4.9e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21990000,0.71,0.00085,-0.013,0.7,0.0026,0.005,0.014,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00012,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.5e-05,0,0,-4.9e+02,8.9e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.5e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22090000,0.71,0.00088,-0.013,0.7,0.0025,0.0065,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00017,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.5e-05,0,0,-4.9e+02,8.9e-05,8e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22190000,0.71,0.00085,-0.013,0.7,0.002,0.0065,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.4e-05,0.014,0.00023,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22290000,0.71,0.00088,-0.013,0.7,0.0013,0.0062,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.014,0.00029,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.00079,5.6e-05,0,0,-4.9e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22390000,0.71,0.0009,-0.013,0.7,-0.00098,0.006,0.015,0,0,-4.9e+02,-0.0013,-0.0058,7.9e-05,0.014,0.00049,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.7e-05,0,0,-4.9e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22490000,0.71,0.00094,-0.013,0.7,-0.0021,0.0068,0.016,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.014,0.00063,-0.13,0.21,-5.4e-06,0.43,-0.00036,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22590000,0.71,0.00096,-0.013,0.7,-0.0038,0.0062,0.015,0,0,-4.9e+02,-0.0014,-0.0058,8.3e-05,0.015,0.0008,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.00081,5.2e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22690000,0.71,0.001,-0.013,0.7,-0.0051,0.0076,0.016,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.015,0.00069,-0.13,0.21,-5.3e-06,0.43,-0.00039,0.00082,5.1e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22790000,0.71,0.001,-0.013,0.7,-0.0071,0.0065,0.017,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.015,0.0015,-0.13,0.21,-5.5e-06,0.43,-0.00038,0.00081,5.7e-05,0,0,-4.9e+02,8.3e-05,7.5e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.8e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22890000,0.71,0.001,-0.013,0.7,-0.0075,0.0074,0.019,0,0,-4.9e+02,-0.0014,-0.0058,7.8e-05,0.015,0.0013,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.0008,5.3e-05,0,0,-4.9e+02,8.4e-05,7.5e-05,0.036,0.013,0.016,0.0071,0.039,0.042,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22990000,0.71,0.00097,-0.013,0.7,-0.0076,0.0065,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.8e-05,0.015,0.0012,-0.13,0.21,-5.1e-06,0.43,-0.00038,0.00081,5.1e-05,0,0,-4.9e+02,8.2e-05,7.4e-05,0.036,0.012,0.015,0.007,0.036,0.038,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23090000,0.71,0.00092,-0.013,0.7,-0.008,0.0063,0.02,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.015,0.0015,-0.13,0.21,-5.3e-06,0.43,-0.00038,0.00079,4.8e-05,0,0,-4.9e+02,8.3e-05,7.4e-05,0.036,0.013,0.016,0.007,0.039,0.042,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23190000,0.71,0.00098,-0.013,0.7,-0.0093,0.0044,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.2e-05,0.015,0.0017,-0.13,0.21,-5.3e-06,0.43,-0.00037,0.00079,4e-05,0,0,-4.9e+02,8.1e-05,7.3e-05,0.036,0.012,0.014,0.0069,0.036,0.038,0.035,4.6e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23290000,0.71,0.0009,-0.013,0.7,-0.0092,0.0038,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.4e-05,0.015,0.0015,-0.13,0.21,-5.2e-06,0.43,-0.00037,0.00079,3.9e-05,0,0,-4.9e+02,8.2e-05,7.3e-05,0.036,0.013,0.016,0.0069,0.039,0.042,0.036,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23390000,0.71,0.00095,-0.013,0.7,-0.0095,0.0026,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.6e-05,0.014,0.0015,-0.13,0.21,-5.3e-06,0.43,-0.00035,0.00078,4e-05,0,0,-4.9e+02,8e-05,7.2e-05,0.036,0.012,0.014,0.0068,0.036,0.038,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23490000,0.71,0.0033,-0.011,0.7,-0.016,0.0029,-0.013,0,0,-4.9e+02,-0.0014,-0.0058,9.2e-05,0.014,0.0013,-0.13,0.21,-5.3e-06,0.43,-0.00034,0.00081,6.5e-05,0,0,-4.9e+02,8.1e-05,7.2e-05,0.036,0.013,0.015,0.0068,0.039,0.042,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23590000,0.71,0.0086,-0.0027,0.7,-0.027,0.0029,-0.045,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.014,0.0013,-0.13,0.21,-5.2e-06,0.43,-0.00034,0.00087,0.00013,0,0,-4.9e+02,7.9e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23690000,0.71,0.0082,0.003,0.71,-0.058,-0.0049,-0.095,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.014,0.0013,-0.13,0.21,-5.2e-06,0.43,-0.00034,0.00081,0.0001,0,0,-4.9e+02,8e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0047,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23790000,0.71,0.0053,-0.00025,0.71,-0.083,-0.017,-0.15,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.014,0.00079,-0.13,0.21,-4.5e-06,0.43,-0.0004,0.0008,0.00045,0,0,-4.9e+02,7.8e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23890000,0.71,0.0026,-0.0063,0.71,-0.1,-0.025,-0.2,0,0,-4.9e+02,-0.0013,-0.0058,9e-05,0.014,0.00093,-0.13,0.21,-4.4e-06,0.43,-0.00042,0.00086,0.00036,0,0,-4.9e+02,7.9e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23990000,0.71,0.0013,-0.011,0.71,-0.1,-0.029,-0.26,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.014,0.00094,-0.13,0.21,-4e-06,0.43,-0.0004,0.00086,0.00034,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24090000,0.71,0.0025,-0.0096,0.71,-0.1,-0.028,-0.3,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.014,0.00064,-0.13,0.21,-3.6e-06,0.43,-0.00042,0.00083,0.00037,0,0,-4.9e+02,7.8e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24190000,0.71,0.0036,-0.0073,0.71,-0.11,-0.03,-0.35,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.014,0.00052,-0.13,0.21,-2.9e-06,0.43,-0.00043,0.00086,0.00037,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24290000,0.71,0.0041,-0.0065,0.71,-0.12,-0.033,-0.41,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.013,0.00049,-0.13,0.21,-2.6e-06,0.43,-0.00046,0.0009,0.00044,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24390000,0.71,0.0042,-0.0067,0.71,-0.13,-0.041,-0.46,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.013,0.0011,-0.13,0.21,2.4e-07,0.43,-0.00034,0.00095,0.00043,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24490000,0.71,0.0051,-0.0025,0.71,-0.14,-0.046,-0.51,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.013,0.0011,-0.13,0.21,2.4e-07,0.43,-0.00034,0.00096,0.00042,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24590000,0.71,0.0055,0.0012,0.71,-0.16,-0.057,-0.56,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.013,0.0011,-0.13,0.21,1.3e-06,0.43,1.3e-05,0.00061,0.00037,0,0,-4.9e+02,7.4e-05,6.7e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24690000,0.71,0.0056,0.0021,0.71,-0.18,-0.07,-0.64,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.013,0.00096,-0.13,0.21,2.3e-06,0.43,-3.2e-05,0.00065,0.00056,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24790000,0.71,0.0053,0.00083,0.71,-0.2,-0.084,-0.73,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.012,0.001,-0.13,0.21,1.6e-06,0.43,1.7e-07,0.00062,0.00032,0,0,-4.9e+02,7.3e-05,6.6e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24890000,0.71,0.0071,0.0025,0.71,-0.22,-0.095,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.012,0.0011,-0.13,0.21,2.4e-06,0.43,-0.00011,0.00077,0.00034,0,0,-4.9e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24990000,0.71,0.0089,0.0043,0.71,-0.24,-0.1,-0.81,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.012,0.00071,-0.13,0.21,1.8e-06,0.43,-0.00019,0.00087,-3.6e-06,0,0,-4.9e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25090000,0.71,0.0092,0.0037,0.71,-0.27,-0.11,-0.86,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.012,0.0008,-0.13,0.21,1.4e-06,0.43,-0.0002,0.00088,-3.9e-05,0,0,-4.9e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25190000,0.71,0.0087,0.0023,0.71,-0.3,-0.13,-0.91,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.011,0.0011,-0.13,0.21,6.8e-06,0.43,5e-05,0.00085,9.7e-05,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25290000,0.71,0.011,0.0091,0.71,-0.33,-0.14,-0.96,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.011,0.0011,-0.13,0.21,6.6e-06,0.43,7.6e-05,0.0008,0.0001,0,0,-4.9e+02,7.2e-05,6.5e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.011,0.00086,-0.13,0.21,1e-05,0.43,0.00048,0.00048,0.00014,0,0,-4.9e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,0.00014,0.011,0.00074,-0.13,0.21,9.2e-06,0.43,0.00065,0.00016,0.00033,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,0.00015,0.0099,0.0011,-0.13,0.21,1.5e-05,0.43,0.00095,0.00016,0.00034,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25690000,0.71,0.015,0.022,0.71,-0.49,-0.23,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0099,0.0011,-0.13,0.21,1.6e-05,0.43,0.00094,0.00018,0.00042,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 +25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0092,0.00041,-0.13,0.21,1.9e-05,0.43,0.0013,-6.7e-05,-3e-05,0,0,-4.9e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25890000,0.71,0.018,0.028,0.71,-0.62,-0.29,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00017,0.0094,0.00034,-0.13,0.21,2.1e-05,0.43,0.0014,7.9e-07,-9.6e-05,0,0,-4.9e+02,7.1e-05,6.3e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25990000,0.7,0.017,0.025,0.71,-0.67,-0.32,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0085,0.00068,-0.13,0.21,2.8e-05,0.43,0.0023,-0.00056,-0.00055,0,0,-4.9e+02,7e-05,6.2e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 +26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0086,0.00085,-0.13,0.21,2.4e-05,0.43,0.0024,-0.00048,-0.0012,0,0,-4.9e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 +26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0075,-0.00028,-0.13,0.21,3.8e-05,0.43,0.0023,0.00041,-0.0014,0,0,-4.9e+02,7.1e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 +26290000,0.7,0.025,0.047,0.71,-0.89,-0.43,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0075,-0.00026,-0.13,0.21,3.7e-05,0.43,0.0023,0.00028,-0.0014,0,0,-4.9e+02,7.1e-05,6.2e-05,0.03,0.015,0.028,0.0059,0.039,0.043,0.033,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.00099,0.0013,0.001,0.00099,1,1,0.01 +26390000,0.7,0.024,0.044,0.71,-0.96,-0.49,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00021,0.0066,0.00051,-0.13,0.21,4.4e-05,0.44,0.0036,-0.00018,-0.0024,0,0,-4.9e+02,7.1e-05,6.1e-05,0.028,0.014,0.027,0.0058,0.036,0.039,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00096,3.9e-05,0.00096,0.0012,0.00096,0.00095,1,1,0.01 +26490000,0.7,0.031,0.06,0.71,-1.1,-0.53,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.0002,0.0066,0.00051,-0.13,0.21,3.8e-05,0.44,0.0039,-0.00099,-0.0026,0,0,-4.9e+02,7.2e-05,6.1e-05,0.028,0.016,0.031,0.0058,0.039,0.044,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00092,3.9e-05,0.00092,0.0012,0.00092,0.00091,1,1,0.01 +26590000,0.7,0.037,0.076,0.71,-1.2,-0.59,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.005,-0.00049,-0.13,0.21,3.7e-05,0.44,0.0041,-0.00065,-0.0048,0,0,-4.9e+02,7.2e-05,6e-05,0.025,0.015,0.031,0.0058,0.036,0.04,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00087,3.9e-05,0.00086,0.001,0.00087,0.00086,1,1,0.01 +26690000,0.7,0.039,0.079,0.71,-1.3,-0.65,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.0051,-0.00058,-0.13,0.21,4.3e-05,0.44,0.0039,-0.00013,-0.004,0,0,-4.9e+02,7.2e-05,6.1e-05,0.025,0.017,0.038,0.0058,0.04,0.045,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00081,3.9e-05,0.0008,0.001,0.00081,0.00079,1,1,0.01 +26790000,0.7,0.036,0.073,0.71,-1.4,-0.74,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00022,0.0034,0.00028,-0.13,0.21,8.2e-05,0.44,0.0054,0.00061,-0.0037,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.016,0.036,0.0057,0.036,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00076,3.9e-05,0.00075,0.00092,0.00076,0.00074,1,1,0.01 +26890000,0.7,0.045,0.095,0.71,-1.6,-0.8,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00022,0.0035,0.00026,-0.13,0.21,8.7e-05,0.44,0.0053,0.0012,-0.0041,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.018,0.043,0.0058,0.04,0.046,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00072,3.9e-05,0.0007,0.00091,0.00072,0.0007,1,1,0.01 +26990000,0.7,0.051,0.12,0.71,-1.7,-0.89,-1.3,0,0,-4.9e+02,-0.00099,-0.0059,0.00022,0.0015,-0.0017,-0.13,0.21,0.00012,0.44,0.006,0.0034,-0.0056,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.017,0.042,0.0057,0.037,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00065,3.9e-05,0.00063,0.00079,0.00065,0.00062,1,1,0.01 +27090000,0.7,0.052,0.12,0.71,-1.9,-0.98,-1.3,0,0,-4.9e+02,-0.00098,-0.0059,0.00022,0.0015,-0.0016,-0.13,0.21,0.00012,0.44,0.006,0.0035,-0.0051,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.02,0.052,0.0057,0.04,0.048,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00059,3.9e-05,0.00056,0.00078,0.0006,0.00056,1,1,0.01 +27190000,0.7,0.05,0.11,0.7,-2.1,-1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00022,0.00037,-0.0018,-0.13,0.21,4.5e-05,0.44,0.002,0.0027,-0.0049,0,0,-4.9e+02,7.5e-05,6e-05,0.016,0.02,0.051,0.0057,0.043,0.05,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00055,3.9e-05,0.00051,0.00066,0.00055,0.00051,1,1,0.01 +27290000,0.71,0.044,0.095,0.7,-2.3,-1.1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00023,0.00042,-0.0018,-0.13,0.21,5.2e-05,0.44,0.0019,0.0034,-0.0049,0,0,-4.9e+02,7.6e-05,6.1e-05,0.016,0.022,0.059,0.0057,0.047,0.057,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00052,3.9e-05,0.00048,0.00066,0.00052,0.00048,1,1,0.01 +27390000,0.71,0.038,0.079,0.7,-2.4,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0058,0.00021,-0.00081,-0.0036,-0.13,0.21,9.9e-06,0.44,-0.0006,0.0031,-0.0063,0,0,-4.9e+02,7.6e-05,6e-05,0.013,0.021,0.052,0.0057,0.049,0.059,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00049,3.9e-05,0.00046,0.00055,0.00049,0.00046,1,1,0.01 +27490000,0.71,0.032,0.064,0.7,-2.5,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00022,-0.00074,-0.0035,-0.13,0.21,1.5e-05,0.44,-0.00068,0.0032,-0.0067,0,0,-4.9e+02,7.7e-05,6.1e-05,0.013,0.023,0.056,0.0057,0.054,0.067,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00048,3.9e-05,0.00045,0.00055,0.00048,0.00044,1,1,0.01 +27590000,0.72,0.028,0.051,0.69,-2.6,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00023,-0.0014,-0.0029,-0.12,0.21,-4.1e-05,0.44,-0.0033,0.0027,-0.0065,0,0,-4.9e+02,7.7e-05,6.1e-05,0.011,0.021,0.047,0.0057,0.056,0.068,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00044,1,1,0.01 +27690000,0.72,0.027,0.05,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00022,-0.0014,-0.0028,-0.12,0.21,-3.6e-05,0.44,-0.0034,0.0026,-0.0067,0,0,-4.9e+02,7.8e-05,6.1e-05,0.011,0.022,0.049,0.0057,0.062,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00043,1,1,0.01 +27790000,0.72,0.027,0.051,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00022,-0.0019,-0.0026,-0.12,0.21,-6.4e-05,0.44,-0.0052,0.002,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.02,0.042,0.0056,0.064,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00045,3.9e-05,0.00043,0.00041,0.00045,0.00043,1,1,0.01 +27890000,0.72,0.027,0.049,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00022,-0.002,-0.0026,-0.12,0.21,-6.5e-05,0.44,-0.0051,0.002,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.021,0.043,0.0057,0.07,0.087,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00044,3.9e-05,0.00043,0.00041,0.00044,0.00042,1,1,0.01 +27990000,0.72,0.026,0.046,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00024,-0.0018,-0.0017,-0.12,0.21,-8.1e-05,0.44,-0.0065,0.0014,-0.0071,0,0,-4.9e+02,8e-05,6.1e-05,0.0086,0.02,0.038,0.0056,0.072,0.087,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00043,3.9e-05,0.00042,0.00037,0.00043,0.00042,1,1,0.01 +28090000,0.72,0.032,0.059,0.69,-2.8,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00023,-0.0019,-0.0015,-0.12,0.21,-8e-05,0.44,-0.0066,0.0013,-0.0072,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0086,0.021,0.039,0.0057,0.078,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00042,3.9e-05,0.00042,0.00036,0.00042,0.00041,1,1,0.01 +28190000,0.72,0.037,0.072,0.69,-2.8,-1.2,-0.94,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.002,-0.001,-0.12,0.21,-9.9e-05,0.44,-0.0074,0.00088,-0.0071,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0079,0.02,0.034,0.0056,0.08,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00041,3.9e-05,0.00041,0.00033,0.00041,0.00041,1,1,0.01 +28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0021,-0.00091,-0.12,0.21,-0.0001,0.44,-0.0075,0.0013,-0.0069,0,0,-4.9e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,0,0,-4.9e+02,-0.00094,-0.0059,0.00023,-0.002,-0.00063,-0.12,0.21,-8.9e-05,0.44,-0.0076,0.0013,-0.007,0,0,-4.9e+02,8.3e-05,6.2e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28490000,0.73,0.0028,0.0059,0.69,-2.8,-1.2,1.1,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0016,-0.00036,-0.12,0.21,-7e-05,0.44,-0.0077,0.0014,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28590000,0.73,0.00089,0.0024,0.69,-2.7,-1.2,0.97,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0017,-0.00031,-0.12,0.21,-7.3e-05,0.44,-0.0076,0.0015,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28690000,0.73,0.00019,0.0015,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0014,4.3e-05,-0.12,0.21,-5.6e-05,0.44,-0.0077,0.0014,-0.0068,0,0,-4.9e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 +28790000,0.73,-1.2e-05,0.0014,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00098,-0.0059,0.00024,-0.00092,0.00026,-0.12,0.21,-9.6e-05,0.44,-0.0092,0.0006,-0.006,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 +28890000,0.73,-2.3e-06,0.0016,0.69,-2.5,-1.2,0.97,0,0,-4.9e+02,-0.00099,-0.0059,0.00024,-0.00064,0.00058,-0.12,0.21,-8e-05,0.44,-0.0093,0.00056,-0.0059,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 +28990000,0.73,0.00035,0.0022,0.68,-2.5,-1.1,0.97,0,0,-4.9e+02,-0.001,-0.0059,0.00025,0.00066,0.001,-0.12,0.21,-0.00011,0.44,-0.011,-0.00036,-0.0047,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.0003,0.00039,0.00039,1,1,0.01 +29090000,0.73,0.00052,0.0026,0.68,-2.4,-1.1,0.96,0,0,-4.9e+02,-0.001,-0.0059,0.00025,0.00085,0.0014,-0.12,0.21,-9.7e-05,0.44,-0.011,-0.00042,-0.0046,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.021,0.025,0.0057,0.14,0.17,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29190000,0.73,0.00076,0.003,0.68,-2.4,-1.1,0.96,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.0012,0.0014,-0.12,0.21,-0.00013,0.44,-0.011,-0.00065,-0.0041,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0072,0.02,0.023,0.0056,0.14,0.17,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29290000,0.73,0.0011,0.0038,0.68,-2.3,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.0013,0.0019,-0.12,0.21,-0.00012,0.44,-0.011,-0.00072,-0.0041,0,0,-4.9e+02,8.8e-05,6.2e-05,0.0072,0.021,0.024,0.0056,0.14,0.18,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29390000,0.73,0.0017,0.0054,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.002,0.0024,-0.12,0.21,-0.00016,0.44,-0.012,-0.0014,-0.0033,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.18,0.031,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 +29490000,0.73,0.0022,0.0064,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.002,0.0026,-0.12,0.21,-0.00015,0.44,-0.012,-0.0014,-0.0033,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.15,0.19,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 +29590000,0.73,0.0027,0.0075,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.0026,0.0026,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.19,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00038,1,1,0.01 +29690000,0.73,0.003,0.0081,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00025,0.0026,0.003,-0.12,0.21,-0.00016,0.44,-0.012,-0.0017,-0.0029,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29790000,0.73,0.0034,0.0085,0.68,-2.2,-1.1,0.96,0,0,-4.9e+02,-0.0012,-0.0059,0.00025,0.0036,0.0028,-0.12,0.21,-0.00018,0.44,-0.013,-0.0019,-0.0022,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29890000,0.73,0.0034,0.0087,0.68,-2.1,-1.1,0.95,0,0,-4.9e+02,-0.0012,-0.0059,0.00024,0.0033,0.0033,-0.12,0.21,-0.00018,0.44,-0.013,-0.002,-0.0023,0,0,-4.9e+02,9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.17,0.21,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29990000,0.73,0.0036,0.0087,0.68,-2.1,-1.1,0.94,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0038,0.0029,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-4.9e+02,8.9e-05,6e-05,0.0071,0.02,0.024,0.0056,0.17,0.21,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +30090000,0.73,0.0035,0.0086,0.68,-2.1,-1.1,0.93,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0035,0.0033,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.18,0.22,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00028,0.00037,0.00038,1,1,0.01 +30190000,0.73,0.0036,0.0084,0.68,-2.1,-1.1,0.91,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0043,0.0027,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.02,0.025,0.0055,0.18,0.22,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00038,0.00028,0.00037,0.00038,1,1,0.01 +30290000,0.73,0.0035,0.0081,0.68,-2,-1.1,0.9,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0041,0.0029,-0.12,0.21,-0.00023,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.9e-05,6.1e-05,0.007,0.021,0.026,0.0056,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30390000,0.73,0.0035,0.0079,0.68,-2,-1.1,0.89,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0049,0.0028,-0.12,0.21,-0.00022,0.43,-0.013,-0.0025,-0.0014,0,0,-4.9e+02,8.7e-05,6e-05,0.007,0.021,0.025,0.0055,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30490000,0.73,0.0034,0.0076,0.68,-2,-1.1,0.87,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0049,0.003,-0.12,0.21,-0.00022,0.43,-0.013,-0.0025,-0.0014,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.022,0.027,0.0056,0.2,0.24,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.8e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30590000,0.73,0.0034,0.0071,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0057,0.0026,-0.12,0.21,-0.00024,0.43,-0.012,-0.0025,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.021,0.026,0.0055,0.2,0.24,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30690000,0.73,0.0031,0.0068,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0054,0.0029,-0.12,0.21,-0.00025,0.43,-0.012,-0.0025,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.022,0.028,0.0055,0.21,0.25,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30790000,0.73,0.0031,0.0063,0.68,-1.9,-1,0.82,0,0,-4.9e+02,-0.0012,-0.0059,0.00014,0.0062,0.0023,-0.12,0.21,-0.00025,0.43,-0.012,-0.0026,-0.00077,0,0,-4.9e+02,8.4e-05,6e-05,0.0068,0.021,0.027,0.0055,0.21,0.25,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30890000,0.73,0.0029,0.0059,0.68,-1.9,-1,0.81,0,0,-4.9e+02,-0.0013,-0.0059,0.00015,0.0062,0.0026,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00075,0,0,-4.9e+02,8.5e-05,6e-05,0.0068,0.022,0.029,0.0055,0.22,0.26,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00038,1,1,0.01 +30990000,0.73,0.003,0.0052,0.68,-1.8,-1,0.8,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0071,0.0021,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00041,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.021,0.028,0.0055,0.22,0.26,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.5e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00038,1,1,0.01 +31090000,0.73,0.0027,0.0047,0.68,-1.8,-1,0.79,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0069,0.0025,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00041,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31190000,0.73,0.0026,0.0043,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.5e-05,0.0072,0.0024,-0.12,0.21,-0.00027,0.43,-0.011,-0.0028,-0.00024,0,0,-4.9e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31290000,0.73,0.0023,0.0037,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.8e-05,0.0069,0.0028,-0.11,0.21,-0.00028,0.43,-0.011,-0.0027,-0.00027,0,0,-4.9e+02,8.2e-05,6e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00035,0.00037,1,1,0.01 +31390000,0.73,0.0022,0.003,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.0074,0.0026,-0.11,0.21,-0.00031,0.43,-0.011,-0.0028,-2e-05,0,0,-4.9e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31490000,0.73,0.0019,0.0024,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.0073,0.0032,-0.11,0.2,-0.0003,0.43,-0.011,-0.0029,1.8e-05,0,0,-4.9e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31590000,0.73,0.002,0.0019,0.68,-1.7,-0.97,0.77,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.0082,0.0028,-0.11,0.2,-0.00029,0.43,-0.01,-0.0029,0.00026,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31690000,0.73,0.0017,0.0012,0.68,-1.6,-0.97,0.78,0,0,-4.9e+02,-0.0013,-0.0058,4.7e-05,0.0079,0.0031,-0.11,0.2,-0.0003,0.43,-0.01,-0.0029,0.00022,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31790000,0.73,0.0015,0.0004,0.69,-1.6,-0.95,0.78,0,0,-4.9e+02,-0.0013,-0.0058,2.2e-05,0.0089,0.003,-0.11,0.2,-0.0003,0.43,-0.0098,-0.0029,0.00056,0,0,-4.9e+02,7.6e-05,5.9e-05,0.0062,0.021,0.029,0.0054,0.26,0.3,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +31890000,0.73,0.0013,-0.00032,0.69,-1.6,-0.95,0.78,0,0,-4.9e+02,-0.0013,-0.0058,2.3e-05,0.0088,0.0035,-0.11,0.2,-0.0003,0.43,-0.0099,-0.0029,0.00059,0,0,-4.9e+02,7.6e-05,5.9e-05,0.0062,0.022,0.031,0.0054,0.27,0.31,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +31990000,0.73,0.0012,-0.00093,0.69,-1.6,-0.93,0.77,0,0,-4.9e+02,-0.0013,-0.0058,-8.8e-06,0.0093,0.0034,-0.11,0.2,-0.0003,0.43,-0.0094,-0.003,0.00076,0,0,-4.9e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32090000,0.73,0.00083,-0.0017,0.69,-1.5,-0.93,0.78,0,0,-4.9e+02,-0.0013,-0.0058,-9.7e-06,0.0091,0.0039,-0.11,0.2,-0.00029,0.43,-0.0094,-0.003,0.00077,0,0,-4.9e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32190000,0.73,0.00063,-0.0026,0.69,-1.5,-0.91,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-4.4e-05,0.0096,0.0039,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.6e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32290000,0.73,0.00035,-0.0034,0.69,-1.5,-0.91,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-4.3e-05,0.0094,0.0045,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00035,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32390000,0.73,0.00026,-0.0041,0.69,-1.5,-0.89,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-6.2e-05,0.0099,0.0044,-0.11,0.2,-0.0003,0.43,-0.0085,-0.0031,0.0012,0,0,-4.9e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32490000,0.72,0.00011,-0.0044,0.69,-1.4,-0.88,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-6e-05,0.0097,0.0048,-0.11,0.2,-0.0003,0.43,-0.0085,-0.0031,0.0011,0,0,-4.9e+02,7.2e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32590000,0.72,0.00016,-0.0047,0.69,-1.4,-0.87,0.78,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.01,0.0048,-0.11,0.2,-0.00031,0.43,-0.0081,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32690000,0.72,0.00012,-0.0048,0.69,-1.4,-0.86,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.01,0.0052,-0.11,0.2,-0.00031,0.43,-0.0081,-0.0031,0.0014,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.022,0.032,0.0053,0.31,0.35,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32790000,0.72,0.00024,-0.0048,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.0001,0.01,0.0052,-0.11,0.2,-0.0003,0.43,-0.0077,-0.0031,0.0015,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0054,0.022,0.03,0.0053,0.3,0.35,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +32890000,0.72,0.00031,-0.0048,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.01,0.0058,-0.11,0.2,-0.0003,0.43,-0.0078,-0.0032,0.0016,0,0,-4.9e+02,6.9e-05,5.8e-05,0.0054,0.022,0.031,0.0053,0.32,0.36,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +32990000,0.72,0.00053,-0.0049,0.69,-1.3,-0.82,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.011,0.0059,-0.11,0.2,-0.00031,0.43,-0.0073,-0.0032,0.0017,0,0,-4.9e+02,6.7e-05,5.7e-05,0.0052,0.021,0.029,0.0053,0.31,0.36,0.03,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +33090000,0.72,0.0005,-0.0049,0.69,-1.3,-0.82,0.76,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.011,0.0062,-0.11,0.2,-0.00031,0.43,-0.0073,-0.0032,0.0017,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0053,0.022,0.031,0.0053,0.33,0.37,0.031,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +33190000,0.72,0.004,-0.0041,0.7,-1.2,-0.8,0.7,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.011,0.0061,-0.11,0.2,-0.00031,0.43,-0.0069,-0.0032,0.0017,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.029,0.0053,0.32,0.37,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 +33290000,0.67,0.016,-0.0035,0.74,-1.2,-0.78,0.68,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.011,0.0064,-0.11,0.2,-0.00029,0.43,-0.0071,-0.0033,0.0016,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.031,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.3e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 +33390000,0.56,0.014,-0.0038,0.83,-1.2,-0.77,0.88,0,0,-4.9e+02,-0.0014,-0.0057,-0.00013,0.011,0.0067,-0.11,0.2,-0.00035,0.43,-0.0064,-0.0032,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 +33490000,0.43,0.007,-0.0013,0.9,-1.2,-0.76,0.89,0,0,-4.9e+02,-0.0014,-0.0057,-0.00015,0.011,0.0068,-0.11,0.21,-0.00044,0.43,-0.0059,-0.002,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 +33590000,0.27,0.00087,-0.0037,0.96,-1.2,-0.75,0.86,0,0,-4.9e+02,-0.0014,-0.0057,-0.00018,0.011,0.0068,-0.11,0.21,-0.00068,0.43,-0.0039,-0.0014,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 +33690000,0.098,-0.0027,-0.0067,1,-1.1,-0.74,0.87,0,0,-4.9e+02,-0.0014,-0.0057,-0.00019,0.011,0.0068,-0.11,0.21,-0.00074,0.43,-0.0036,-0.001,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 +33790000,-0.074,-0.0046,-0.0085,1,-1.1,-0.72,0.85,0,0,-4.9e+02,-0.0014,-0.0057,-0.00021,0.011,0.0068,-0.11,0.21,-0.0009,0.43,-0.0021,-0.001,0.0023,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 +33890000,-0.24,-0.006,-0.0091,0.97,-0.99,-0.69,0.83,0,0,-4.9e+02,-0.0014,-0.0057,-0.00022,0.011,0.0068,-0.11,0.21,-0.001,0.43,-0.0013,-0.0011,0.0024,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 +33990000,-0.39,-0.0048,-0.012,0.92,-0.94,-0.64,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.011,0.0069,-0.11,0.21,-0.001,0.43,-0.0011,-0.00064,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 +34090000,-0.5,-0.0039,-0.014,0.87,-0.88,-0.59,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.011,0.0072,-0.11,0.21,-0.00097,0.43,-0.0013,-0.00052,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0014,0.023,0.03,0.0052,0.37,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,3e-05,3.4e-05,0.00036,2.2e-05,2.3e-05,0.00036,1,1,0.01 +34190000,-0.57,-0.0038,-0.012,0.82,-0.85,-0.54,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0086,0.0099,-0.11,0.21,-0.00096,0.43,-0.001,-0.00031,0.0028,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0013,0.023,0.029,0.0052,0.37,0.38,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.5e-05,3.4e-05,0.00036,1.8e-05,1.8e-05,0.00036,1,1,0.01 +34290000,-0.61,-0.0048,-0.0091,0.79,-0.8,-0.48,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0084,0.01,-0.11,0.21,-0.00098,0.43,-0.00093,-0.00018,0.0027,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0012,0.025,0.032,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.2e-05,3.4e-05,0.00036,1.5e-05,1.5e-05,0.00036,1,1,0.01 +34390000,-0.63,-0.0054,-0.0062,0.77,-0.77,-0.44,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0056,0.014,-0.11,0.21,-0.00094,0.43,-0.00092,1.8e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0012,0.025,0.031,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.2e-05,2e-05,3.3e-05,0.00036,1.3e-05,1.3e-05,0.00036,1,1,0.01 +34490000,-0.65,-0.0063,-0.004,0.76,-0.72,-0.39,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0054,0.015,-0.11,0.21,-0.00095,0.43,-0.00086,-2.3e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0011,0.027,0.035,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.1e-05,1.8e-05,3.3e-05,0.00036,1.2e-05,1.2e-05,0.00036,1,1,0.01 +34590000,-0.66,-0.0063,-0.0026,0.75,-0.7,-0.36,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,0.00036,0.021,-0.11,0.21,-0.00091,0.43,-0.00093,3.3e-05,0.0032,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.027,0.034,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,1.1e-05,1e-05,0.00036,1,1,0.01 +34690000,-0.67,-0.0067,-0.0018,0.75,-0.64,-0.32,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,0.00011,0.021,-0.11,0.21,-0.00093,0.43,-0.00079,0.00023,0.0031,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.03,0.037,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,9.9e-06,9.4e-06,0.00036,1,1,0.01 +34790000,-0.67,-0.0061,-0.0013,0.74,-0.63,-0.3,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.0059,0.028,-0.11,0.21,-0.00092,0.43,-0.00063,0.00033,0.0033,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.029,0.036,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,9.1e-06,8.6e-06,0.00036,1,1,0.01 +34890000,-0.67,-0.0061,-0.0012,0.74,-0.57,-0.25,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.0061,0.028,-0.11,0.21,-0.00092,0.43,-0.00064,0.00028,0.0033,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.032,0.04,0.0052,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,8.4e-06,7.9e-06,0.00036,1,1,0.01 +34990000,-0.67,-0.013,-0.0037,0.74,0.47,0.36,-0.043,0,0,-4.9e+02,-0.0016,-0.0058,-6.8e-05,-0.014,0.038,-0.11,0.21,-0.00089,0.43,-0.00052,0.00034,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.001,0.034,0.046,0.0054,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.3e-05,3.3e-05,0.00036,8e-06,7.4e-06,0.00036,1,1,0.01 +35090000,-0.67,-0.013,-0.0037,0.74,0.61,0.39,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-6.9e-05,-0.014,0.038,-0.11,0.21,-0.00089,0.43,-0.00048,0.00029,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.037,0.051,0.0055,0.42,0.43,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.5e-06,6.9e-06,0.00036,1,1,0.01 +35190000,-0.67,-0.013,-0.0038,0.74,0.64,0.43,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-7e-05,-0.014,0.038,-0.11,0.21,-0.0009,0.43,-0.00041,0.00033,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.041,0.055,0.0055,0.43,0.44,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.1e-06,6.4e-06,0.00036,1,1,0.01 +35290000,-0.67,-0.013,-0.0039,0.74,0.67,0.48,-0.098,0,0,-4.9e+02,-0.0016,-0.0058,-7.1e-05,-0.014,0.038,-0.11,0.21,-0.00092,0.43,-0.00034,0.00033,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.044,0.06,0.0055,0.44,0.45,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.8e-06,6.1e-06,0.00036,1,1,0.01 +35390000,-0.67,-0.013,-0.0038,0.74,0.7,0.52,-0.096,0,0,-4.9e+02,-0.0016,-0.0058,-7.4e-05,-0.014,0.038,-0.11,0.21,-0.00093,0.43,-0.00024,0.0003,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.048,0.064,0.0055,0.46,0.47,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.5e-06,5.8e-06,0.00036,1,1,0.01 +35490000,-0.67,-0.013,-0.0039,0.74,0.73,0.56,-0.094,0,0,-4.9e+02,-0.0016,-0.0058,-7.7e-05,-0.014,0.038,-0.11,0.21,-0.00094,0.43,-0.00015,0.00025,0.0037,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.053,0.069,0.0055,0.47,0.48,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.2e-06,5.5e-06,0.00036,1,1,0.01 +35590000,-0.67,-0.013,-0.0039,0.74,0.76,0.6,-0.093,0,0,-4.9e+02,-0.0016,-0.0058,-7.6e-05,-0.014,0.038,-0.11,0.21,-0.00094,0.43,-0.00017,0.00025,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.057,0.075,0.0055,0.49,0.5,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6e-06,5.2e-06,0.00036,1,1,0.01 +35690000,-0.68,-0.013,-0.0038,0.74,0.79,0.65,-0.091,0,0,-4.9e+02,-0.0016,-0.0058,-7.7e-05,-0.014,0.038,-0.11,0.21,-0.00095,0.43,-9.9e-05,0.00024,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.062,0.08,0.0056,0.51,0.52,0.031,2.4e-07,2.6e-07,7.4e-07,0.003,0.0032,8e-05,1e-05,3.3e-05,0.00036,5.8e-06,5e-06,0.00036,1,1,0.01 +35790000,-0.68,-0.013,-0.0038,0.74,0.82,0.69,-0.088,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.014,0.038,-0.11,0.21,-0.00095,0.43,-7.4e-05,0.00024,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.067,0.086,0.0056,0.53,0.54,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.6e-06,4.8e-06,0.00036,1,1,0.023 +35890000,-0.68,-0.013,-0.0039,0.74,0.86,0.73,-0.085,0,0,-4.9e+02,-0.0016,-0.0058,-7.9e-05,-0.014,0.038,-0.11,0.21,-0.00096,0.43,-5.9e-05,0.00023,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.072,0.092,0.0056,0.55,0.56,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.4e-06,4.6e-06,0.00036,1,1,0.048 +35990000,-0.68,-0.013,-0.0039,0.74,0.89,0.77,-0.082,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.014,0.038,-0.11,0.21,-0.00095,0.43,-8.4e-05,0.00022,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00097,0.078,0.099,0.0056,0.57,0.59,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,9.9e-06,3.3e-05,0.00036,5.3e-06,4.5e-06,0.00036,1,1,0.073 +36090000,-0.68,-0.013,-0.0039,0.74,0.92,0.81,-0.078,0,0,-4.9e+02,-0.0016,-0.0058,-8e-05,-0.014,0.038,-0.11,0.21,-0.00096,0.43,-4.9e-05,0.0002,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.083,0.11,0.0056,0.6,0.62,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.7e-06,3.3e-05,0.00036,5.2e-06,4.3e-06,0.00036,1,1,0.099 +36190000,-0.68,-0.013,-0.0039,0.74,0.95,0.86,-0.074,0,0,-4.9e+02,-0.0016,-0.0058,-8.4e-05,-0.014,0.038,-0.11,0.21,-0.00097,0.43,4.9e-05,0.00019,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.089,0.11,0.0056,0.63,0.65,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.5e-06,3.3e-05,0.00036,5e-06,4.2e-06,0.00036,1,1,0.12 +36290000,-0.68,-0.013,-0.0038,0.74,0.98,0.9,-0.069,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.014,0.037,-0.11,0.21,-0.00098,0.43,6.9e-05,0.00019,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.096,0.12,0.0056,0.66,0.68,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.4e-06,3.3e-05,0.00036,4.9e-06,4.1e-06,0.00036,1,1,0.15 +36390000,-0.68,-0.013,-0.0038,0.74,1,0.94,-0.066,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.014,0.037,-0.11,0.21,-0.00098,0.43,6.6e-05,0.00022,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.1,0.13,0.0056,0.69,0.72,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.3e-06,3.3e-05,0.00036,4.8e-06,3.9e-06,0.00036,1,1,0.17 +36490000,-0.68,-0.013,-0.0039,0.74,1,0.98,-0.063,0,0,-4.9e+02,-0.0016,-0.0058,-8.3e-05,-0.014,0.037,-0.11,0.21,-0.00097,0.43,3.7e-05,0.00023,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.11,0.13,0.0056,0.72,0.76,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.2e-06,3.3e-05,0.00036,4.7e-06,3.8e-06,0.00036,1,1,0.2 +36590000,-0.68,-0.013,-0.0038,0.74,1.1,1,-0.057,0,0,-4.9e+02,-0.0016,-0.0058,-8.6e-05,-0.013,0.037,-0.11,0.21,-0.00098,0.43,8.5e-05,0.00026,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.14,0.0056,0.76,0.8,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.1e-06,3.3e-05,0.00036,4.6e-06,3.7e-06,0.00036,1,1,0.23 +36690000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.052,0,0,-4.9e+02,-0.0016,-0.0058,-8.8e-05,-0.013,0.037,-0.11,0.21,-0.00099,0.43,0.00012,0.00027,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.15,0.0057,0.8,0.85,0.032,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9e-06,3.3e-05,0.00036,4.6e-06,3.6e-06,0.00036,1,1,0.25 +36790000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.046,0,0,-4.9e+02,-0.0016,-0.0058,-9.2e-05,-0.013,0.037,-0.11,0.21,-0.001,0.43,0.00016,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.13,0.16,0.0057,0.84,0.9,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.9e-06,3.3e-05,0.00036,4.5e-06,3.5e-06,0.00036,1,1,0.28 +36890000,-0.68,-0.013,-0.0038,0.74,1.2,1.2,-0.041,0,0,-4.9e+02,-0.0016,-0.0058,-9.5e-05,-0.013,0.037,-0.11,0.21,-0.001,0.43,0.0002,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0057,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00036,1,1,0.3 +36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00036,1,1,0.33 +37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00036,1,1,0.35 +37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.6e-05,8.6e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.38 +37290000,-0.68,-0.013,-0.0037,0.74,1.3,1.3,-0.019,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00024,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.41 +37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00026,0.00027,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00036,1,1,0.43 +37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0088,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00036,1,1,0.46 +37590000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,-0.0022,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00031,0.00031,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.2,0.23,0.0057,1.3,1.5,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.3e-06,3.3e-05,0.00036,4e-06,3e-06,0.00036,1,1,0.48 +37690000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,0.0051,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00033,0.00029,0.0038,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.21,0.24,0.0057,1.4,1.6,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00036,1,1,0.51 +37790000,-0.68,-0.013,-0.0036,0.74,1.5,1.5,0.012,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.035,-0.11,0.21,-0.001,0.43,0.00034,0.0003,0.0038,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.22,0.26,0.0057,1.5,1.7,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00036,1,1,0.54 +37890000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.018,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.035,-0.11,0.21,-0.001,0.43,0.00034,0.00029,0.0038,0,0,-4.9e+02,4.5e-05,4.5e-05,0.00096,0.23,0.27,0.0056,1.6,1.8,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00036,1,1,0.56 +37990000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.026,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.035,-0.11,0.21,-0.001,0.43,0.00035,0.00029,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.24,0.28,0.0057,1.7,1.9,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00036,1,1,0.59 +38090000,-0.68,-0.014,-0.0036,0.74,1.6,1.7,0.034,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00036,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.25,0.29,0.0056,1.8,2,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00036,1,1,0.61 +38190000,-0.68,-0.013,-0.0036,0.74,1.6,1.7,0.04,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00029,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.26,0.3,0.0056,1.9,2.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00036,1,1,0.64 +38290000,-0.68,-0.014,-0.0036,0.74,1.6,1.8,0.047,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00028,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.27,0.31,0.0056,2.1,2.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00036,1,1,0.67 +38390000,-0.68,-0.014,-0.0035,0.74,1.7,1.8,0.052,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.6e-05,0.00096,0.28,0.33,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00036,1,1,0.69 +38490000,-0.68,-0.014,-0.0035,0.74,1.7,1.8,0.058,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00036,1,1,0.72 +38590000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.064,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00032,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00036,1,1,0.75 +38690000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.069,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00036,1,1,0.77 +38790000,-0.68,-0.014,-0.0034,0.74,1.8,2,0.075,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 +38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.0004,0.00031,0.0038,0,0,-4.9e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index b9bbada6df04..f83bc0ad462c 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -15,337 +15,337 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.1,0,0,-4.9e+02,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00079,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.00038,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.094,0,0,-4.9e+02,-0.00016,-9.3e-05,1.5e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00043,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.11,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,-4.9e+02,0.027,0.027,0.00033,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 -1590000,1,-0.012,-0.014,0.0004,0.031,-0.024,-0.13,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 -1690000,1,-0.012,-0.014,0.00045,0.028,-0.019,-0.13,0,0,-4.9e+02,-0.00073,-0.00074,-3.4e-07,0,0,-0.0018,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 -1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0,0,-4.9e+02,-0.00073,-0.00073,-2.9e-07,0,0,-0.0028,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1590000,1,-0.012,-0.014,0.00039,0.031,-0.024,-0.13,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.012,-0.014,0.00044,0.028,-0.019,-0.13,0,0,-4.9e+02,-0.00073,-0.00074,-3.5e-07,0,0,-0.0018,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0,0,-4.9e+02,-0.00073,-0.00073,-3e-07,0,0,-0.0028,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0,0,-4.9e+02,-0.00072,-0.00072,-2.7e-07,0,0,-0.0032,0,0,0,0,0,0,0,0,-4.9e+02,0.031,0.031,0.00037,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 -1990000,1,-0.011,-0.014,0.00041,0.035,-0.018,-0.14,0,0,-4.9e+02,-0.0011,-0.0013,-3.6e-06,0,0,-0.0046,0,0,0,0,0,0,0,0,-4.9e+02,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0,0,-4.9e+02,-0.0011,-0.0013,-3.7e-06,0,0,-0.0046,0,0,0,0,0,0,0,0,-4.9e+02,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0,0,-4.9e+02,-0.0011,-0.0012,-3.5e-06,0,0,-0.0064,0,0,0,0,0,0,0,0,-4.9e+02,0.027,0.027,0.00032,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 -2190000,1,-0.011,-0.014,0.00039,0.033,-0.014,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.7e-06,0,0,-0.0075,0,0,0,0,0,0,0,0,-4.9e+02,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 -2290000,1,-0.011,-0.014,0.00039,0.038,-0.014,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.5e-06,0,0,-0.0097,0,0,0,0,0,0,0,0,-4.9e+02,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 -2390000,1,-0.011,-0.013,0.0004,0.029,-0.01,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.4e-05,0,0,-0.012,0,0,0,0,0,0,0,0,-4.9e+02,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 -2490000,1,-0.011,-0.014,0.00048,0.033,-0.0091,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.4e-05,0,0,-0.013,0,0,0,0,0,0,0,0,-4.9e+02,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 -2590000,1,-0.01,-0.013,0.0004,0.023,-0.0062,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2e-05,0,0,-0.015,0,0,0,0,0,0,0,0,-4.9e+02,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 -2690000,1,-0.01,-0.013,0.00044,0.027,-0.0055,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,-4.9e+02,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 -2790000,1,-0.01,-0.013,0.00038,0.022,-0.0032,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.5e-05,0,0,-0.022,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 -2890000,1,-0.01,-0.013,0.00031,0.026,-0.005,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.5e-05,0,0,-0.025,0,0,0,0,0,0,0,0,-4.9e+02,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 -2990000,1,-0.01,-0.013,0.00033,0.02,-0.0039,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3e-05,0,0,-0.028,0,0,0,0,0,0,0,0,-4.9e+02,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 -3090000,1,-0.01,-0.013,0.00053,0.025,-0.0068,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3e-05,0,0,-0.031,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 -3190000,1,-0.01,-0.013,0.00058,0.02,-0.0065,-0.15,0,0,-4.9e+02,-0.0021,-0.0036,-3.5e-05,0,0,-0.032,0,0,0,0,0,0,0,0,-4.9e+02,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 -3290000,1,-0.01,-0.013,0.0006,0.023,-0.0067,-0.15,0,0,-4.9e+02,-0.0021,-0.0036,-3.4e-05,0,0,-0.034,0,0,0,0,0,0,0,0,-4.9e+02,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 -3390000,1,-0.0098,-0.013,0.00061,0.019,-0.0036,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.8e-05,0,0,-0.039,0,0,0,0,0,0,0,0,-4.9e+02,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 -3490000,1,-0.0097,-0.013,0.0006,0.025,-0.0023,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.8e-05,0,0,-0.044,0,0,0,0,0,0,0,0,-4.9e+02,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 -3590000,1,-0.0095,-0.012,0.00056,0.021,-0.0018,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.2e-05,0,0,-0.046,0,0,0,0,0,0,0,0,-4.9e+02,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 -3690000,1,-0.0095,-0.013,0.00054,0.024,-0.0011,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.2e-05,0,0,-0.051,0,0,0,0,0,0,0,0,-4.9e+02,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 -3790000,1,-0.0094,-0.012,0.00057,0.019,0.0033,-0.15,0,0,-4.9e+02,-0.0022,-0.0043,-4.7e-05,0,0,-0.054,0,0,0,0,0,0,0,0,-4.9e+02,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 -3890000,1,-0.0094,-0.013,0.00065,0.021,0.0045,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.058,0,0,0,0,0,0,0,0,-4.9e+02,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -3990000,1,-0.0094,-0.013,0.00072,0.026,0.0042,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.063,0,0,0,0,0,0,0,0,-4.9e+02,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -4090000,1,-0.0093,-0.012,0.00078,0.022,0.0037,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.1e-05,0,0,-0.071,0,0,0,0,0,0,0,0,-4.9e+02,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4190000,1,-0.0094,-0.012,0.00075,0.024,0.0034,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.1e-05,0,0,-0.074,0,0,0,0,0,0,0,0,-4.9e+02,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4290000,1,-0.0095,-0.012,0.00076,0.021,0.0033,-0.13,0,0,-4.9e+02,-0.0022,-0.0046,-5.6e-05,0,0,-0.076,0,0,0,0,0,0,0,0,-4.9e+02,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4390000,1,-0.0094,-0.012,0.00072,0.025,0.0018,-0.11,0,0,-4.9e+02,-0.0022,-0.0046,-5.6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,-4.9e+02,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4490000,1,-0.0094,-0.012,0.00078,0.021,0.0036,-0.11,0,0,-4.9e+02,-0.0022,-0.0048,-6.1e-05,0,0,-0.086,0,0,0,0,0,0,0,0,-4.9e+02,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4590000,1,-0.0094,-0.012,0.00085,0.023,0.0024,-0.11,0,0,-4.9e+02,-0.0022,-0.0048,-6.1e-05,0,0,-0.088,0,0,0,0,0,0,0,0,-4.9e+02,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4690000,1,-0.0094,-0.012,0.00079,0.017,0.0027,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,-4.9e+02,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4790000,1,-0.0093,-0.012,0.00089,0.015,0.0048,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,-4.9e+02,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4890000,1,-0.0093,-0.012,0.00093,0.01,0.0024,-0.093,0,0,-4.9e+02,-0.0021,-0.0051,-6.9e-05,0,0,-0.099,0,0,0,0,0,0,0,0,-4.9e+02,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -4990000,1,-0.0092,-0.012,0.00091,0.013,0.0031,-0.085,0,0,-4.9e+02,-0.0021,-0.0051,-6.9e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5090000,1,-0.0091,-0.011,0.00099,0.01,0.0034,-0.083,0,0,-4.9e+02,-0.0021,-0.0052,-7.2e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5190000,1,-0.0089,-0.012,0.001,0.0099,0.007,-0.08,0,0,-4.9e+02,-0.0021,-0.0052,-7.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0071,-0.069,0,0,-4.9e+02,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0,0,-4.9e+02,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.061,0,0,-4.9e+02,-0.002,-0.0054,-7.7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5590000,1,-0.0088,-0.012,0.001,0.0083,0.016,-0.054,0,0,-4.9e+02,-0.002,-0.0054,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5690000,1,-0.0089,-0.011,0.00093,0.0077,0.016,-0.053,0,0,-4.9e+02,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5790000,1,-0.0088,-0.011,0.00088,0.0089,0.018,-0.05,0,0,-4.9e+02,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5890000,1,-0.0088,-0.011,0.00092,0.0095,0.015,-0.048,0,0,-4.9e+02,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5990000,1,-0.0088,-0.012,0.00089,0.011,0.017,-0.042,0,0,-4.9e+02,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -6090000,1,-0.0088,-0.011,0.00071,0.011,0.018,-0.039,0,0,-4.9e+02,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6190000,1,-0.0089,-0.011,0.00072,0.0087,0.017,-0.038,0,0,-4.9e+02,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6290000,1,-0.0089,-0.011,0.00075,0.008,0.019,-0.041,0,0,-4.9e+02,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6390000,1,-0.0089,-0.011,0.00076,0.0082,0.016,-0.042,0,0,-4.9e+02,-0.0018,-0.0056,-8.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6490000,1,-0.0089,-0.011,0.00066,0.0057,0.016,-0.04,0,0,-4.9e+02,-0.0018,-0.0056,-8.7e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6590000,1,-0.0089,-0.011,0.00059,0.0039,0.015,-0.042,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6690000,1,-0.0088,-0.011,0.00055,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6790000,1,-0.0089,-0.011,0.00052,0.0029,0.015,-0.043,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6890000,1,-0.0087,-0.011,0.00043,0.0023,0.015,-0.039,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 -6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 -7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 -7190000,0.98,-0.0065,-0.012,0.18,-6.1e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.7e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 -7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.2e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00094,-0.032,0,0,-4.9e+02,-0.0017,-0.0057,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.8e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7490000,0.98,-0.0063,-0.012,0.18,0.00097,0.0035,-0.026,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.9e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.006,-0.023,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00034,-0.00039,-9.5e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7690000,0.98,-0.0064,-0.013,0.18,0.0021,0.0093,-0.022,0,0,-4.9e+02,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-3.4e-05,0.43,-0.00031,-0.0004,2.4e-06,0,0,-4.9e+02,0.0016,0.0015,0.042,25,25,0.025,52,52,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00017,0.0013,0.0014,0.0016,0.0013,1,1,2 -7790000,0.98,-0.0064,-0.013,0.18,0.0056,0.01,-0.025,0,0,-4.9e+02,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.9e-05,0.43,-0.00021,-0.00039,-1.5e-06,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.024,35,35,0.061,6.3e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00015,0.0013,0.0014,0.0016,0.0013,1,1,2 -7890000,0.98,-0.0065,-0.013,0.18,0.0046,0.014,-0.025,0,0,-4.9e+02,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.6e-05,0.43,-0.00019,-0.0004,4.4e-05,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.023,36,36,0.06,6.3e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00013,0.0013,0.0014,0.0016,0.0013,1,1,2 -7990000,0.98,-0.0063,-0.013,0.18,0.0032,0.017,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-2.5e-05,0.43,-0.0002,-0.00042,7.4e-05,0,0,-4.9e+02,0.0017,0.0016,0.042,24,24,0.022,28,28,0.059,6.2e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00012,0.0013,0.0014,0.0016,0.0013,1,1,2 -8090000,0.98,-0.0063,-0.013,0.18,0.0043,0.019,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-2.2e-05,0.43,-0.00017,-0.00042,9.9e-05,0,0,-4.9e+02,0.0017,0.0017,0.042,24,24,0.022,30,30,0.059,6.2e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,0.00011,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8190000,0.98,-0.0064,-0.012,0.18,0.0052,0.022,-0.018,0,0,-4.9e+02,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.44,-0.00014,-0.00041,0.00014,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.021,24,24,0.058,6.1e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0013,0.0001,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8290000,0.98,-0.0062,-0.012,0.18,0.002,0.028,-0.017,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.43,-0.00014,-0.00044,6.7e-05,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.02,27,27,0.057,6.1e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0013,9.6e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8390000,0.98,-0.0062,-0.012,0.18,-0.0023,0.028,-0.016,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00044,2.1e-05,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.02,23,23,0.057,6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0013,9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8490000,0.98,-0.0059,-0.012,0.18,-0.0061,0.035,-0.017,0,0,-4.9e+02,-0.0017,-0.0059,-9.6e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00048,-7.4e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.019,25,25,0.056,5.9e-05,5.6e-05,2.2e-06,0.04,0.04,0.0011,0.0013,8.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8590000,0.98,-0.006,-0.012,0.18,-0.00035,0.034,-0.012,0,0,-4.9e+02,-0.0016,-0.0057,-9.6e-05,-0.00036,0.00036,-0.14,0.2,-1.7e-05,0.43,-0.00018,-0.00043,3.3e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,19,19,0.018,22,22,0.055,5.8e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0013,7.9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8690000,0.98,-0.006,-0.012,0.18,-0.0022,0.037,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-1.6e-05,0.43,-0.00022,-0.00045,-8.7e-05,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,24,24,0.055,5.8e-05,5.3e-05,2.2e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8790000,0.98,-0.006,-0.012,0.18,-0.005,0.039,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00019,-0.00045,-0.00013,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,27,27,0.055,5.7e-05,5.2e-05,2.2e-06,0.04,0.04,0.00099,0.0013,7.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8890000,0.98,-0.006,-0.012,0.18,0.00053,0.041,-0.0094,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00026,-0.00043,-0.00011,0,0,-4.9e+02,0.002,0.0018,0.041,17,17,0.017,24,24,0.054,5.6e-05,5e-05,2.2e-06,0.04,0.04,0.00094,0.0013,6.7e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -8990000,0.98,-0.0059,-0.013,0.18,0.01,0.047,-0.0086,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00037,-0.00039,-6.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,17,17,0.017,27,27,0.054,5.5e-05,4.9e-05,2.2e-06,0.04,0.04,0.00091,0.0013,6.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -9090000,0.98,-0.0055,-0.012,0.18,-0.00033,0.056,-0.0096,0,0,-4.9e+02,-0.0018,-0.0057,-8.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00028,-0.00049,-8.8e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,24,24,0.053,5.3e-05,4.7e-05,2.2e-06,0.04,0.04,0.00087,0.0013,6.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -9190000,0.98,-0.0053,-0.013,0.18,0.0029,0.064,-0.009,0,0,-4.9e+02,-0.0018,-0.0057,-8.6e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00031,-0.00053,-0.00011,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,27,27,0.052,5.2e-05,4.5e-05,2.2e-06,0.04,0.04,0.00083,0.0013,5.9e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.3 -9290000,0.98,-0.0053,-0.013,0.18,0.013,0.062,-0.0074,0,0,-4.9e+02,-0.0018,-0.0056,-8.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.0004,-0.00049,-8.5e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,24,24,0.052,5.1e-05,4.4e-05,2.2e-06,0.04,0.04,0.00079,0.0013,5.6e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 -9390000,0.98,-0.0054,-0.013,0.18,0.014,0.06,-0.0063,0,0,-4.9e+02,-0.0017,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00035,-0.00043,-3.8e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,26,26,0.052,5e-05,4.2e-05,2.2e-06,0.04,0.04,0.00077,0.0013,5.4e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 -9490000,0.98,-0.0053,-0.013,0.18,0.0087,0.061,-0.0046,0,0,-4.9e+02,-0.0018,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00033,-0.00048,-7.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,12,12,0.015,23,23,0.051,4.8e-05,4e-05,2.2e-06,0.04,0.04,0.00074,0.0013,5.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 -9590000,0.98,-0.0057,-0.013,0.18,0.0078,0.052,-0.0045,0,0,-4.9e+02,-0.0016,-0.0057,-9.3e-05,-0.00036,0.00036,-0.14,0.2,-1e-05,0.43,-0.00031,-0.00041,-0.00012,0,0,-4.9e+02,0.0022,0.0018,0.041,12,12,0.014,26,26,0.05,4.7e-05,3.9e-05,2.2e-06,0.04,0.04,0.00071,0.0013,5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 -9690000,0.98,-0.0059,-0.013,0.18,0.0096,0.047,-0.0016,0,0,-4.9e+02,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-9.2e-06,0.43,-0.00031,-0.00038,-9.9e-05,0,0,-4.9e+02,0.0022,0.0018,0.041,10,10,0.014,23,23,0.05,4.6e-05,3.7e-05,2.2e-06,0.04,0.04,0.00068,0.0013,4.8e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9790000,0.98,-0.0061,-0.012,0.18,0.001,0.041,-0.003,0,0,-4.9e+02,-0.0015,-0.0058,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.7e-06,0.43,-0.00019,-0.00036,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,10,10,0.014,25,25,0.05,4.4e-05,3.5e-05,2.2e-06,0.04,0.04,0.00066,0.0013,4.7e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9890000,0.98,-0.0061,-0.012,0.18,0.0086,0.04,-0.0017,0,0,-4.9e+02,-0.0015,-0.0057,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.6e-06,0.43,-0.00023,-0.00034,-9.4e-05,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,22,22,0.049,4.3e-05,3.4e-05,2.2e-06,0.04,0.04,0.00063,0.0013,4.5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9990000,0.98,-0.0063,-0.012,0.18,0.0025,0.034,-0.001,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-6.4e-06,0.43,-0.00014,-0.00033,-0.00013,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,24,24,0.049,4.2e-05,3.2e-05,2.2e-06,0.04,0.04,0.00061,0.0013,4.4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -10090000,0.98,-0.0067,-0.012,0.18,0.00072,0.019,0.00016,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-4.9e-06,0.43,-9.8e-05,-0.00025,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,7.4,7.5,0.013,21,21,0.048,4e-05,3.1e-05,2.2e-06,0.04,0.04,0.00059,0.0013,4.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10190000,0.98,-0.0072,-0.012,0.18,0.0053,0.0047,0.001,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00036,0.00036,-0.14,0.2,-3.3e-06,0.43,-9.1e-05,-0.00012,-0.00015,0,0,-4.9e+02,0.0022,0.0016,0.041,7.4,7.6,0.012,23,23,0.048,3.9e-05,2.9e-05,2.2e-06,0.04,0.04,0.00057,0.0013,4.1e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10290000,0.98,-0.0071,-0.012,0.18,0.012,0.0085,-3.9e-05,0,0,-4.9e+02,-0.0012,-0.0057,-0.00011,-0.00036,0.00036,-0.14,0.2,-3.7e-06,0.43,-0.00016,-0.00011,-0.00012,0,0,-4.9e+02,0.0022,0.0016,0.041,6.4,6.5,0.012,20,20,0.048,3.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10390000,0.98,-0.0071,-0.012,0.18,0.0076,0.0026,-0.0025,0,0,-4.9e+02,-0.0012,-0.0056,-0.00011,-0.0003,0.00041,-0.14,0.2,-3.9e-06,0.43,-0.0002,-0.00011,-6.5e-05,0,0,-4.9e+02,0.0022,0.0016,0.041,0.25,0.25,0.56,0.5,0.5,0.048,3.6e-05,2.7e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10490000,0.98,-0.0073,-0.012,0.18,0.0096,0.00082,0.007,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.00033,0.00027,-0.14,0.2,-3e-06,0.43,-0.00016,-5.9e-05,-8.7e-05,0,0,-4.9e+02,0.0021,0.0016,0.041,0.25,0.26,0.55,0.51,0.51,0.057,3.5e-05,2.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10590000,0.98,-0.007,-0.012,0.18,0.00031,-0.00018,0.013,0,0,-4.9e+02,-0.0012,-0.0056,-0.00011,-0.0003,0.00025,-0.14,0.2,-3.6e-06,0.43,-0.00018,-9.8e-05,-7.3e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.13,0.27,0.17,0.17,0.055,3.3e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10690000,0.98,-0.0071,-0.013,0.18,0.0031,-0.0028,0.016,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00027,0.00018,-0.14,0.2,-3.3e-06,0.43,-0.00021,-4.9e-05,-7e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.14,0.26,0.17,0.18,0.065,3.2e-05,2.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10790000,0.98,-0.0073,-0.013,0.18,0.0057,-0.006,0.014,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00016,7.3e-05,-0.14,0.2,-3.1e-06,0.43,-0.00026,1.3e-05,-9e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.09,0.095,0.17,0.11,0.11,0.061,3e-05,2.1e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10890000,0.98,-0.0069,-0.013,0.18,0.008,-0.0029,0.01,0,0,-4.9e+02,-0.0012,-0.0055,-0.00011,-5.4e-05,0.0002,-0.14,0.2,-4.1e-06,0.43,-0.00033,-3e-05,-7.2e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.097,0.1,0.16,0.11,0.11,0.068,2.9e-05,2e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -10990000,0.98,-0.0069,-0.013,0.18,0.0054,0.0032,0.016,0,0,-4.9e+02,-0.0012,-0.0056,-0.00011,-0.00021,0.00015,-0.14,0.2,-4.1e-06,0.43,-0.00026,-5.9e-05,-0.00012,0,0,-4.9e+02,0.0019,0.0014,0.041,0.074,0.081,0.12,0.079,0.079,0.065,2.6e-05,1.9e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11090000,0.98,-0.0075,-0.013,0.18,0.0093,0.0017,0.02,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.0003,-5.8e-05,-0.14,0.2,-2.9e-06,0.43,-0.00024,2.5e-05,-0.0001,0,0,-4.9e+02,0.0019,0.0013,0.041,0.082,0.093,0.11,0.084,0.085,0.069,2.6e-05,1.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11190000,0.98,-0.0077,-0.013,0.18,0.0083,0.0023,0.026,0,0,-4.9e+02,-0.001,-0.0056,-0.00012,-0.00041,-1.3e-05,-0.14,0.2,-2.4e-06,0.43,-0.00017,-1.2e-06,-0.00012,0,0,-4.9e+02,0.0017,0.0013,0.04,0.066,0.076,0.084,0.065,0.066,0.066,2.2e-05,1.6e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11290000,0.98,-0.0077,-0.012,0.18,0.0065,-0.0003,0.025,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00052,0.00011,-0.14,0.2,-2.2e-06,0.43,-0.0001,-4e-05,-0.00015,0,0,-4.9e+02,0.0017,0.0012,0.04,0.074,0.088,0.078,0.071,0.072,0.069,2.2e-05,1.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11390000,0.98,-0.0076,-0.012,0.18,0.0045,0.00081,0.016,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00054,1.8e-05,-0.14,0.2,-2.3e-06,0.43,-7.8e-05,-5.1e-05,-0.00018,0,0,-4.9e+02,0.0015,0.0012,0.04,0.062,0.074,0.064,0.058,0.058,0.066,1.9e-05,1.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11490000,0.98,-0.0075,-0.012,0.18,0.0052,-0.00071,0.02,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00052,-6.5e-05,-0.14,0.2,-2.3e-06,0.43,-9.9e-05,-3e-05,-0.00017,0,0,-4.9e+02,0.0015,0.0011,0.04,0.071,0.086,0.058,0.063,0.064,0.067,1.8e-05,1.3e-05,2.2e-06,0.04,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11590000,0.98,-0.0076,-0.012,0.18,0.0047,-0.00064,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00061,2.5e-05,-0.14,0.2,-2.3e-06,0.43,-5e-05,-6.9e-05,-0.00019,0,0,-4.9e+02,0.0014,0.0011,0.04,0.06,0.072,0.049,0.053,0.054,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11690000,0.98,-0.0075,-0.012,0.18,0.0042,0.0018,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00057,0.00015,-0.14,0.2,-2.5e-06,0.43,-4.4e-05,-8.6e-05,-0.00022,0,0,-4.9e+02,0.0014,0.001,0.04,0.068,0.084,0.046,0.059,0.06,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11790000,0.98,-0.0075,-0.012,0.18,0.0028,0.0026,0.019,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-4.6e-05,0.00011,-0.14,0.2,-2.6e-06,0.43,-5.7e-05,-9.1e-05,-0.00022,0,0,-4.9e+02,0.0012,0.00096,0.039,0.058,0.07,0.039,0.05,0.051,0.062,1.3e-05,9.4e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11890000,0.98,-0.0077,-0.012,0.18,0.0039,0.0012,0.017,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00025,0.00027,-0.14,0.2,-2.4e-06,0.43,-2.6e-05,-9.1e-05,-0.00026,0,0,-4.9e+02,0.0012,0.00096,0.039,0.066,0.082,0.036,0.056,0.058,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11990000,0.98,-0.0077,-0.012,0.18,0.0067,0.0034,0.014,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00032,0.00042,-0.14,0.2,-2.7e-06,0.43,-4.4e-05,-9.8e-05,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.057,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -12090000,0.98,-0.0077,-0.012,0.18,0.0099,0.001,0.017,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00051,0.00017,-0.14,0.2,-2.4e-06,0.43,-4.3e-05,-6.6e-05,-0.00025,0,0,-4.9e+02,0.0011,0.00088,0.039,0.064,0.079,0.029,0.055,0.056,0.06,1e-05,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12190000,0.98,-0.0077,-0.012,0.18,0.011,-0.00015,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.00048,-0.14,0.2,-2.3e-06,0.43,-6.9e-06,-3e-05,-0.00028,0,0,-4.9e+02,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12290000,0.98,-0.0078,-0.012,0.18,0.0081,-0.0036,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0015,-0.00031,-0.14,0.2,-2.1e-06,0.43,2e-05,-3.8e-05,-0.00028,0,0,-4.9e+02,0.00095,0.00081,0.039,0.062,0.075,0.024,0.054,0.056,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12390000,0.98,-0.0079,-0.012,0.18,0.008,-0.0049,0.013,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0018,-0.00078,-0.14,0.2,-2e-06,0.43,4.7e-05,-1.7e-05,-0.0003,0,0,-4.9e+02,0.00085,0.00075,0.039,0.053,0.062,0.022,0.047,0.048,0.056,6.9e-06,5.6e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12490000,0.98,-0.008,-0.012,0.18,0.0096,-0.0064,0.017,0,0,-4.9e+02,-0.00099,-0.0058,-0.00013,-0.0021,-0.001,-0.14,0.2,-1.9e-06,0.43,5.5e-05,1.3e-05,-0.00031,0,0,-4.9e+02,0.00085,0.00075,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.8e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12590000,0.98,-0.0081,-0.012,0.18,0.012,-0.01,0.018,0,0,-4.9e+02,-0.00098,-0.0058,-0.00013,-0.0022,-0.0009,-0.14,0.2,-1.8e-06,0.43,6.9e-05,1.3e-05,-0.00032,0,0,-4.9e+02,0.00077,0.0007,0.039,0.051,0.059,0.019,0.046,0.047,0.054,5.7e-06,4.7e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12690000,0.98,-0.008,-0.012,0.18,0.012,-0.014,0.018,0,0,-4.9e+02,-0.00097,-0.0058,-0.00013,-0.0024,-0.00085,-0.14,0.2,-1.8e-06,0.43,7.9e-05,1.7e-05,-0.00033,0,0,-4.9e+02,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.055,0.054,5.7e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.02,0,0,-4.9e+02,-0.00098,-0.0058,-0.00013,-0.0017,-0.0013,-0.14,0.2,-1.8e-06,0.43,4.4e-05,4.1e-05,-0.00031,0,0,-4.9e+02,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4.1e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12890000,0.98,-0.008,-0.012,0.18,0.016,-0.013,0.021,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.0014,-0.0015,-0.14,0.2,-1.9e-06,0.43,2.4e-05,4.5e-05,-0.00029,0,0,-4.9e+02,0.00071,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0013,-0.0016,-0.14,0.2,-2.4e-06,0.43,3.8e-05,2e-05,-0.00032,0,0,-4.9e+02,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00075,-0.0018,-0.14,0.2,-2.4e-06,0.43,1.5e-05,2.6e-05,-0.0003,0,0,-4.9e+02,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13190000,0.98,-0.0079,-0.012,0.18,0.0097,-0.0078,0.017,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00053,-0.0023,-0.14,0.2,-2.6e-06,0.43,3.9e-05,2.4e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00076,-0.0028,-0.14,0.2,-2.4e-06,0.43,4.8e-05,4.4e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.6e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13390000,0.98,-0.0079,-0.012,0.18,0.0091,-0.0081,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0014,-0.0028,-0.14,0.2,-2.7e-06,0.43,8.6e-05,4.2e-05,-0.00035,0,0,-4.9e+02,0.00057,0.00056,0.038,0.042,0.047,0.012,0.045,0.046,0.047,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13490000,0.98,-0.0079,-0.012,0.18,0.011,-0.0068,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0015,-0.0033,-0.14,0.2,-2.6e-06,0.43,8.9e-05,6.4e-05,-0.00035,0,0,-4.9e+02,0.00058,0.00056,0.038,0.047,0.052,0.011,0.052,0.053,0.046,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13590000,0.98,-0.0079,-0.012,0.18,0.014,-0.0071,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.002,-0.0034,-0.14,0.2,-2.6e-06,0.43,9.8e-05,5.7e-05,-0.00035,0,0,-4.9e+02,0.00054,0.00053,0.038,0.04,0.044,0.011,0.045,0.046,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13690000,0.98,-0.0078,-0.012,0.18,0.014,-0.0082,0.015,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0016,-0.0031,-0.14,0.2,-2.6e-06,0.43,8.5e-05,3.7e-05,-0.00033,0,0,-4.9e+02,0.00055,0.00053,0.038,0.044,0.049,0.011,0.052,0.053,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13790000,0.98,-0.0077,-0.012,0.18,0.019,-0.0044,0.015,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0022,-0.0026,-0.14,0.2,-2.8e-06,0.43,8.3e-05,1.6e-05,-0.00034,0,0,-4.9e+02,0.00052,0.00051,0.038,0.038,0.042,0.01,0.045,0.046,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13890000,0.98,-0.0076,-0.012,0.18,0.019,-0.0029,0.016,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0015,-0.002,-0.14,0.2,-3e-06,0.43,6.2e-05,3.3e-07,-0.00034,0,0,-4.9e+02,0.00052,0.00052,0.038,0.042,0.046,0.01,0.051,0.053,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13990000,0.98,-0.0076,-0.012,0.18,0.019,-0.001,0.014,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.001,-0.0025,-0.14,0.2,-2.9e-06,0.43,7e-05,-9.3e-07,-0.00031,0,0,-4.9e+02,0.0005,0.0005,0.038,0.036,0.039,0.0097,0.045,0.046,0.043,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -14090000,0.98,-0.0077,-0.011,0.18,0.017,-0.0082,0.015,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0028,-0.0023,-0.14,0.2,-2.8e-06,0.43,0.00012,7.3e-06,-0.00035,0,0,-4.9e+02,0.0005,0.0005,0.038,0.04,0.043,0.0096,0.051,0.053,0.042,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14190000,0.98,-0.0078,-0.011,0.18,0.016,-0.0076,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0039,-0.0034,-0.14,0.2,-2.6e-06,0.43,0.00018,3.3e-05,-0.00037,0,0,-4.9e+02,0.00048,0.00049,0.038,0.034,0.037,0.0094,0.045,0.046,0.042,2.1e-06,1.9e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14290000,0.98,-0.0077,-0.011,0.18,0.018,-0.0081,0.013,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.004,-0.0037,-0.14,0.2,-2.6e-06,0.43,0.00018,3.6e-05,-0.00036,0,0,-4.9e+02,0.00048,0.00049,0.038,0.038,0.041,0.0092,0.051,0.052,0.041,2.1e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14390000,0.98,-0.0078,-0.011,0.18,0.018,-0.0095,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0038,-0.0044,-0.14,0.2,-2.4e-06,0.43,0.00019,4.1e-05,-0.00035,0,0,-4.9e+02,0.00047,0.00047,0.038,0.033,0.035,0.009,0.045,0.046,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14490000,0.98,-0.008,-0.011,0.18,0.018,-0.011,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0049,-0.0045,-0.14,0.2,-2.3e-06,0.43,0.00022,5e-05,-0.00037,0,0,-4.9e+02,0.00047,0.00047,0.038,0.036,0.038,0.009,0.051,0.052,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14590000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00015,-0.0054,-0.0058,-0.14,0.2,-2.1e-06,0.43,0.00027,6.9e-05,-0.00037,0,0,-4.9e+02,0.00046,0.00046,0.038,0.031,0.033,0.0088,0.045,0.045,0.04,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14690000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0053,-0.0062,-0.14,0.2,-2e-06,0.43,0.00027,7.6e-05,-0.00037,0,0,-4.9e+02,0.00046,0.00046,0.038,0.034,0.036,0.0088,0.05,0.051,0.039,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14790000,0.98,-0.0082,-0.011,0.18,0.019,-0.0034,0.016,0,0,-4.9e+02,-0.0011,-0.0058,-0.00014,-0.0047,-0.0076,-0.14,0.2,-2.2e-06,0.43,0.00027,7.4e-05,-0.00035,0,0,-4.9e+02,0.00044,0.00045,0.038,0.03,0.031,0.0086,0.044,0.045,0.039,1.7e-06,1.5e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14890000,0.98,-0.0082,-0.011,0.18,0.021,-0.0055,0.02,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.0093,-0.14,0.2,-1.8e-06,0.43,0.00027,9.5e-05,-0.00031,0,0,-4.9e+02,0.00045,0.00045,0.038,0.032,0.034,0.0087,0.05,0.051,0.039,1.6e-06,1.5e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -14990000,0.98,-0.0083,-0.011,0.18,0.02,-0.0057,0.022,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0041,-0.011,-0.14,0.2,-1.6e-06,0.43,0.0003,0.00012,-0.00029,0,0,-4.9e+02,0.00044,0.00045,0.038,0.028,0.03,0.0085,0.044,0.045,0.038,1.6e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15090000,0.98,-0.0083,-0.011,0.18,0.021,-0.0049,0.026,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.011,-0.14,0.2,-1.5e-06,0.43,0.0003,0.00013,-0.00029,0,0,-4.9e+02,0.00044,0.00045,0.038,0.031,0.032,0.0085,0.05,0.051,0.038,1.5e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15190000,0.98,-0.0085,-0.011,0.18,0.019,-0.0059,0.027,0,0,-4.9e+02,-0.0011,-0.0058,-0.00014,-0.0048,-0.012,-0.14,0.2,-1.4e-06,0.43,0.00034,0.00014,-0.00029,0,0,-4.9e+02,0.00043,0.00044,0.038,0.027,0.028,0.0085,0.044,0.045,0.038,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15290000,0.98,-0.0086,-0.012,0.18,0.022,-0.0065,0.026,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.2e-06,0.43,0.00033,0.00017,-0.00027,0,0,-4.9e+02,0.00043,0.00044,0.038,0.029,0.031,0.0085,0.049,0.05,0.037,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15390000,0.98,-0.0088,-0.011,0.18,0.021,-0.004,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0045,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,-4.9e+02,0.00042,0.00043,0.038,0.026,0.027,0.0084,0.044,0.044,0.037,1.4e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15490000,0.98,-0.0088,-0.011,0.18,0.022,-0.0067,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0046,-0.014,-0.14,0.2,-1.4e-06,0.43,0.00037,0.00019,-0.00029,0,0,-4.9e+02,0.00042,0.00043,0.038,0.028,0.029,0.0085,0.049,0.05,0.037,1.4e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15590000,0.98,-0.0088,-0.011,0.18,0.023,-0.0081,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00036,0.00019,-0.00028,0,0,-4.9e+02,0.00041,0.00043,0.038,0.024,0.026,0.0084,0.043,0.044,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15690000,0.98,-0.0086,-0.011,0.18,0.024,-0.01,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0032,-0.012,-0.14,0.2,-1.8e-06,0.43,0.00031,0.00013,-0.00026,0,0,-4.9e+02,0.00041,0.00043,0.038,0.026,0.028,0.0085,0.049,0.049,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15790000,0.98,-0.0086,-0.011,0.18,0.019,-0.0094,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0021,-0.012,-0.14,0.2,-2.2e-06,0.43,0.0003,0.00012,-0.00026,0,0,-4.9e+02,0.00041,0.00042,0.038,0.023,0.025,0.0084,0.043,0.044,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15890000,0.98,-0.0087,-0.011,0.18,0.02,-0.0085,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0031,-0.012,-0.14,0.2,-1.9e-06,0.43,0.00034,0.00015,-0.00027,0,0,-4.9e+02,0.00041,0.00042,0.038,0.025,0.027,0.0085,0.048,0.049,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15990000,0.98,-0.0086,-0.011,0.18,0.018,-0.0074,0.022,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0034,-0.014,-0.14,0.2,-2.1e-06,0.43,0.00038,0.00019,-0.00029,0,0,-4.9e+02,0.0004,0.00042,0.038,0.022,0.024,0.0084,0.043,0.043,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -16090000,0.98,-0.0087,-0.011,0.18,0.019,-0.0092,0.02,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.004,-0.014,-0.14,0.2,-2e-06,0.43,0.00041,0.00021,-0.00031,0,0,-4.9e+02,0.0004,0.00042,0.038,0.024,0.025,0.0085,0.048,0.049,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16190000,0.98,-0.0086,-0.011,0.18,0.015,-0.0076,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0047,-0.015,-0.13,0.2,-2e-06,0.43,0.00044,0.00021,-0.00033,0,0,-4.9e+02,0.0004,0.00041,0.038,0.021,0.023,0.0084,0.043,0.043,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16290000,0.98,-0.0087,-0.011,0.18,0.016,-0.009,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0044,-0.014,-0.13,0.2,-2e-06,0.43,0.00043,0.0002,-0.00032,0,0,-4.9e+02,0.0004,0.00041,0.038,0.023,0.024,0.0085,0.047,0.048,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16390000,0.98,-0.0087,-0.011,0.18,0.019,-0.0081,0.018,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.004,-0.017,-0.13,0.2,-1.7e-06,0.43,0.00045,0.00024,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.022,0.0084,0.042,0.043,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16490000,0.98,-0.0089,-0.011,0.18,0.022,-0.01,0.021,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0044,-0.017,-0.13,0.2,-1.5e-06,0.43,0.00046,0.00025,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.022,0.023,0.0085,0.047,0.048,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16590000,0.98,-0.0089,-0.011,0.18,0.025,-0.01,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0043,-0.018,-0.13,0.2,-1.7e-06,0.43,0.00048,0.00025,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.02,0.021,0.0084,0.042,0.043,0.036,1.1e-06,9.7e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16690000,0.98,-0.0089,-0.011,0.18,0.028,-0.015,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0037,-0.017,-0.13,0.2,-1.9e-06,0.43,0.00046,0.00024,-0.0003,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.023,0.0085,0.047,0.047,0.036,1.1e-06,9.6e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16790000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0031,-0.017,-0.13,0.2,-2.2e-06,0.43,0.00046,0.00024,-0.00029,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.042,0.042,0.036,1e-06,9.3e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16890000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0021,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00043,0.00023,-0.00028,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.022,0.0085,0.046,0.047,0.036,1e-06,9.2e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -16990000,0.98,-0.0088,-0.011,0.18,0.026,-0.013,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.0018,-0.018,-0.13,0.2,-2.3e-06,0.43,0.00044,0.00025,-0.00028,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,1e-06,9e-07,2e-06,0.035,0.032,0.00042,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17090000,0.98,-0.0089,-0.011,0.18,0.03,-0.017,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0022,-0.019,-0.13,0.2,-2.4e-06,0.43,0.00048,0.00032,-0.00029,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.021,0.0085,0.046,0.047,0.035,9.9e-07,8.9e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17190000,0.98,-0.009,-0.011,0.18,0.028,-0.021,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0031,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00053,0.00033,-0.00033,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,9.6e-07,8.7e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17290000,0.98,-0.009,-0.011,0.18,0.03,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0038,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00055,0.00034,-0.00035,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.045,0.046,0.036,9.6e-07,8.6e-07,2e-06,0.035,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17390000,0.98,-0.0089,-0.011,0.18,0.025,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0025,-0.018,-0.13,0.2,-3.2e-06,0.43,0.00053,0.00034,-0.00034,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.041,0.041,0.035,9.3e-07,8.4e-07,2e-06,0.034,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17490000,0.98,-0.009,-0.011,0.18,0.024,-0.025,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0028,-0.019,-0.13,0.2,-3.2e-06,0.43,0.00054,0.00034,-0.00035,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.02,0.0085,0.045,0.046,0.036,9.3e-07,8.3e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17590000,0.98,-0.0089,-0.011,0.18,0.021,-0.022,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0024,-0.019,-0.13,0.2,-3.8e-06,0.43,0.00055,0.00034,-0.00035,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.04,0.041,0.035,9e-07,8.1e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17690000,0.98,-0.009,-0.011,0.18,0.022,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0021,-0.019,-0.13,0.2,-3.5e-06,0.43,0.00053,0.00032,-0.00033,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.019,0.0084,0.045,0.045,0.035,9e-07,8e-07,2e-06,0.034,0.031,0.00038,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17790000,0.98,-0.0091,-0.011,0.18,0.022,-0.022,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00013,-0.0013,-0.019,-0.13,0.2,-3.5e-06,0.43,0.00051,0.00029,-0.00031,0,0,-4.9e+02,0.00037,0.00039,0.038,0.016,0.017,0.0084,0.04,0.041,0.036,8.8e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17890000,0.98,-0.0089,-0.011,0.18,0.025,-0.024,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00075,-0.018,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.0003,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.019,0.0084,0.044,0.045,0.036,8.7e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17990000,0.98,-0.0089,-0.011,0.18,0.025,-0.02,0.024,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,4.1e-05,-0.02,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.00029,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0083,0.04,0.041,0.035,8.5e-07,7.6e-07,2e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -18090000,0.98,-0.009,-0.011,0.18,0.026,-0.021,0.024,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,8.7e-05,-0.02,-0.13,0.2,-3.3e-06,0.43,0.00048,0.00027,-0.00028,0,0,-4.9e+02,0.00036,0.00038,0.038,0.017,0.018,0.0083,0.044,0.045,0.036,8.5e-07,7.5e-07,1.9e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18190000,0.98,-0.0091,-0.011,0.18,0.025,-0.019,0.024,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0017,-0.021,-0.13,0.2,-3.6e-06,0.43,0.00045,0.00029,-0.00025,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0082,0.04,0.04,0.035,8.2e-07,7.4e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18290000,0.98,-0.0092,-0.011,0.18,0.028,-0.02,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0018,-0.022,-0.13,0.2,-3.4e-06,0.43,0.00045,0.0003,-0.00024,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.018,0.0082,0.044,0.044,0.036,8.2e-07,7.3e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18390000,0.98,-0.0091,-0.011,0.18,0.027,-0.019,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.002,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00046,0.00031,-0.00025,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18490000,0.98,-0.0091,-0.011,0.18,0.031,-0.019,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0027,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00045,0.00031,-0.00024,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0082,0.043,0.044,0.036,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18590000,0.98,-0.0089,-0.011,0.18,0.028,-0.019,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0029,-0.021,-0.13,0.2,-4.2e-06,0.43,0.00044,0.00028,-0.00024,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,7.8e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18690000,0.98,-0.0088,-0.011,0.18,0.028,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0039,-0.02,-0.13,0.2,-4.2e-06,0.43,0.00041,0.00025,-0.00022,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0081,0.043,0.044,0.035,7.7e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18790000,0.98,-0.0087,-0.011,0.18,0.026,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0043,-0.02,-0.13,0.2,-4.6e-06,0.43,0.00041,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.008,0.039,0.04,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00032,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18890000,0.98,-0.0088,-0.011,0.18,0.026,-0.018,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0041,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00043,0.00028,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.017,0.008,0.043,0.044,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -18990000,0.98,-0.0087,-0.011,0.18,0.023,-0.018,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0049,-0.019,-0.13,0.2,-5.3e-06,0.43,0.00042,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19090000,0.98,-0.0087,-0.011,0.18,0.023,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0054,-0.019,-0.13,0.2,-5.2e-06,0.43,0.00041,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19190000,0.98,-0.0086,-0.011,0.18,0.02,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0052,-0.019,-0.13,0.2,-6e-06,0.43,0.00043,0.00028,-0.00026,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19290000,0.98,-0.0086,-0.011,0.18,0.021,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0052,-0.019,-0.13,0.2,-5.9e-06,0.43,0.00044,0.00029,-0.00026,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.016,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0058,-0.019,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00028,-0.00026,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19490000,0.98,-0.0088,-0.011,0.18,0.02,-0.017,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0052,-0.02,-0.13,0.2,-6e-06,0.43,0.00043,0.00026,-0.00025,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19590000,0.98,-0.0087,-0.011,0.18,0.018,-0.019,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0054,-0.02,-0.13,0.2,-6.2e-06,0.43,0.00043,0.00026,-0.00025,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.03,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19690000,0.98,-0.0088,-0.011,0.18,0.018,-0.017,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0059,-0.021,-0.13,0.2,-5.9e-06,0.43,0.00043,0.00026,-0.00024,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.016,0.0076,0.042,0.043,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19790000,0.98,-0.0089,-0.011,0.18,0.015,-0.016,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0059,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0076,0.038,0.039,0.035,6.6e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19890000,0.98,-0.0089,-0.011,0.18,0.017,-0.016,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0059,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.5e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19990000,0.98,-0.0089,-0.012,0.18,0.015,-0.016,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0068,-0.021,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.038,0.038,0.035,6.4e-07,5.7e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -20090000,0.98,-0.0089,-0.012,0.18,0.017,-0.019,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.007,-0.021,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00028,-0.00026,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.4e-07,5.6e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5.1 -20190000,0.98,-0.0089,-0.012,0.18,0.017,-0.016,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00013,0.007,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00043,0.0003,-0.00027,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.037,0.038,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20290000,0.98,-0.009,-0.012,0.18,0.015,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.007,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00043,0.0003,-0.00028,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20390000,0.98,-0.0089,-0.012,0.18,0.013,-0.016,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.008,-0.022,-0.13,0.2,-7.5e-06,0.43,0.00041,0.00028,-0.00026,0,0,-4.9e+02,0.00034,0.00035,0.038,0.013,0.014,0.0073,0.037,0.038,0.035,6.1e-07,5.4e-07,1.8e-06,0.031,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20490000,0.98,-0.0089,-0.012,0.18,0.0094,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0081,-0.022,-0.13,0.2,-7.3e-06,0.43,0.00041,0.00027,-0.00026,0,0,-4.9e+02,0.00034,0.00035,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6e-07,5.3e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20590000,0.98,-0.0088,-0.012,0.18,0.0087,-0.017,0.02,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0093,-0.022,-0.13,0.2,-7.2e-06,0.43,0.00039,0.00025,-0.00025,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0072,0.037,0.038,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20690000,0.98,-0.0088,-0.012,0.18,0.0076,-0.017,0.021,0,0,-4.9e+02,-0.0013,-0.0058,-0.00012,0.009,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00039,0.00025,-0.00025,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0072,0.041,0.042,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20790000,0.98,-0.0081,-0.012,0.18,0.0037,-0.013,0.006,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0098,-0.022,-0.13,0.2,-7.8e-06,0.43,0.00038,0.00025,-0.00025,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0071,0.037,0.038,0.035,5.8e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20890000,0.98,0.00097,-0.0077,0.18,-0.00018,-0.002,-0.11,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00038,-0.00019,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0071,0.04,0.041,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20990000,0.98,0.0044,-0.0043,0.18,-0.012,0.017,-0.25,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00036,-0.00015,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.007,0.037,0.038,0.034,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21090000,0.98,0.0027,-0.0047,0.18,-0.024,0.032,-0.37,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-6.9e-06,0.43,0.00043,0.00016,-0.00019,0,0,-4.9e+02,0.00033,0.00035,0.038,0.014,0.015,0.007,0.04,0.041,0.035,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21190000,0.98,-6.9e-05,-0.0063,0.18,-0.03,0.039,-0.5,0,0,-4.9e+02,-0.0013,-0.0058,-0.0001,0.0099,-0.024,-0.13,0.2,-6.2e-06,0.43,0.00044,0.00019,-0.00018,0,0,-4.9e+02,0.00033,0.00034,0.038,0.013,0.014,0.0069,0.037,0.038,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21290000,0.98,-0.0023,-0.0076,0.18,-0.029,0.041,-0.63,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0095,-0.024,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00023,-0.00011,0,0,-4.9e+02,0.00033,0.00034,0.037,0.014,0.015,0.0069,0.04,0.041,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21390000,0.98,-0.0037,-0.0082,0.18,-0.026,0.038,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,0.0096,-0.024,-0.13,0.2,-6.3e-06,0.43,0.00044,0.00015,-0.00012,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.014,0.0068,0.037,0.038,0.034,5.3e-07,4.7e-07,1.7e-06,0.031,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21490000,0.98,-0.0045,-0.0086,0.18,-0.021,0.035,-0.89,0,0,-4.9e+02,-0.0013,-0.0058,-0.0001,0.01,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00044,0.00018,-0.00017,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0068,0.04,0.041,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21590000,0.98,-0.0049,-0.0086,0.18,-0.015,0.032,-1,0,0,-4.9e+02,-0.0013,-0.0058,-9.5e-05,0.01,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00042,0.00023,-0.00018,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.015,0.0067,0.037,0.038,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21690000,0.98,-0.0052,-0.0085,0.18,-0.012,0.027,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,-8.7e-05,0.011,-0.024,-0.13,0.2,-6.3e-06,0.43,0.00039,0.00027,-0.00017,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0067,0.04,0.041,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21790000,0.98,-0.0055,-0.0086,0.18,-0.0066,0.022,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-7.8e-05,0.012,-0.023,-0.13,0.2,-6.3e-06,0.43,0.00039,0.00027,-0.00022,0,0,-4.9e+02,0.00032,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21890000,0.98,-0.0058,-0.0088,0.18,-0.003,0.017,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-8.1e-05,0.012,-0.023,-0.13,0.2,-6.7e-06,0.43,0.00037,0.00029,-0.00021,0,0,-4.9e+02,0.00032,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21990000,0.98,-0.0064,-0.009,0.18,-0.00094,0.013,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-7.5e-05,0.012,-0.022,-0.13,0.2,-6.4e-06,0.43,0.0004,0.0003,-0.00023,0,0,-4.9e+02,0.00031,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22090000,0.98,-0.0071,-0.0099,0.18,0.0011,0.0095,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.6e-05,0.012,-0.022,-0.13,0.2,-6.3e-06,0.43,0.00039,0.00032,-0.00024,0,0,-4.9e+02,0.00031,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22190000,0.98,-0.0075,-0.01,0.18,0.0069,0.0051,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.8e-05,0.013,-0.02,-0.13,0.2,-6.5e-06,0.43,0.0004,0.00033,-0.00024,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0065,0.037,0.038,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22290000,0.98,-0.0082,-0.01,0.18,0.013,-0.00055,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6e-05,0.013,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00033,-0.00023,0,0,-4.9e+02,0.00031,0.00032,0.037,0.014,0.015,0.0065,0.04,0.041,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22390000,0.98,-0.0085,-0.01,0.18,0.017,-0.0091,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.2e-05,0.012,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00041,0.00033,-0.00026,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0064,0.037,0.038,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22490000,0.98,-0.0087,-0.011,0.18,0.021,-0.016,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.6e-05,0.011,-0.02,-0.13,0.2,-6.9e-06,0.43,0.00041,0.00035,-0.00028,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22590000,0.98,-0.0086,-0.012,0.18,0.029,-0.024,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.8e-05,0.012,-0.02,-0.13,0.2,-6.4e-06,0.43,0.00042,0.00037,-0.00027,0,0,-4.9e+02,0.0003,0.00032,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22690000,0.98,-0.0085,-0.012,0.18,0.032,-0.028,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6e-05,0.011,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00042,0.00037,-0.00029,0,0,-4.9e+02,0.0003,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22790000,0.98,-0.0085,-0.012,0.18,0.038,-0.036,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.4e-05,0.011,-0.021,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00034,-0.00031,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22890000,0.98,-0.0087,-0.012,0.18,0.042,-0.041,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.5e-05,0.011,-0.021,-0.13,0.2,-6.2e-06,0.43,0.00041,0.00035,-0.00029,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.015,0.0063,0.04,0.041,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22990000,0.98,-0.0086,-0.013,0.18,0.046,-0.046,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.6e-05,0.012,-0.02,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00032,-0.00027,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0062,0.036,0.038,0.033,4.3e-07,3.9e-07,1.6e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23090000,0.98,-0.0086,-0.013,0.18,0.051,-0.05,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.012,-0.02,-0.13,0.2,-6.6e-06,0.43,0.00039,0.00033,-0.00025,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.014,0.0062,0.04,0.041,0.033,4.3e-07,3.9e-07,1.5e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23190000,0.98,-0.0086,-0.013,0.18,0.057,-0.052,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.9e-05,0.012,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00037,0.0003,-0.00025,0,0,-4.9e+02,0.00029,0.00031,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23290000,0.98,-0.0091,-0.014,0.18,0.062,-0.057,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.012,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00034,0.00034,-0.00025,0,0,-4.9e+02,0.00029,0.00031,0.037,0.013,0.014,0.0062,0.039,0.041,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23390000,0.98,-0.009,-0.014,0.18,0.066,-0.06,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.9e-05,0.012,-0.019,-0.13,0.2,-7.7e-06,0.43,0.00035,0.00032,-0.00023,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23490000,0.98,-0.0091,-0.014,0.18,0.072,-0.062,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.012,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00034,0.00037,-0.00028,0,0,-4.9e+02,0.00029,0.0003,0.037,0.013,0.014,0.0061,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23590000,0.98,-0.0093,-0.014,0.18,0.075,-0.064,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.7e-05,0.013,-0.019,-0.13,0.2,-8.4e-06,0.43,0.00029,0.00034,-0.00024,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23690000,0.98,-0.01,-0.015,0.18,0.074,-0.066,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-3.9e-05,0.013,-0.02,-0.13,0.2,-8.2e-06,0.43,0.00027,0.00036,-0.00024,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.014,0.006,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23790000,0.98,-0.012,-0.017,0.18,0.068,-0.062,-0.95,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.014,-0.02,-0.13,0.2,-7.9e-06,0.43,0.00026,0.00039,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.011,0.013,0.006,0.036,0.037,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23890000,0.98,-0.015,-0.021,0.18,0.064,-0.062,-0.52,0,0,-4.9e+02,-0.0013,-0.0058,-3.5e-05,0.014,-0.02,-0.13,0.2,-7.9e-06,0.43,0.00024,0.00042,-0.00024,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.039,0.041,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23990000,0.98,-0.017,-0.024,0.18,0.063,-0.061,-0.13,0,0,-4.9e+02,-0.0013,-0.0058,-4.2e-05,0.015,-0.02,-0.13,0.2,-8e-06,0.43,0.00021,0.00043,3.6e-06,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.033,3.9e-07,3.5e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24090000,0.98,-0.017,-0.023,0.18,0.07,-0.069,0.099,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.015,-0.019,-0.13,0.2,-8e-06,0.43,0.00022,0.0004,3.6e-06,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.9e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24190000,0.98,-0.014,-0.019,0.18,0.08,-0.074,0.089,0,0,-4.9e+02,-0.0013,-0.0058,-4.5e-05,0.017,-0.02,-0.13,0.2,-7.6e-06,0.43,0.00022,0.00037,0.0001,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.032,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24290000,0.98,-0.012,-0.016,0.18,0.084,-0.078,0.067,0,0,-4.9e+02,-0.0013,-0.0058,-4.3e-05,0.017,-0.02,-0.13,0.2,-7.2e-06,0.43,0.00026,0.00032,9.8e-05,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24390000,0.98,-0.011,-0.015,0.18,0.077,-0.073,0.083,0,0,-4.9e+02,-0.0013,-0.0058,-4.1e-05,0.018,-0.02,-0.13,0.2,-5.9e-06,0.43,0.00025,0.00037,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24490000,0.98,-0.011,-0.015,0.18,0.073,-0.069,0.081,0,0,-4.9e+02,-0.0013,-0.0058,-2.8e-05,0.019,-0.021,-0.13,0.2,-5.9e-06,0.43,0.00017,0.00047,0.00021,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24590000,0.98,-0.012,-0.015,0.18,0.069,-0.066,0.077,0,0,-4.9e+02,-0.0013,-0.0058,-3.9e-05,0.02,-0.021,-0.13,0.2,-4.6e-06,0.43,0.00021,0.00044,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24690000,0.98,-0.012,-0.015,0.18,0.067,-0.065,0.076,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.02,-0.021,-0.13,0.2,-4.8e-06,0.43,0.0002,0.00047,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24790000,0.98,-0.012,-0.014,0.18,0.064,-0.063,0.068,0,0,-4.9e+02,-0.0014,-0.0058,-4.5e-05,0.022,-0.022,-0.13,0.2,-4.2e-06,0.43,0.00019,0.00048,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24890000,0.98,-0.012,-0.014,0.18,0.063,-0.067,0.057,0,0,-4.9e+02,-0.0014,-0.0058,-4e-05,0.022,-0.022,-0.13,0.2,-4e-06,0.43,0.00019,0.00049,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24990000,0.98,-0.012,-0.014,0.18,0.055,-0.063,0.05,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.023,-0.023,-0.13,0.2,-4.2e-06,0.43,0.00015,0.00059,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25090000,0.98,-0.012,-0.014,0.18,0.051,-0.063,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.024,-0.023,-0.13,0.2,-4.8e-06,0.43,0.00014,0.00067,0.00029,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25190000,0.98,-0.012,-0.014,0.18,0.045,-0.057,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-7.2e-05,0.025,-0.024,-0.13,0.2,-4.8e-06,0.43,0.00012,0.0007,0.00028,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25290000,0.98,-0.012,-0.013,0.18,0.041,-0.059,0.043,0,0,-4.9e+02,-0.0014,-0.0058,-7.9e-05,0.025,-0.024,-0.13,0.2,-5.4e-06,0.43,0.00011,0.00072,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25390000,0.98,-0.012,-0.013,0.18,0.032,-0.052,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9.4e-05,0.027,-0.025,-0.13,0.2,-6.2e-06,0.43,7.1e-05,0.00078,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25490000,0.98,-0.013,-0.013,0.18,0.028,-0.053,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9.6e-05,0.026,-0.025,-0.13,0.2,-5.9e-06,0.43,6.5e-05,0.00073,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25590000,0.98,-0.013,-0.012,0.18,0.023,-0.049,0.042,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-6.7e-06,0.43,3.4e-05,0.00076,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25690000,0.98,-0.012,-0.012,0.18,0.023,-0.048,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-6.8e-06,0.43,4e-05,0.00079,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25790000,0.98,-0.012,-0.012,0.18,0.011,-0.04,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.029,-0.026,-0.13,0.2,-7.3e-06,0.43,-2.1e-05,0.00072,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25890000,0.98,-0.012,-0.012,0.18,0.0059,-0.039,0.033,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.029,-0.026,-0.13,0.2,-7.5e-06,0.43,-4e-05,0.00069,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25990000,0.98,-0.012,-0.012,0.18,-0.0031,-0.032,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.031,-0.026,-0.13,0.2,-8.9e-06,0.43,-9.5e-05,0.00067,0.00014,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26090000,0.98,-0.012,-0.012,0.18,-0.0077,-0.032,0.025,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.031,-0.027,-0.13,0.2,-8.4e-06,0.43,-9.2e-05,0.00066,0.00017,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.025,0.021,0,0,-4.9e+02,-0.0015,-0.0058,-0.00014,0.032,-0.026,-0.13,0.2,-8.7e-06,0.43,-0.00012,0.00066,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.024,0.015,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,0.032,-0.027,-0.13,0.2,-9.3e-06,0.43,-0.00012,0.00067,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26390000,0.98,-0.011,-0.013,0.18,-0.021,-0.017,0.019,0,0,-4.9e+02,-0.0015,-0.0058,-0.00016,0.033,-0.027,-0.13,0.2,-1e-05,0.43,-0.00016,0.00065,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26490000,0.98,-0.011,-0.013,0.18,-0.023,-0.015,0.028,0,0,-4.9e+02,-0.0015,-0.0058,-0.00016,0.033,-0.028,-0.13,0.2,-1.1e-05,0.43,-0.00017,0.00068,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26590000,0.98,-0.01,-0.013,0.18,-0.026,-0.0064,0.029,0,0,-4.9e+02,-0.0015,-0.0058,-0.00017,0.033,-0.028,-0.13,0.2,-1.2e-05,0.43,-0.00019,0.0007,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26690000,0.98,-0.01,-0.012,0.18,-0.027,-0.0023,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.033,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00021,0.00071,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26790000,0.98,-0.0099,-0.012,0.18,-0.035,0.0019,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00023,0.00069,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26890000,0.98,-0.0092,-0.012,0.18,-0.04,0.0048,0.022,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.2e-05,0.43,-0.00023,0.00067,0.00011,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26990000,0.98,-0.0087,-0.013,0.18,-0.048,0.012,0.021,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00025,0.00065,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27090000,0.98,-0.0086,-0.013,0.18,-0.05,0.018,0.025,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.035,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00025,0.00065,0.00012,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27190000,0.98,-0.0086,-0.013,0.18,-0.056,0.025,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00026,0.00064,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27290000,0.98,-0.0088,-0.014,0.18,-0.063,0.03,0.14,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00027,0.00064,0.00013,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0054,0.038,0.039,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27390000,0.98,-0.01,-0.016,0.18,-0.07,0.037,0.46,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00033,0.00057,0.00018,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27490000,0.98,-0.012,-0.018,0.18,-0.074,0.043,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.13,0.2,-1.6e-05,0.43,-0.0003,0.00053,0.00018,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27590000,0.98,-0.011,-0.017,0.18,-0.069,0.046,0.87,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00048,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27690000,0.98,-0.01,-0.014,0.18,-0.065,0.042,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.5e-05,0.43,-0.00027,0.00047,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27790000,0.98,-0.0088,-0.012,0.18,-0.065,0.041,0.77,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00043,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27890000,0.98,-0.0085,-0.013,0.18,-0.071,0.047,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.035,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00043,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27990000,0.98,-0.0089,-0.013,0.18,-0.072,0.049,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00037,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28090000,0.98,-0.0092,-0.013,0.18,-0.076,0.05,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00039,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28190000,0.98,-0.0086,-0.013,0.18,-0.077,0.047,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.033,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00026,0.00038,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28290000,0.98,-0.0081,-0.013,0.18,-0.081,0.051,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.034,-0.029,-0.12,0.2,-1.5e-05,0.43,-0.00027,0.0004,0.00019,0,0,-4.9e+02,0.00029,0.00029,0.037,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28390000,0.98,-0.0081,-0.014,0.18,-0.082,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.033,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00031,0.00031,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28490000,0.98,-0.0084,-0.015,0.18,-0.084,0.058,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.033,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00028,0.0003,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28590000,0.98,-0.0085,-0.014,0.18,-0.078,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00025,0.00028,0.00022,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28690000,0.98,-0.0082,-0.014,0.18,-0.078,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00022,0.00028,0.00022,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.032,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00025,0.00018,0.00022,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28890000,0.98,-0.0074,-0.013,0.18,-0.08,0.058,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.032,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00025,0.00021,0.0002,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28990000,0.98,-0.0072,-0.013,0.18,-0.077,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.031,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00028,6.7e-05,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29090000,0.98,-0.007,-0.013,0.18,-0.08,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.031,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.0003,4.9e-05,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.03,-0.029,-0.12,0.2,-2e-05,0.43,-0.00033,-8.1e-05,0.00024,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.03,-0.028,-0.12,0.2,-2e-05,0.43,-0.00032,-0.0001,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29390000,0.98,-0.0076,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.029,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00036,-0.00023,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29490000,0.98,-0.0076,-0.013,0.18,-0.08,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,0.029,-0.028,-0.12,0.2,-2.1e-05,0.43,-0.00038,-0.0002,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-8.6e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00043,-0.00031,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-7.9e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00045,-0.00027,0.00021,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-6.3e-05,0.027,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.0005,-0.00039,0.00021,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29890000,0.98,-0.0068,-0.013,0.18,-0.08,0.056,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.7e-05,0.028,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00052,-0.00036,0.00019,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29990000,0.98,-0.007,-0.013,0.18,-0.075,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-4.5e-05,0.026,-0.029,-0.12,0.2,-2.6e-05,0.43,-0.00051,-0.00041,0.00021,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30090000,0.98,-0.0071,-0.013,0.18,-0.077,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.6e-05,0.026,-0.029,-0.12,0.2,-2.7e-05,0.43,-0.00046,-0.00048,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30190000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.8e-05,0.025,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00046,-0.00069,0.00032,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30290000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.7e-05,0.026,-0.029,-0.12,0.2,-2.8e-05,0.43,-0.0005,-0.00074,0.00032,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30390000,0.98,-0.0071,-0.013,0.18,-0.065,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-3.9e-05,0.025,-0.029,-0.12,0.2,-3e-05,0.43,-0.00064,-0.00095,0.00032,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30490000,0.98,-0.0071,-0.013,0.18,-0.068,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-3.3e-05,0.025,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00068,-0.00094,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30590000,0.98,-0.0074,-0.014,0.18,-0.064,0.041,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.8e-05,0.024,-0.03,-0.12,0.2,-3e-05,0.43,-0.00079,-0.001,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30690000,0.98,-0.0078,-0.014,0.18,-0.062,0.04,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.7e-05,0.025,-0.03,-0.12,0.2,-3e-05,0.43,-0.00076,-0.00099,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30790000,0.98,-0.0075,-0.013,0.17,-0.056,0.034,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.3e-05,0.024,-0.031,-0.12,0.2,-3.1e-05,0.43,-0.00084,-0.0012,0.00033,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30890000,0.98,-0.0069,-0.013,0.17,-0.056,0.031,0.79,0,0,-4.9e+02,-0.0014,-0.0058,-2e-05,0.024,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00075,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30990000,0.98,-0.0071,-0.013,0.17,-0.049,0.025,0.8,0,0,-4.9e+02,-0.0014,-0.0057,-1.3e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00078,-0.0012,0.00038,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31090000,0.98,-0.0073,-0.013,0.17,-0.048,0.024,0.79,0,0,-4.9e+02,-0.0014,-0.0057,-1.7e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00073,-0.0012,0.00039,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31190000,0.98,-0.0075,-0.013,0.17,-0.043,0.02,0.8,0,0,-4.9e+02,-0.0013,-0.0057,-1.2e-06,0.023,-0.033,-0.12,0.2,-3.4e-05,0.43,-0.00083,-0.0012,0.00039,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.018,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3.9e-06,0.024,-0.033,-0.12,0.2,-3.3e-05,0.43,-0.00086,-0.0011,0.00037,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3.2e-06,0.023,-0.033,-0.12,0.2,-3.2e-05,0.43,-0.00095,-0.0015,0.00042,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0081,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.5e-06,0.023,-0.033,-0.12,0.2,-3.1e-05,0.43,-0.001,-0.0017,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31590000,0.98,-0.0071,-0.014,0.17,-0.033,0.0062,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.1e-05,0.023,-0.034,-0.12,0.2,-3.1e-05,0.43,-0.0011,-0.0018,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31690000,0.98,-0.0071,-0.014,0.17,-0.036,0.0056,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.9e-05,0.023,-0.033,-0.12,0.2,-3.1e-05,0.43,-0.0012,-0.0018,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31790000,0.98,-0.0074,-0.015,0.17,-0.026,0.003,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.023,-0.034,-0.12,0.2,-3e-05,0.43,-0.0013,-0.0019,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31890000,0.99,-0.0071,-0.015,0.17,-0.025,0.0008,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.8e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.002,0.00047,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31990000,0.99,-0.0074,-0.014,0.17,-0.017,-0.00029,0.79,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.0022,0.00053,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32090000,0.99,-0.0077,-0.014,0.17,-0.019,-0.0037,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.6e-05,0.024,-0.034,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0022,0.00054,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32190000,0.99,-0.008,-0.014,0.17,-0.013,-0.0073,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.1e-05,0.024,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0024,0.00058,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32290000,0.99,-0.0079,-0.014,0.17,-0.013,-0.0099,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.6e-05,0.024,-0.034,-0.12,0.2,-2.8e-05,0.43,-0.0015,-0.0025,0.00059,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32390000,0.99,-0.008,-0.015,0.17,-0.0063,-0.012,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.025,-0.035,-0.12,0.2,-2.6e-05,0.43,-0.0016,-0.0027,0.00065,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32490000,0.99,-0.011,-0.012,0.17,0.018,-0.077,-0.076,0,0,-4.9e+02,-0.0013,-0.0057,1.9e-05,0.025,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0015,-0.0026,0.00064,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32590000,0.99,-0.011,-0.013,0.17,0.021,-0.079,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,1.9e-05,0.025,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32690000,0.99,-0.011,-0.012,0.17,0.016,-0.085,-0.08,0,0,-4.9e+02,-0.0013,-0.0057,1.8e-05,0.025,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32790000,0.99,-0.011,-0.013,0.17,0.017,-0.084,-0.081,0,0,-4.9e+02,-0.0013,-0.0057,1.6e-05,0.025,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0022,0.00078,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0,0,-4.9e+02,-0.0013,-0.0057,2e-05,0.025,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0021,0.00079,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.025,-0.035,-0.12,0.2,-2.3e-05,0.43,-0.0015,-0.002,0.00085,0,0,-4.9e+02,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -33090000,0.98,-0.011,-0.013,0.17,0.013,-0.093,-0.079,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.025,-0.035,-0.12,0.2,-2.3e-05,0.43,-0.0015,-0.002,0.00085,0,0,-4.9e+02,0.00027,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,1.3e-05,0.026,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0015,-0.002,0.00088,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33290000,0.98,-0.01,-0.013,0.17,0.0073,-0.096,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,2.3e-05,0.026,-0.034,-0.12,0.2,-2e-05,0.43,-0.0016,-0.002,0.00092,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33390000,0.98,-0.01,-0.013,0.17,0.0051,-0.09,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,1.6e-05,0.028,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0016,-0.002,0.00096,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 -33490000,0.98,-0.01,-0.013,0.17,-0.00023,-0.093,-0.075,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.028,-0.032,-0.12,0.2,-1.6e-05,0.43,-0.0017,-0.0021,0.00099,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 -33590000,0.98,-0.01,-0.013,0.17,-0.003,-0.092,-0.072,0,0,-4.9e+02,-0.0014,-0.0057,2e-05,0.029,-0.032,-0.12,0.2,-1.3e-05,0.43,-0.0018,-0.0022,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.083 -33690000,0.98,-0.01,-0.013,0.17,-0.0063,-0.095,-0.073,0,0,-4.9e+02,-0.0014,-0.0057,2.4e-05,0.029,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.0018,-0.002,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.11 -33790000,0.98,-0.01,-0.013,0.17,-0.0082,-0.092,-0.068,0,0,-4.9e+02,-0.0014,-0.0057,1.4e-05,0.031,-0.032,-0.12,0.2,-1.2e-05,0.43,-0.0017,-0.0018,0.0011,0,0,-4.9e+02,0.00027,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.13 -33890000,0.98,-0.01,-0.013,0.17,-0.012,-0.093,-0.067,0,0,-4.9e+02,-0.0014,-0.0057,2.4e-05,0.031,-0.032,-0.12,0.2,-1.1e-05,0.43,-0.0018,-0.0018,0.0011,0,0,-4.9e+02,0.00027,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.4e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.16 -33990000,0.98,-0.0099,-0.013,0.17,-0.013,-0.088,-0.064,0,0,-4.9e+02,-0.0014,-0.0057,1.2e-05,0.033,-0.031,-0.12,0.2,-8.4e-06,0.43,-0.0017,-0.0018,0.0011,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.18 -34090000,0.98,-0.0098,-0.013,0.17,-0.017,-0.093,-0.062,0,0,-4.9e+02,-0.0014,-0.0057,1.5e-05,0.033,-0.031,-0.12,0.2,-8.8e-06,0.43,-0.0017,-0.0017,0.0011,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.21 -34190000,0.98,-0.0098,-0.013,0.17,-0.018,-0.088,-0.06,0,0,-4.9e+02,-0.0014,-0.0057,1e-05,0.035,-0.031,-0.12,0.2,-6.4e-06,0.43,-0.0016,-0.0015,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.23 -34290000,0.98,-0.0096,-0.013,0.17,-0.019,-0.092,-0.058,0,0,-4.9e+02,-0.0014,-0.0057,1.7e-05,0.035,-0.031,-0.12,0.2,-5.7e-06,0.43,-0.0017,-0.0016,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.26 -34390000,0.98,-0.0095,-0.013,0.17,-0.021,-0.085,-0.054,0,0,-4.9e+02,-0.0014,-0.0056,9.9e-06,0.036,-0.03,-0.12,0.2,-3.6e-06,0.43,-0.0016,-0.0015,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.28 -34490000,0.98,-0.0095,-0.013,0.17,-0.024,-0.089,-0.052,0,0,-4.9e+02,-0.0014,-0.0056,1.8e-05,0.036,-0.03,-0.12,0.2,-3.3e-06,0.43,-0.0017,-0.0014,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.31 -34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.081,0.74,0,0,-4.9e+02,-0.0014,-0.0056,1.1e-05,0.038,-0.03,-0.12,0.2,-8.1e-07,0.43,-0.0016,-0.0014,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 -34690000,0.98,-0.0094,-0.013,0.17,-0.03,-0.079,1.7,0,0,-4.9e+02,-0.0014,-0.0056,1.4e-05,0.038,-0.03,-0.12,0.2,-5.2e-07,0.43,-0.0017,-0.0014,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 -34790000,0.98,-0.0093,-0.013,0.17,-0.033,-0.07,2.7,0,0,-4.9e+02,-0.0015,-0.0056,7.9e-06,0.04,-0.031,-0.12,0.2,2.1e-06,0.43,-0.0017,-0.0013,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 -34890000,0.98,-0.0092,-0.013,0.17,-0.038,-0.069,3.7,0,0,-4.9e+02,-0.0015,-0.0056,1.1e-05,0.04,-0.031,-0.12,0.2,2.2e-06,0.43,-0.0017,-0.0013,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 +2190000,1,-0.011,-0.014,0.00039,0.033,-0.013,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.8e-06,0,0,-0.0075,0,0,0,0,0,0,0,0,-4.9e+02,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,0.00038,0.038,-0.014,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.6e-06,0,0,-0.0097,0,0,0,0,0,0,0,0,-4.9e+02,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,0.0004,0.029,-0.0098,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.5e-05,0,0,-0.012,0,0,0,0,0,0,0,0,-4.9e+02,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.014,0.00048,0.033,-0.0088,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.5e-05,0,0,-0.013,0,0,0,0,0,0,0,0,-4.9e+02,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.01,-0.013,0.00039,0.023,-0.0059,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2.1e-05,0,0,-0.015,0,0,0,0,0,0,0,0,-4.9e+02,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.01,-0.013,0.00043,0.027,-0.0051,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,-4.9e+02,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.01,-0.013,0.00038,0.022,-0.0029,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.6e-05,0,0,-0.022,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.01,-0.013,0.0003,0.026,-0.0046,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.6e-05,0,0,-0.025,0,0,0,0,0,0,0,0,-4.9e+02,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.01,-0.013,0.00032,0.02,-0.0035,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3.1e-05,0,0,-0.028,0,0,0,0,0,0,0,0,-4.9e+02,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.01,-0.013,0.00052,0.025,-0.0063,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3.1e-05,0,0,-0.031,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.01,-0.013,0.00057,0.02,-0.0061,-0.15,0,0,-4.9e+02,-0.002,-0.0036,-3.5e-05,0,0,-0.032,0,0,0,0,0,0,0,0,-4.9e+02,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.01,-0.013,0.00059,0.023,-0.0062,-0.15,0,0,-4.9e+02,-0.002,-0.0036,-3.5e-05,0,0,-0.034,0,0,0,0,0,0,0,0,-4.9e+02,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.0098,-0.013,0.0006,0.019,-0.0032,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.9e-05,0,0,-0.039,0,0,0,0,0,0,0,0,-4.9e+02,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.0097,-0.013,0.00059,0.025,-0.0018,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.9e-05,0,0,-0.044,0,0,0,0,0,0,0,0,-4.9e+02,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.0095,-0.012,0.00054,0.021,-0.0014,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.3e-05,0,0,-0.046,0,0,0,0,0,0,0,0,-4.9e+02,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.0095,-0.013,0.00053,0.024,-0.00063,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.3e-05,0,0,-0.051,0,0,0,0,0,0,0,0,-4.9e+02,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.0094,-0.012,0.00055,0.019,0.0037,-0.15,0,0,-4.9e+02,-0.0022,-0.0043,-4.8e-05,0,0,-0.054,0,0,0,0,0,0,0,0,-4.9e+02,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.0094,-0.013,0.00064,0.021,0.005,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.058,0,0,0,0,0,0,0,0,-4.9e+02,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.0094,-0.013,0.0007,0.026,0.0048,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.063,0,0,0,0,0,0,0,0,-4.9e+02,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.0093,-0.012,0.00077,0.022,0.0042,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.2e-05,0,0,-0.071,0,0,0,0,0,0,0,0,-4.9e+02,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.0094,-0.012,0.00073,0.024,0.0039,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.2e-05,0,0,-0.074,0,0,0,0,0,0,0,0,-4.9e+02,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.0095,-0.012,0.00075,0.021,0.0037,-0.13,0,0,-4.9e+02,-0.0021,-0.0046,-5.7e-05,0,0,-0.076,0,0,0,0,0,0,0,0,-4.9e+02,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.0094,-0.012,0.0007,0.025,0.0023,-0.11,0,0,-4.9e+02,-0.0021,-0.0046,-5.7e-05,0,0,-0.083,0,0,0,0,0,0,0,0,-4.9e+02,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.0094,-0.012,0.00077,0.021,0.004,-0.11,0,0,-4.9e+02,-0.0021,-0.0048,-6.2e-05,0,0,-0.086,0,0,0,0,0,0,0,0,-4.9e+02,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.0094,-0.012,0.00083,0.023,0.0029,-0.11,0,0,-4.9e+02,-0.0021,-0.0048,-6.2e-05,0,0,-0.088,0,0,0,0,0,0,0,0,-4.9e+02,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.0094,-0.012,0.00077,0.017,0.0031,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.6e-05,0,0,-0.093,0,0,0,0,0,0,0,0,-4.9e+02,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.0093,-0.012,0.00087,0.015,0.0053,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.6e-05,0,0,-0.095,0,0,0,0,0,0,0,0,-4.9e+02,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.0092,-0.012,0.00091,0.01,0.0028,-0.093,0,0,-4.9e+02,-0.0021,-0.0051,-7e-05,0,0,-0.099,0,0,0,0,0,0,0,0,-4.9e+02,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.0092,-0.012,0.00089,0.013,0.0035,-0.085,0,0,-4.9e+02,-0.0021,-0.0051,-7e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.0091,-0.011,0.00097,0.01,0.0038,-0.083,0,0,-4.9e+02,-0.002,-0.0052,-7.3e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.0089,-0.012,0.001,0.0099,0.0074,-0.08,0,0,-4.9e+02,-0.002,-0.0052,-7.3e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0074,-0.069,0,0,-4.9e+02,-0.002,-0.0053,-7.6e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0,0,-4.9e+02,-0.002,-0.0053,-7.6e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.061,0,0,-4.9e+02,-0.002,-0.0054,-7.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0088,-0.012,0.001,0.0083,0.016,-0.054,0,0,-4.9e+02,-0.002,-0.0054,-7.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0089,-0.011,0.0009,0.0077,0.016,-0.053,0,0,-4.9e+02,-0.0019,-0.0054,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0087,-0.011,0.00086,0.0089,0.018,-0.05,0,0,-4.9e+02,-0.0019,-0.0054,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0088,-0.011,0.0009,0.0095,0.016,-0.048,0,0,-4.9e+02,-0.0019,-0.0055,-8.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0088,-0.012,0.00087,0.011,0.017,-0.042,0,0,-4.9e+02,-0.0019,-0.0055,-8.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0087,-0.011,0.00068,0.011,0.018,-0.039,0,0,-4.9e+02,-0.0019,-0.0055,-8.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0089,-0.011,0.00069,0.0087,0.017,-0.038,0,0,-4.9e+02,-0.0018,-0.0055,-8.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0088,-0.011,0.00072,0.008,0.02,-0.041,0,0,-4.9e+02,-0.0018,-0.0055,-8.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0089,-0.011,0.00073,0.0082,0.017,-0.042,0,0,-4.9e+02,-0.0017,-0.0056,-8.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0088,-0.011,0.00063,0.0057,0.016,-0.04,0,0,-4.9e+02,-0.0017,-0.0056,-8.8e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0089,-0.011,0.00056,0.0039,0.015,-0.042,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0088,-0.011,0.00052,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,1,-0.0089,-0.011,0.00049,0.003,0.016,-0.043,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6890000,1,-0.0087,-0.011,0.0004,0.0023,0.016,-0.039,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 +6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 +7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 +7190000,0.98,-0.0065,-0.012,0.18,-6.6e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 +7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00095,-0.032,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.9e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7490000,0.98,-0.0063,-0.012,0.18,0.00098,0.0035,-0.026,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.8e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.0061,-0.023,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00035,-0.00039,-8.8e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7690000,0.98,-0.0064,-0.013,0.18,0.0021,0.0093,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.4e-05,0.43,-0.00031,-0.0004,3.2e-06,0,0,-4.9e+02,0.0016,0.0015,0.042,25,25,0.025,52,52,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00017,0.0013,0.0014,0.0016,0.0013,1,1,2 +7790000,0.98,-0.0064,-0.013,0.18,0.0056,0.01,-0.025,0,0,-4.9e+02,-0.0015,-0.0055,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-2.9e-05,0.43,-0.00021,-0.00039,-8.1e-07,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.024,35,35,0.061,6.3e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00015,0.0013,0.0014,0.0016,0.0013,1,1,2 +7890000,0.98,-0.0064,-0.013,0.18,0.0047,0.014,-0.025,0,0,-4.9e+02,-0.0015,-0.0055,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-2.6e-05,0.43,-0.00019,-0.0004,4.5e-05,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.023,36,36,0.06,6.3e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00013,0.0013,0.0014,0.0016,0.0013,1,1,2 +7990000,0.98,-0.0063,-0.013,0.18,0.0032,0.017,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.3e-05,-0.00036,0.00036,-0.13,0.2,-2.5e-05,0.43,-0.0002,-0.00042,7.5e-05,0,0,-4.9e+02,0.0017,0.0016,0.042,24,24,0.022,28,28,0.059,6.2e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00012,0.0013,0.0014,0.0016,0.0013,1,1,2 +8090000,0.98,-0.0062,-0.013,0.18,0.0043,0.019,-0.022,0,0,-4.9e+02,-0.0015,-0.0056,-9.5e-05,-0.00036,0.00036,-0.13,0.2,-2.2e-05,0.43,-0.00017,-0.00042,0.0001,0,0,-4.9e+02,0.0017,0.0017,0.042,24,24,0.022,30,30,0.059,6.2e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,0.00011,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8190000,0.98,-0.0064,-0.012,0.18,0.0053,0.022,-0.018,0,0,-4.9e+02,-0.0015,-0.0056,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.44,-0.00014,-0.00041,0.00015,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.021,24,24,0.058,6.1e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0013,0.0001,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8290000,0.98,-0.0062,-0.012,0.18,0.002,0.028,-0.017,0,0,-4.9e+02,-0.0015,-0.0058,-9.8e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.43,-0.00014,-0.00044,6.8e-05,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.02,27,27,0.057,6.1e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0013,9.6e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8390000,0.98,-0.0061,-0.012,0.18,-0.0023,0.028,-0.016,0,0,-4.9e+02,-0.0015,-0.0058,-9.9e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00044,2.1e-05,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.02,23,23,0.057,6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0013,9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8490000,0.98,-0.0058,-0.012,0.18,-0.0061,0.035,-0.017,0,0,-4.9e+02,-0.0016,-0.0059,-9.8e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00048,-6.7e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.019,25,25,0.056,5.9e-05,5.6e-05,2.2e-06,0.04,0.04,0.0011,0.0013,8.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8590000,0.98,-0.006,-0.012,0.18,-0.0003,0.034,-0.012,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.14,0.2,-1.7e-05,0.43,-0.00018,-0.00043,3.8e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,19,19,0.018,22,22,0.055,5.8e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0013,7.9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8690000,0.98,-0.0059,-0.012,0.18,-0.0022,0.037,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.14,0.2,-1.6e-05,0.43,-0.00022,-0.00045,-8.7e-05,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,24,24,0.055,5.8e-05,5.3e-05,2.2e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8790000,0.98,-0.0059,-0.012,0.18,-0.005,0.039,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.9e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00019,-0.00045,-0.00013,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,27,27,0.055,5.7e-05,5.2e-05,2.2e-06,0.04,0.04,0.00099,0.0013,7.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8890000,0.98,-0.006,-0.012,0.18,0.00052,0.041,-0.0094,0,0,-4.9e+02,-0.0016,-0.0057,-9.6e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00026,-0.00043,-0.00011,0,0,-4.9e+02,0.002,0.0018,0.041,17,17,0.017,24,24,0.054,5.6e-05,5e-05,2.2e-06,0.04,0.04,0.00094,0.0013,6.7e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +8990000,0.98,-0.0059,-0.013,0.18,0.01,0.047,-0.0086,0,0,-4.9e+02,-0.0016,-0.0056,-9.1e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00037,-0.00039,-6.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,17,17,0.017,27,27,0.054,5.5e-05,4.9e-05,2.2e-06,0.04,0.04,0.00091,0.0013,6.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +9090000,0.98,-0.0055,-0.012,0.18,-0.0004,0.057,-0.0096,0,0,-4.9e+02,-0.0017,-0.0057,-9e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00028,-0.0005,-8.8e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,24,24,0.053,5.3e-05,4.7e-05,2.2e-06,0.04,0.04,0.00087,0.0013,6.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +9190000,0.98,-0.0053,-0.013,0.18,0.0028,0.064,-0.009,0,0,-4.9e+02,-0.0018,-0.0057,-8.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00031,-0.00053,-0.00011,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,27,27,0.052,5.2e-05,4.5e-05,2.2e-06,0.04,0.04,0.00083,0.0013,5.9e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.3 +9290000,0.98,-0.0052,-0.013,0.18,0.013,0.062,-0.0074,0,0,-4.9e+02,-0.0017,-0.0056,-8.6e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.0004,-0.00049,-8.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,24,24,0.052,5.1e-05,4.4e-05,2.2e-06,0.04,0.04,0.00079,0.0013,5.6e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 +9390000,0.98,-0.0054,-0.013,0.18,0.014,0.06,-0.0063,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00035,-0.00044,-3.9e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,26,26,0.052,5e-05,4.2e-05,2.2e-06,0.04,0.04,0.00077,0.0013,5.4e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 +9490000,0.98,-0.0053,-0.013,0.18,0.0085,0.062,-0.0046,0,0,-4.9e+02,-0.0017,-0.0057,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00033,-0.00048,-7.7e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,12,12,0.015,23,23,0.051,4.8e-05,4e-05,2.2e-06,0.04,0.04,0.00074,0.0013,5.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 +9590000,0.98,-0.0057,-0.013,0.18,0.0075,0.053,-0.0045,0,0,-4.9e+02,-0.0016,-0.0057,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-1e-05,0.43,-0.0003,-0.00042,-0.00012,0,0,-4.9e+02,0.0022,0.0018,0.041,12,12,0.014,26,26,0.05,4.7e-05,3.9e-05,2.2e-06,0.04,0.04,0.00071,0.0013,5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 +9690000,0.98,-0.0059,-0.013,0.18,0.0093,0.048,-0.0016,0,0,-4.9e+02,-0.0015,-0.0057,-9.7e-05,-0.00036,0.00036,-0.14,0.2,-9.2e-06,0.43,-0.00031,-0.00038,-0.0001,0,0,-4.9e+02,0.0022,0.0018,0.041,10,10,0.014,23,23,0.05,4.6e-05,3.7e-05,2.2e-06,0.04,0.04,0.00068,0.0013,4.8e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9790000,0.98,-0.0061,-0.012,0.18,0.00062,0.042,-0.003,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.8e-06,0.43,-0.00018,-0.00037,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,10,10,0.014,25,25,0.05,4.4e-05,3.5e-05,2.2e-06,0.04,0.04,0.00066,0.0013,4.7e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9890000,0.98,-0.0061,-0.012,0.18,0.0082,0.041,-0.0017,0,0,-4.9e+02,-0.0014,-0.0057,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.6e-06,0.43,-0.00023,-0.00035,-9.7e-05,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,22,22,0.049,4.3e-05,3.4e-05,2.2e-06,0.04,0.04,0.00063,0.0013,4.5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9990000,0.98,-0.0063,-0.012,0.18,0.002,0.034,-0.001,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-6.5e-06,0.43,-0.00013,-0.00033,-0.00013,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,24,24,0.049,4.2e-05,3.2e-05,2.2e-06,0.04,0.04,0.00061,0.0013,4.4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +10090000,0.98,-0.0067,-0.012,0.18,0.00021,0.019,0.00017,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-4.9e-06,0.43,-9.2e-05,-0.00026,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,7.4,7.5,0.013,21,21,0.048,4e-05,3.1e-05,2.2e-06,0.04,0.04,0.00059,0.0013,4.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10190000,0.98,-0.0072,-0.012,0.18,0.0047,0.0051,0.001,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00036,0.00036,-0.14,0.2,-3.3e-06,0.43,-8.4e-05,-0.00012,-0.00015,0,0,-4.9e+02,0.0022,0.0016,0.041,7.4,7.6,0.012,23,23,0.048,3.9e-05,2.9e-05,2.2e-06,0.04,0.04,0.00057,0.0013,4.1e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10290000,0.98,-0.0071,-0.012,0.18,0.011,0.0088,-3.6e-05,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.00036,0.00036,-0.14,0.2,-3.7e-06,0.43,-0.00015,-0.00011,-0.00012,0,0,-4.9e+02,0.0022,0.0016,0.041,6.4,6.5,0.012,20,20,0.048,3.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10390000,0.98,-0.007,-0.012,0.18,0.0076,0.0026,-0.0025,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.0003,0.00041,-0.14,0.2,-3.9e-06,0.43,-0.00019,-0.00012,-6.9e-05,0,0,-4.9e+02,0.0022,0.0016,0.041,0.25,0.25,0.56,0.5,0.5,0.048,3.6e-05,2.7e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10490000,0.98,-0.0073,-0.012,0.18,0.0096,0.00086,0.007,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00034,0.00027,-0.14,0.2,-3e-06,0.43,-0.00016,-6.3e-05,-9.1e-05,0,0,-4.9e+02,0.0021,0.0016,0.041,0.25,0.26,0.55,0.51,0.51,0.057,3.5e-05,2.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10590000,0.98,-0.007,-0.012,0.18,0.00025,-0.00016,0.013,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.0003,0.00025,-0.14,0.2,-3.6e-06,0.43,-0.00018,-0.0001,-7.7e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.13,0.27,0.17,0.17,0.055,3.3e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10690000,0.98,-0.0071,-0.013,0.18,0.003,-0.0027,0.016,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00027,0.00018,-0.14,0.2,-3.3e-06,0.43,-0.00021,-5.3e-05,-7.4e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.14,0.26,0.17,0.18,0.065,3.2e-05,2.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10790000,0.98,-0.0073,-0.013,0.18,0.0057,-0.006,0.014,0,0,-4.9e+02,-0.001,-0.0056,-0.00011,-0.00016,7.7e-05,-0.14,0.2,-3.2e-06,0.43,-0.00025,8.2e-06,-9.4e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.09,0.095,0.17,0.11,0.11,0.061,3e-05,2.1e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10890000,0.98,-0.0069,-0.013,0.18,0.0078,-0.0029,0.01,0,0,-4.9e+02,-0.0011,-0.0055,-0.00011,-5.6e-05,0.0002,-0.14,0.2,-4.1e-06,0.43,-0.00032,-3.5e-05,-7.6e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.097,0.1,0.16,0.11,0.11,0.068,2.9e-05,2e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +10990000,0.98,-0.0069,-0.013,0.18,0.0053,0.0032,0.016,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00021,0.00016,-0.14,0.2,-4.1e-06,0.43,-0.00025,-6.5e-05,-0.00012,0,0,-4.9e+02,0.0019,0.0014,0.041,0.074,0.081,0.12,0.079,0.079,0.065,2.6e-05,1.9e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11090000,0.98,-0.0075,-0.013,0.18,0.0091,0.0017,0.02,0,0,-4.9e+02,-0.001,-0.0056,-0.00011,-0.00029,-4.9e-05,-0.14,0.2,-2.9e-06,0.43,-0.00023,1.9e-05,-0.00011,0,0,-4.9e+02,0.0019,0.0013,0.041,0.082,0.093,0.11,0.084,0.085,0.069,2.6e-05,1.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11190000,0.98,-0.0077,-0.013,0.18,0.0081,0.0023,0.026,0,0,-4.9e+02,-0.00097,-0.0057,-0.00012,-0.00041,8.1e-07,-0.14,0.2,-2.4e-06,0.43,-0.00016,-7.8e-06,-0.00012,0,0,-4.9e+02,0.0017,0.0013,0.04,0.066,0.076,0.084,0.065,0.066,0.066,2.2e-05,1.6e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11290000,0.98,-0.0077,-0.012,0.18,0.0063,-0.00021,0.025,0,0,-4.9e+02,-0.00097,-0.0057,-0.00012,-0.00051,0.00012,-0.14,0.2,-2.3e-06,0.43,-9.5e-05,-4.7e-05,-0.00016,0,0,-4.9e+02,0.0017,0.0012,0.04,0.074,0.088,0.078,0.071,0.072,0.069,2.2e-05,1.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11390000,0.98,-0.0076,-0.012,0.18,0.0043,0.00089,0.016,0,0,-4.9e+02,-0.00098,-0.0058,-0.00012,-0.00053,3.9e-05,-0.14,0.2,-2.3e-06,0.43,-7.1e-05,-5.8e-05,-0.00018,0,0,-4.9e+02,0.0015,0.0012,0.04,0.062,0.074,0.064,0.058,0.058,0.066,1.9e-05,1.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11490000,0.98,-0.0075,-0.012,0.18,0.005,-0.00059,0.02,0,0,-4.9e+02,-0.00097,-0.0057,-0.00012,-0.00051,-4.3e-05,-0.14,0.2,-2.3e-06,0.43,-9.3e-05,-3.8e-05,-0.00017,0,0,-4.9e+02,0.0015,0.0011,0.04,0.071,0.086,0.058,0.063,0.064,0.067,1.8e-05,1.3e-05,2.2e-06,0.04,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11590000,0.98,-0.0076,-0.012,0.18,0.0045,-0.00053,0.018,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00059,5.4e-05,-0.14,0.2,-2.4e-06,0.43,-4.5e-05,-7.8e-05,-0.0002,0,0,-4.9e+02,0.0014,0.0011,0.04,0.06,0.072,0.049,0.053,0.054,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11690000,0.98,-0.0075,-0.012,0.18,0.0039,0.0019,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00055,0.00017,-0.14,0.2,-2.6e-06,0.43,-3.9e-05,-9.5e-05,-0.00022,0,0,-4.9e+02,0.0014,0.001,0.04,0.068,0.084,0.046,0.059,0.06,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11790000,0.98,-0.0075,-0.012,0.18,0.0026,0.0027,0.019,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-1.2e-05,0.00014,-0.14,0.2,-2.7e-06,0.43,-5.3e-05,-0.0001,-0.00023,0,0,-4.9e+02,0.0012,0.00096,0.039,0.058,0.07,0.039,0.05,0.051,0.062,1.3e-05,9.4e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11890000,0.98,-0.0077,-0.012,0.18,0.0037,0.0014,0.017,0,0,-4.9e+02,-0.001,-0.0059,-0.00012,-0.00021,0.0003,-0.14,0.2,-2.5e-06,0.43,-2.3e-05,-0.0001,-0.00026,0,0,-4.9e+02,0.0012,0.00096,0.039,0.066,0.082,0.036,0.056,0.058,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11990000,0.98,-0.0077,-0.012,0.18,0.0065,0.0036,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00027,0.00047,-0.14,0.2,-2.8e-06,0.43,-4.1e-05,-0.00011,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.057,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +12090000,0.98,-0.0077,-0.012,0.18,0.0096,0.0012,0.017,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00045,0.00022,-0.14,0.2,-2.5e-06,0.43,-4.1e-05,-7.5e-05,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.064,0.079,0.029,0.055,0.056,0.06,1e-05,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12190000,0.98,-0.0077,-0.012,0.18,0.011,-5e-06,0.016,0,0,-4.9e+02,-0.00097,-0.0058,-0.00012,-0.0011,-0.00042,-0.14,0.2,-2.4e-06,0.43,-6.2e-06,-4e-05,-0.00028,0,0,-4.9e+02,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12290000,0.98,-0.0079,-0.012,0.18,0.0079,-0.0035,0.015,0,0,-4.9e+02,-0.00096,-0.0058,-0.00013,-0.0014,-0.00025,-0.14,0.2,-2.2e-06,0.43,2e-05,-4.9e-05,-0.00028,0,0,-4.9e+02,0.00095,0.00081,0.039,0.062,0.075,0.024,0.054,0.056,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12390000,0.98,-0.0079,-0.012,0.18,0.0078,-0.0047,0.013,0,0,-4.9e+02,-0.00095,-0.0058,-0.00013,-0.0017,-0.00072,-0.14,0.2,-2.2e-06,0.43,4.6e-05,-2.8e-05,-0.0003,0,0,-4.9e+02,0.00085,0.00075,0.039,0.053,0.062,0.022,0.047,0.048,0.056,6.9e-06,5.6e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12490000,0.98,-0.008,-0.012,0.18,0.0094,-0.0062,0.017,0,0,-4.9e+02,-0.00093,-0.0058,-0.00013,-0.002,-0.00096,-0.14,0.2,-2.1e-06,0.43,5.3e-05,1.9e-06,-0.00031,0,0,-4.9e+02,0.00085,0.00075,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.8e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12590000,0.98,-0.0081,-0.012,0.18,0.011,-0.01,0.018,0,0,-4.9e+02,-0.00092,-0.0058,-0.00013,-0.0021,-0.00083,-0.14,0.2,-1.9e-06,0.43,6.6e-05,2e-06,-0.00032,0,0,-4.9e+02,0.00077,0.0007,0.039,0.051,0.059,0.019,0.046,0.047,0.054,5.7e-06,4.7e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12690000,0.98,-0.0081,-0.012,0.18,0.012,-0.014,0.018,0,0,-4.9e+02,-0.00091,-0.0058,-0.00013,-0.0023,-0.00078,-0.14,0.2,-1.9e-06,0.43,7.5e-05,5.9e-06,-0.00033,0,0,-4.9e+02,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.055,0.054,5.7e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.02,0,0,-4.9e+02,-0.00093,-0.0058,-0.00013,-0.0016,-0.0012,-0.14,0.2,-2e-06,0.43,3.9e-05,3e-05,-0.0003,0,0,-4.9e+02,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4.1e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12890000,0.98,-0.0081,-0.012,0.18,0.015,-0.013,0.021,0,0,-4.9e+02,-0.00094,-0.0058,-0.00013,-0.0012,-0.0014,-0.14,0.2,-2e-06,0.43,1.9e-05,3.4e-05,-0.00029,0,0,-4.9e+02,0.00071,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0,0,-4.9e+02,-0.00098,-0.0058,-0.00012,-0.0011,-0.0015,-0.14,0.2,-2.5e-06,0.43,3.2e-05,9.2e-06,-0.00031,0,0,-4.9e+02,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00055,-0.0017,-0.14,0.2,-2.6e-06,0.43,8.6e-06,1.4e-05,-0.0003,0,0,-4.9e+02,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13190000,0.98,-0.0079,-0.012,0.18,0.0096,-0.0078,0.017,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00031,-0.0022,-0.14,0.2,-2.7e-06,0.43,3.1e-05,1.2e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00051,-0.0027,-0.14,0.2,-2.6e-06,0.43,3.9e-05,3.2e-05,-0.0003,0,0,-4.9e+02,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.6e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13390000,0.98,-0.0079,-0.012,0.18,0.009,-0.0082,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.0027,-0.14,0.2,-2.8e-06,0.43,7.7e-05,3e-05,-0.00034,0,0,-4.9e+02,0.00057,0.00056,0.038,0.042,0.047,0.012,0.045,0.046,0.047,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13490000,0.98,-0.0079,-0.012,0.18,0.011,-0.0069,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.0032,-0.14,0.2,-2.8e-06,0.43,7.9e-05,5.3e-05,-0.00035,0,0,-4.9e+02,0.00058,0.00056,0.038,0.047,0.052,0.011,0.052,0.053,0.046,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13590000,0.98,-0.0079,-0.012,0.18,0.014,-0.0071,0.015,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.0017,-0.0033,-0.14,0.2,-2.7e-06,0.43,8.6e-05,4.5e-05,-0.00034,0,0,-4.9e+02,0.00054,0.00053,0.038,0.04,0.044,0.011,0.045,0.046,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13690000,0.98,-0.0078,-0.012,0.18,0.014,-0.0083,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0013,-0.003,-0.14,0.2,-2.7e-06,0.43,7.3e-05,2.4e-05,-0.00032,0,0,-4.9e+02,0.00055,0.00053,0.038,0.044,0.049,0.011,0.052,0.053,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13790000,0.98,-0.0078,-0.012,0.18,0.019,-0.0045,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0018,-0.0024,-0.14,0.2,-3e-06,0.43,7.1e-05,2.9e-06,-0.00033,0,0,-4.9e+02,0.00052,0.00051,0.038,0.038,0.042,0.01,0.045,0.046,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13890000,0.98,-0.0076,-0.012,0.18,0.019,-0.0031,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.0019,-0.14,0.2,-3.2e-06,0.43,4.9e-05,-1.2e-05,-0.00033,0,0,-4.9e+02,0.00052,0.00052,0.038,0.042,0.046,0.01,0.051,0.053,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13990000,0.98,-0.0076,-0.012,0.18,0.019,-0.0012,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00062,-0.0024,-0.14,0.2,-3.1e-06,0.43,5.6e-05,-1.5e-05,-0.0003,0,0,-4.9e+02,0.0005,0.0005,0.038,0.036,0.039,0.0097,0.045,0.046,0.043,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +14090000,0.98,-0.0077,-0.011,0.18,0.017,-0.0084,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0024,-0.0022,-0.14,0.2,-3e-06,0.43,0.00011,-6.9e-06,-0.00034,0,0,-4.9e+02,0.0005,0.0005,0.038,0.04,0.043,0.0096,0.051,0.053,0.042,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14190000,0.98,-0.0078,-0.011,0.18,0.016,-0.0078,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0035,-0.0033,-0.14,0.2,-2.8e-06,0.43,0.00016,1.8e-05,-0.00036,0,0,-4.9e+02,0.00048,0.00048,0.038,0.034,0.037,0.0094,0.045,0.046,0.042,2.1e-06,1.9e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14290000,0.98,-0.0078,-0.011,0.18,0.018,-0.0084,0.013,0,0,-4.9e+02,-0.00099,-0.0058,-0.00014,-0.0036,-0.0035,-0.14,0.2,-2.7e-06,0.43,0.00017,2.1e-05,-0.00036,0,0,-4.9e+02,0.00048,0.00049,0.038,0.038,0.041,0.0092,0.051,0.052,0.041,2.1e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14390000,0.98,-0.0078,-0.011,0.18,0.018,-0.0098,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0033,-0.0043,-0.14,0.2,-2.6e-06,0.43,0.00017,2.6e-05,-0.00034,0,0,-4.9e+02,0.00047,0.00047,0.038,0.033,0.035,0.009,0.045,0.046,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14490000,0.98,-0.008,-0.011,0.18,0.018,-0.011,0.018,0,0,-4.9e+02,-0.00098,-0.0058,-0.00014,-0.0044,-0.0044,-0.14,0.2,-2.5e-06,0.43,0.0002,3.4e-05,-0.00036,0,0,-4.9e+02,0.00047,0.00047,0.038,0.036,0.038,0.009,0.051,0.052,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14590000,0.98,-0.0081,-0.011,0.18,0.017,-0.011,0.016,0,0,-4.9e+02,-0.00098,-0.0058,-0.00014,-0.0049,-0.0057,-0.14,0.2,-2.4e-06,0.43,0.00025,5.2e-05,-0.00037,0,0,-4.9e+02,0.00046,0.00046,0.038,0.031,0.033,0.0088,0.045,0.045,0.04,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14690000,0.98,-0.0082,-0.011,0.18,0.017,-0.011,0.016,0,0,-4.9e+02,-0.00098,-0.0058,-0.00014,-0.0048,-0.0061,-0.14,0.2,-2.3e-06,0.43,0.00025,5.9e-05,-0.00036,0,0,-4.9e+02,0.00046,0.00046,0.038,0.034,0.036,0.0088,0.05,0.051,0.039,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14790000,0.98,-0.0082,-0.011,0.18,0.019,-0.0038,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0041,-0.0075,-0.14,0.2,-2.4e-06,0.43,0.00025,5.7e-05,-0.00034,0,0,-4.9e+02,0.00044,0.00045,0.038,0.03,0.031,0.0086,0.044,0.045,0.039,1.7e-06,1.5e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14890000,0.98,-0.0082,-0.011,0.18,0.021,-0.006,0.02,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0036,-0.0091,-0.14,0.2,-2e-06,0.43,0.00025,7.8e-05,-0.0003,0,0,-4.9e+02,0.00045,0.00045,0.038,0.032,0.034,0.0087,0.05,0.051,0.039,1.6e-06,1.5e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +14990000,0.98,-0.0084,-0.011,0.18,0.02,-0.0062,0.022,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0035,-0.011,-0.14,0.2,-1.8e-06,0.43,0.00027,0.00011,-0.00028,0,0,-4.9e+02,0.00044,0.00045,0.038,0.028,0.03,0.0085,0.044,0.045,0.038,1.6e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15090000,0.98,-0.0083,-0.011,0.18,0.021,-0.0055,0.026,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0036,-0.011,-0.14,0.2,-1.8e-06,0.43,0.00028,0.00011,-0.00028,0,0,-4.9e+02,0.00044,0.00045,0.038,0.031,0.032,0.0085,0.05,0.051,0.038,1.5e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15190000,0.98,-0.0085,-0.011,0.18,0.019,-0.0064,0.027,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0041,-0.012,-0.14,0.2,-1.6e-06,0.43,0.00031,0.00012,-0.00028,0,0,-4.9e+02,0.00043,0.00044,0.038,0.027,0.028,0.0085,0.044,0.045,0.038,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15290000,0.98,-0.0087,-0.012,0.18,0.022,-0.0071,0.026,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0035,-0.013,-0.14,0.2,-1.5e-06,0.43,0.00031,0.00015,-0.00026,0,0,-4.9e+02,0.00043,0.00044,0.038,0.029,0.031,0.0085,0.049,0.05,0.037,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15390000,0.98,-0.0088,-0.012,0.18,0.021,-0.0046,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0038,-0.014,-0.14,0.2,-1.6e-06,0.43,0.00034,0.00017,-0.00028,0,0,-4.9e+02,0.00042,0.00043,0.038,0.026,0.027,0.0084,0.044,0.044,0.037,1.4e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15490000,0.98,-0.0088,-0.012,0.18,0.022,-0.0074,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0038,-0.014,-0.14,0.2,-1.6e-06,0.43,0.00034,0.00017,-0.00028,0,0,-4.9e+02,0.00042,0.00043,0.038,0.028,0.029,0.0085,0.049,0.05,0.037,1.4e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15590000,0.98,-0.0089,-0.011,0.18,0.023,-0.0088,0.024,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0034,-0.014,-0.14,0.2,-1.6e-06,0.43,0.00034,0.00017,-0.00027,0,0,-4.9e+02,0.00041,0.00043,0.038,0.024,0.026,0.0084,0.043,0.044,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15690000,0.98,-0.0087,-0.011,0.18,0.024,-0.011,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0023,-0.012,-0.14,0.2,-2e-06,0.43,0.00029,0.00011,-0.00025,0,0,-4.9e+02,0.00041,0.00043,0.038,0.026,0.028,0.0085,0.049,0.049,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15790000,0.98,-0.0086,-0.011,0.18,0.019,-0.01,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00011,-0.0012,-0.011,-0.14,0.2,-2.4e-06,0.43,0.00027,0.0001,-0.00024,0,0,-4.9e+02,0.00041,0.00042,0.038,0.023,0.025,0.0084,0.043,0.044,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15890000,0.98,-0.0088,-0.011,0.18,0.02,-0.0093,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0022,-0.012,-0.14,0.2,-2.2e-06,0.43,0.00031,0.00013,-0.00026,0,0,-4.9e+02,0.00041,0.00042,0.038,0.025,0.027,0.0085,0.048,0.049,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15990000,0.98,-0.0087,-0.011,0.18,0.018,-0.0083,0.022,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0025,-0.013,-0.14,0.2,-2.4e-06,0.43,0.00035,0.00017,-0.00028,0,0,-4.9e+02,0.0004,0.00042,0.038,0.022,0.024,0.0084,0.043,0.043,0.036,1.2e-06,1.1e-06,2.1e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +16090000,0.98,-0.0087,-0.011,0.18,0.019,-0.01,0.02,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0031,-0.014,-0.14,0.2,-2.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,-4.9e+02,0.0004,0.00042,0.038,0.024,0.025,0.0085,0.048,0.049,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16190000,0.98,-0.0087,-0.011,0.18,0.015,-0.0085,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0037,-0.015,-0.13,0.2,-2.3e-06,0.43,0.00041,0.00019,-0.00031,0,0,-4.9e+02,0.0004,0.00041,0.038,0.021,0.023,0.0084,0.043,0.043,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16290000,0.98,-0.0087,-0.011,0.18,0.017,-0.01,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0034,-0.014,-0.13,0.2,-2.3e-06,0.43,0.0004,0.00018,-0.0003,0,0,-4.9e+02,0.0004,0.00041,0.038,0.023,0.024,0.0085,0.047,0.048,0.036,1.2e-06,1e-06,2e-06,0.035,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16390000,0.98,-0.0088,-0.011,0.18,0.019,-0.009,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0029,-0.017,-0.13,0.2,-2.1e-06,0.43,0.00041,0.00022,-0.00028,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.022,0.0084,0.042,0.043,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16490000,0.98,-0.0089,-0.011,0.18,0.022,-0.011,0.021,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0033,-0.017,-0.13,0.2,-1.9e-06,0.43,0.00042,0.00023,-0.00028,0,0,-4.9e+02,0.00039,0.00041,0.038,0.022,0.023,0.0085,0.047,0.048,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16590000,0.98,-0.0089,-0.011,0.18,0.026,-0.011,0.024,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0031,-0.017,-0.13,0.2,-2.1e-06,0.43,0.00044,0.00023,-0.00029,0,0,-4.9e+02,0.00039,0.00041,0.038,0.02,0.021,0.0084,0.042,0.043,0.036,1.1e-06,9.7e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16690000,0.98,-0.0089,-0.011,0.18,0.028,-0.016,0.024,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0025,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00042,0.00022,-0.00028,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.023,0.0085,0.047,0.047,0.036,1.1e-06,9.6e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16790000,0.98,-0.0088,-0.011,0.18,0.028,-0.015,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.002,-0.017,-0.13,0.2,-2.6e-06,0.43,0.00042,0.00022,-0.00028,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.042,0.042,0.036,1e-06,9.3e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16890000,0.98,-0.0087,-0.011,0.18,0.028,-0.015,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.00087,-0.016,-0.13,0.2,-2.7e-06,0.43,0.00039,0.00021,-0.00026,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.022,0.0085,0.046,0.047,0.036,1e-06,9.2e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +16990000,0.98,-0.0088,-0.011,0.18,0.026,-0.015,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.00051,-0.017,-0.13,0.2,-2.7e-06,0.43,0.0004,0.00023,-0.00026,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,1e-06,9e-07,2e-06,0.035,0.032,0.00042,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17090000,0.98,-0.009,-0.011,0.18,0.03,-0.018,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.0009,-0.019,-0.13,0.2,-2.8e-06,0.43,0.00044,0.0003,-0.00027,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.021,0.0085,0.046,0.047,0.035,9.9e-07,8.9e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17190000,0.98,-0.0091,-0.011,0.18,0.028,-0.023,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0018,-0.019,-0.13,0.2,-3.3e-06,0.43,0.00048,0.00031,-0.00031,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,9.6e-07,8.7e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17290000,0.98,-0.0091,-0.011,0.18,0.031,-0.024,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0025,-0.019,-0.13,0.2,-3.3e-06,0.43,0.0005,0.00032,-0.00033,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.045,0.046,0.036,9.6e-07,8.6e-07,2e-06,0.035,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17390000,0.98,-0.009,-0.011,0.18,0.025,-0.025,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0011,-0.018,-0.13,0.2,-3.6e-06,0.43,0.00048,0.00032,-0.00032,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.041,0.041,0.035,9.3e-07,8.4e-07,2e-06,0.034,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17490000,0.98,-0.009,-0.011,0.18,0.024,-0.026,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0014,-0.018,-0.13,0.2,-3.6e-06,0.43,0.00049,0.00032,-0.00032,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.02,0.0085,0.045,0.046,0.036,9.3e-07,8.3e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17590000,0.98,-0.009,-0.011,0.18,0.021,-0.023,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00091,-0.019,-0.13,0.2,-4.2e-06,0.43,0.0005,0.00032,-0.00033,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.04,0.041,0.035,9e-07,8.1e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17690000,0.98,-0.0091,-0.011,0.18,0.022,-0.024,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00059,-0.019,-0.13,0.2,-4e-06,0.43,0.00048,0.0003,-0.00031,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.019,0.0084,0.045,0.045,0.035,9e-07,8e-07,2e-06,0.034,0.031,0.00038,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17790000,0.98,-0.0091,-0.011,0.18,0.023,-0.023,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,0.00019,-0.019,-0.13,0.2,-3.9e-06,0.43,0.00046,0.00027,-0.00029,0,0,-4.9e+02,0.00037,0.00039,0.038,0.016,0.017,0.0084,0.04,0.041,0.036,8.8e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17890000,0.98,-0.009,-0.011,0.18,0.026,-0.025,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,0.0008,-0.018,-0.13,0.2,-3.9e-06,0.43,0.00043,0.00025,-0.00027,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.019,0.0084,0.044,0.045,0.036,8.7e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17990000,0.98,-0.009,-0.011,0.18,0.025,-0.021,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,0.0016,-0.02,-0.13,0.2,-4e-06,0.43,0.00043,0.00025,-0.00026,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0083,0.04,0.041,0.035,8.5e-07,7.6e-07,2e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +18090000,0.98,-0.0091,-0.011,0.18,0.026,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,0.0017,-0.02,-0.13,0.2,-3.8e-06,0.43,0.00042,0.00024,-0.00025,0,0,-4.9e+02,0.00036,0.00038,0.038,0.017,0.018,0.0083,0.044,0.045,0.036,8.5e-07,7.5e-07,1.9e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18190000,0.98,-0.0091,-0.011,0.18,0.025,-0.021,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,0.0033,-0.021,-0.13,0.2,-4.1e-06,0.43,0.0004,0.00027,-0.00023,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0082,0.04,0.04,0.035,8.2e-07,7.4e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18290000,0.98,-0.0092,-0.011,0.18,0.028,-0.022,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.0001,0.0035,-0.022,-0.13,0.2,-3.8e-06,0.43,0.0004,0.00028,-0.00022,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.018,0.0082,0.044,0.044,0.036,8.2e-07,7.3e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18390000,0.98,-0.0091,-0.011,0.18,0.027,-0.02,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,0.0038,-0.022,-0.13,0.2,-4.2e-06,0.43,0.00041,0.00029,-0.00023,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18490000,0.98,-0.0092,-0.011,0.18,0.031,-0.021,0.022,0,0,-4.9e+02,-0.0011,-0.0058,-0.0001,0.0044,-0.022,-0.13,0.2,-4.1e-06,0.43,0.00039,0.00029,-0.00021,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0082,0.043,0.044,0.036,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18590000,0.98,-0.0089,-0.011,0.18,0.028,-0.02,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0047,-0.021,-0.13,0.2,-4.6e-06,0.43,0.00039,0.00026,-0.00022,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,7.8e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18690000,0.98,-0.0088,-0.011,0.18,0.028,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.7e-05,0.0057,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00035,0.00023,-0.0002,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0081,0.043,0.044,0.035,7.7e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18790000,0.98,-0.0088,-0.011,0.18,0.026,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.8e-05,0.0062,-0.02,-0.13,0.2,-5.1e-06,0.43,0.00035,0.00024,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.008,0.039,0.04,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00032,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18890000,0.98,-0.0088,-0.011,0.18,0.027,-0.019,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-9.9e-05,0.006,-0.02,-0.13,0.2,-5.2e-06,0.43,0.00037,0.00026,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.017,0.008,0.043,0.044,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +18990000,0.98,-0.0087,-0.011,0.18,0.024,-0.019,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0068,-0.019,-0.13,0.2,-5.8e-06,0.43,0.00036,0.00025,-0.00022,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19090000,0.98,-0.0088,-0.011,0.18,0.023,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.3e-05,0.0074,-0.019,-0.13,0.2,-5.7e-06,0.43,0.00035,0.00025,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19190000,0.98,-0.0087,-0.011,0.18,0.02,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19290000,0.98,-0.0087,-0.011,0.18,0.021,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.017,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0079,-0.019,-0.13,0.2,-6.9e-06,0.43,0.00036,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19490000,0.98,-0.0089,-0.011,0.18,0.021,-0.019,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0073,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00024,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19590000,0.98,-0.0088,-0.011,0.18,0.018,-0.02,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0076,-0.02,-0.13,0.2,-6.8e-06,0.43,0.00037,0.00024,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19690000,0.98,-0.0088,-0.011,0.18,0.018,-0.019,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0081,-0.021,-0.13,0.2,-6.5e-06,0.43,0.00036,0.00024,-0.00022,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.016,0.0076,0.042,0.043,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19790000,0.98,-0.0089,-0.011,0.18,0.016,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0082,-0.021,-0.13,0.2,-7e-06,0.43,0.00036,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0076,0.038,0.039,0.035,6.6e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19890000,0.98,-0.0089,-0.011,0.18,0.017,-0.018,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0082,-0.021,-0.13,0.2,-7e-06,0.43,0.00036,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.5e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19990000,0.98,-0.0089,-0.012,0.18,0.015,-0.018,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0092,-0.021,-0.13,0.2,-7.5e-06,0.43,0.00035,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.038,0.038,0.035,6.4e-07,5.7e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +20090000,0.98,-0.0089,-0.012,0.18,0.017,-0.021,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0093,-0.021,-0.13,0.2,-7.5e-06,0.43,0.00035,0.00026,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.4e-07,5.6e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5.1 +20190000,0.98,-0.009,-0.012,0.18,0.017,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0094,-0.022,-0.13,0.2,-8e-06,0.43,0.00035,0.00027,-0.00025,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.037,0.038,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20290000,0.98,-0.009,-0.012,0.18,0.015,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0094,-0.022,-0.13,0.2,-8e-06,0.43,0.00036,0.00028,-0.00025,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20390000,0.98,-0.0089,-0.012,0.18,0.013,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.01,-0.022,-0.13,0.2,-8.2e-06,0.43,0.00034,0.00026,-0.00024,0,0,-4.9e+02,0.00034,0.00035,0.038,0.013,0.014,0.0073,0.037,0.038,0.035,6.1e-07,5.4e-07,1.8e-06,0.031,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20490000,0.98,-0.0089,-0.012,0.18,0.0096,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.011,-0.022,-0.13,0.2,-8e-06,0.43,0.00034,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00035,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6e-07,5.3e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20590000,0.98,-0.0089,-0.012,0.18,0.0088,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.012,-0.022,-0.13,0.2,-7.9e-06,0.43,0.00032,0.00023,-0.00022,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0072,0.037,0.038,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20690000,0.98,-0.0088,-0.012,0.18,0.0078,-0.018,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.012,-0.022,-0.13,0.2,-8.1e-06,0.43,0.00032,0.00023,-0.00022,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0072,0.041,0.042,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20790000,0.98,-0.0082,-0.012,0.18,0.0039,-0.015,0.006,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.012,-0.022,-0.13,0.2,-8.5e-06,0.43,0.0003,0.00023,-0.00022,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0071,0.037,0.038,0.035,5.8e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20890000,0.98,0.00095,-0.0079,0.18,1.9e-05,-0.0038,-0.11,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.013,-0.023,-0.13,0.2,-9.4e-06,0.43,0.0003,0.00035,-0.00017,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0071,0.04,0.041,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20990000,0.98,0.0043,-0.0044,0.18,-0.012,0.015,-0.25,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.013,-0.023,-0.13,0.2,-9.4e-06,0.43,0.0003,0.00034,-0.00013,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.007,0.037,0.038,0.034,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21090000,0.98,0.0027,-0.0048,0.18,-0.023,0.031,-0.37,0,0,-4.9e+02,-0.0012,-0.0058,-9.9e-05,0.013,-0.023,-0.13,0.2,-7.6e-06,0.43,0.00035,0.00014,-0.00018,0,0,-4.9e+02,0.00033,0.00035,0.038,0.014,0.015,0.007,0.04,0.041,0.035,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21190000,0.98,-9.1e-05,-0.0064,0.18,-0.029,0.037,-0.5,0,0,-4.9e+02,-0.0012,-0.0058,-9.1e-05,0.013,-0.024,-0.13,0.2,-7e-06,0.43,0.00036,0.00016,-0.00016,0,0,-4.9e+02,0.00033,0.00034,0.038,0.013,0.014,0.0069,0.037,0.038,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21290000,0.98,-0.0023,-0.0077,0.18,-0.028,0.039,-0.63,0,0,-4.9e+02,-0.0012,-0.0058,-9.9e-05,0.012,-0.024,-0.13,0.2,-7.7e-06,0.43,0.00034,0.0002,-9.5e-05,0,0,-4.9e+02,0.00033,0.00034,0.037,0.014,0.015,0.0069,0.04,0.041,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21390000,0.98,-0.0037,-0.0083,0.18,-0.026,0.036,-0.75,0,0,-4.9e+02,-0.0012,-0.0058,-9.7e-05,0.012,-0.024,-0.13,0.2,-7.2e-06,0.43,0.00036,0.00013,-0.00011,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.014,0.0068,0.037,0.038,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21490000,0.98,-0.0045,-0.0088,0.18,-0.021,0.033,-0.89,0,0,-4.9e+02,-0.0012,-0.0058,-9.1e-05,0.013,-0.024,-0.13,0.2,-7.2e-06,0.43,0.00035,0.00015,-0.00016,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0068,0.04,0.041,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21590000,0.98,-0.0049,-0.0088,0.18,-0.014,0.03,-1,0,0,-4.9e+02,-0.0012,-0.0058,-8.5e-05,0.013,-0.024,-0.13,0.2,-7.3e-06,0.43,0.00034,0.00021,-0.00017,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.015,0.0067,0.037,0.038,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21690000,0.98,-0.0052,-0.0086,0.18,-0.011,0.025,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,-7.8e-05,0.014,-0.024,-0.13,0.2,-7.2e-06,0.43,0.00031,0.00025,-0.00016,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0067,0.04,0.041,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21790000,0.98,-0.0055,-0.0088,0.18,-0.0062,0.021,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-6.9e-05,0.015,-0.023,-0.13,0.2,-7.2e-06,0.43,0.00031,0.00024,-0.00021,0,0,-4.9e+02,0.00032,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21890000,0.98,-0.0058,-0.0089,0.18,-0.0025,0.016,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-7.1e-05,0.015,-0.023,-0.13,0.2,-7.6e-06,0.43,0.00028,0.00026,-0.0002,0,0,-4.9e+02,0.00032,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21990000,0.98,-0.0064,-0.0092,0.18,-0.00048,0.011,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.6e-05,0.015,-0.022,-0.13,0.2,-7.3e-06,0.43,0.00031,0.00027,-0.00022,0,0,-4.9e+02,0.00031,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22090000,0.98,-0.0071,-0.01,0.18,0.0015,0.0077,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.7e-05,0.015,-0.022,-0.13,0.2,-7.2e-06,0.43,0.00031,0.00029,-0.00023,0,0,-4.9e+02,0.00031,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22190000,0.98,-0.0075,-0.01,0.18,0.0073,0.0034,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.9e-05,0.016,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00031,0.00031,-0.00023,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0065,0.037,0.038,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22290000,0.98,-0.0082,-0.01,0.18,0.013,-0.0024,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.016,-0.021,-0.13,0.2,-7.3e-06,0.43,0.00031,0.0003,-0.00022,0,0,-4.9e+02,0.00031,0.00032,0.037,0.014,0.015,0.0065,0.04,0.041,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22390000,0.98,-0.0085,-0.011,0.18,0.017,-0.011,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.3e-05,0.015,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00032,0.00031,-0.00024,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0064,0.037,0.038,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22490000,0.98,-0.0087,-0.011,0.18,0.022,-0.018,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.7e-05,0.014,-0.02,-0.13,0.2,-7.8e-06,0.43,0.00033,0.00033,-0.00026,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22590000,0.98,-0.0086,-0.012,0.18,0.029,-0.025,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.9e-05,0.015,-0.02,-0.13,0.2,-7.3e-06,0.43,0.00033,0.00034,-0.00025,0,0,-4.9e+02,0.0003,0.00032,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22690000,0.98,-0.0085,-0.012,0.18,0.032,-0.03,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.015,-0.021,-0.13,0.2,-7.3e-06,0.43,0.00033,0.00035,-0.00027,0,0,-4.9e+02,0.0003,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22790000,0.98,-0.0085,-0.012,0.18,0.038,-0.038,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.5e-05,0.014,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00031,0.00031,-0.00029,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22890000,0.98,-0.0087,-0.013,0.18,0.042,-0.043,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.6e-05,0.015,-0.021,-0.13,0.2,-7.1e-06,0.43,0.00032,0.00032,-0.00028,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.015,0.0063,0.04,0.041,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22990000,0.98,-0.0086,-0.013,0.18,0.046,-0.047,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.8e-05,0.015,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00032,0.0003,-0.00025,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0062,0.036,0.038,0.033,4.3e-07,3.9e-07,1.6e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23090000,0.98,-0.0086,-0.013,0.18,0.051,-0.052,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.6e-05,0.015,-0.02,-0.13,0.2,-7.5e-06,0.43,0.0003,0.0003,-0.00023,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.014,0.0062,0.04,0.041,0.033,4.3e-07,3.9e-07,1.5e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23190000,0.98,-0.0086,-0.014,0.18,0.057,-0.053,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4e-05,0.015,-0.019,-0.13,0.2,-8.3e-06,0.43,0.00028,0.00028,-0.00023,0,0,-4.9e+02,0.00029,0.00031,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23290000,0.98,-0.0091,-0.014,0.18,0.062,-0.059,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.015,-0.02,-0.13,0.2,-8.3e-06,0.43,0.00025,0.00032,-0.00023,0,0,-4.9e+02,0.00029,0.00031,0.037,0.013,0.014,0.0062,0.039,0.041,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23390000,0.98,-0.009,-0.014,0.18,0.067,-0.062,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.016,-0.019,-0.13,0.2,-8.5e-06,0.43,0.00026,0.00029,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23490000,0.98,-0.0091,-0.014,0.18,0.072,-0.064,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.3e-05,0.016,-0.02,-0.13,0.2,-8.3e-06,0.43,0.00024,0.00035,-0.00026,0,0,-4.9e+02,0.00029,0.0003,0.037,0.013,0.014,0.0061,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23590000,0.98,-0.0093,-0.014,0.18,0.075,-0.066,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.9e-05,0.016,-0.019,-0.13,0.2,-9.2e-06,0.43,0.0002,0.00032,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23690000,0.98,-0.01,-0.015,0.18,0.074,-0.068,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-3.1e-05,0.016,-0.019,-0.13,0.2,-9e-06,0.43,0.00018,0.00034,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.014,0.006,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23790000,0.98,-0.012,-0.018,0.18,0.068,-0.064,-0.95,0,0,-4.9e+02,-0.0013,-0.0058,-2.9e-05,0.018,-0.019,-0.13,0.2,-8.6e-06,0.43,0.00016,0.00038,-0.0002,0,0,-4.9e+02,0.00029,0.0003,0.037,0.011,0.013,0.006,0.036,0.037,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23890000,0.98,-0.015,-0.022,0.18,0.064,-0.064,-0.52,0,0,-4.9e+02,-0.0013,-0.0058,-2.7e-05,0.018,-0.02,-0.13,0.2,-8.7e-06,0.43,0.00015,0.0004,-0.00023,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.039,0.041,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23990000,0.98,-0.017,-0.024,0.18,0.063,-0.063,-0.13,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.019,-0.019,-0.13,0.2,-8.7e-06,0.43,0.00011,0.00042,2.4e-05,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.033,3.9e-07,3.5e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24090000,0.98,-0.017,-0.023,0.18,0.07,-0.071,0.099,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.019,-0.019,-0.13,0.2,-8.7e-06,0.43,0.00013,0.00039,2.6e-05,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.9e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24190000,0.98,-0.014,-0.019,0.18,0.08,-0.076,0.089,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.02,-0.019,-0.13,0.2,-8.3e-06,0.43,0.00012,0.00036,0.00013,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.032,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24290000,0.98,-0.012,-0.016,0.18,0.084,-0.08,0.067,0,0,-4.9e+02,-0.0013,-0.0058,-3.5e-05,0.02,-0.019,-0.13,0.2,-7.9e-06,0.43,0.00016,0.00031,0.00012,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24390000,0.98,-0.011,-0.015,0.18,0.077,-0.074,0.083,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.022,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00015,0.00036,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24490000,0.98,-0.011,-0.015,0.18,0.073,-0.071,0.081,0,0,-4.9e+02,-0.0013,-0.0058,-2e-05,0.023,-0.02,-0.13,0.2,-6.5e-06,0.43,7.6e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24590000,0.98,-0.012,-0.015,0.18,0.069,-0.067,0.077,0,0,-4.9e+02,-0.0013,-0.0058,-3.2e-05,0.024,-0.021,-0.13,0.2,-5.2e-06,0.43,0.00012,0.00044,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24690000,0.98,-0.012,-0.015,0.18,0.067,-0.067,0.076,0,0,-4.9e+02,-0.0013,-0.0058,-3e-05,0.024,-0.021,-0.13,0.2,-5.4e-06,0.43,0.0001,0.00047,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24790000,0.98,-0.012,-0.015,0.18,0.064,-0.065,0.068,0,0,-4.9e+02,-0.0013,-0.0058,-3.8e-05,0.026,-0.022,-0.13,0.2,-4.8e-06,0.43,9.9e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24890000,0.98,-0.012,-0.014,0.18,0.063,-0.068,0.057,0,0,-4.9e+02,-0.0013,-0.0058,-3.3e-05,0.026,-0.022,-0.13,0.2,-4.6e-06,0.43,9.6e-05,0.00048,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24990000,0.98,-0.012,-0.014,0.18,0.054,-0.065,0.05,0,0,-4.9e+02,-0.0014,-0.0058,-4.7e-05,0.027,-0.023,-0.13,0.2,-4.8e-06,0.43,6e-05,0.00059,0.00027,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25090000,0.98,-0.012,-0.014,0.18,0.051,-0.064,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-4.7e-05,0.028,-0.023,-0.13,0.2,-5.4e-06,0.43,4.1e-05,0.00066,0.00031,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25190000,0.98,-0.012,-0.014,0.18,0.045,-0.058,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-6.5e-05,0.029,-0.024,-0.13,0.2,-5.4e-06,0.43,1.9e-05,0.00069,0.0003,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25290000,0.98,-0.012,-0.013,0.18,0.041,-0.06,0.043,0,0,-4.9e+02,-0.0014,-0.0058,-7.2e-05,0.029,-0.024,-0.13,0.2,-6e-06,0.43,1e-05,0.00072,0.00027,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25390000,0.98,-0.012,-0.013,0.18,0.032,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-8.8e-05,0.031,-0.025,-0.13,0.2,-6.9e-06,0.43,-3e-05,0.00077,0.00029,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25490000,0.98,-0.013,-0.013,0.18,0.028,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9e-05,0.031,-0.025,-0.13,0.2,-6.6e-06,0.43,-3.6e-05,0.00073,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25590000,0.98,-0.013,-0.013,0.18,0.023,-0.05,0.042,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.032,-0.026,-0.13,0.2,-7.4e-06,0.43,-6.9e-05,0.00075,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25690000,0.98,-0.012,-0.012,0.18,0.023,-0.05,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,0.032,-0.026,-0.13,0.2,-7.5e-06,0.43,-6.4e-05,0.00078,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25790000,0.98,-0.012,-0.012,0.18,0.011,-0.041,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.033,-0.026,-0.13,0.2,-8e-06,0.43,-0.00012,0.00071,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25890000,0.98,-0.012,-0.012,0.18,0.0057,-0.04,0.033,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.033,-0.026,-0.13,0.2,-8.2e-06,0.43,-0.00014,0.00068,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25990000,0.98,-0.012,-0.012,0.18,-0.0033,-0.033,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.035,-0.027,-0.13,0.2,-9.7e-06,0.43,-0.0002,0.00066,0.00017,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26090000,0.98,-0.012,-0.012,0.18,-0.0079,-0.033,0.025,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.027,-0.13,0.2,-9.1e-06,0.43,-0.0002,0.00065,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.026,0.021,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.027,-0.13,0.2,-9.5e-06,0.43,-0.00022,0.00064,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.026,0.015,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.027,-0.13,0.2,-1e-05,0.43,-0.00023,0.00066,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26390000,0.98,-0.011,-0.013,0.18,-0.021,-0.018,0.019,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.037,-0.028,-0.13,0.2,-1.1e-05,0.43,-0.00027,0.00063,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26490000,0.98,-0.011,-0.013,0.18,-0.023,-0.016,0.028,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.037,-0.028,-0.13,0.2,-1.2e-05,0.43,-0.00028,0.00066,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26590000,0.98,-0.01,-0.013,0.18,-0.026,-0.0074,0.029,0,0,-4.9e+02,-0.0015,-0.0058,-0.00017,0.038,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00031,0.00068,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26690000,0.98,-0.01,-0.013,0.18,-0.027,-0.0035,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00018,0.037,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00033,0.00069,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26790000,0.98,-0.0099,-0.012,0.18,-0.035,0.00085,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.038,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00035,0.00067,0.00014,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26890000,0.98,-0.0092,-0.012,0.18,-0.041,0.0037,0.022,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.038,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00035,0.00065,0.00014,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26990000,0.98,-0.0087,-0.013,0.18,-0.048,0.011,0.021,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00037,0.00063,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27090000,0.98,-0.0085,-0.013,0.18,-0.05,0.017,0.025,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00036,0.00063,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27190000,0.98,-0.0086,-0.013,0.18,-0.056,0.024,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00037,0.00061,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27290000,0.98,-0.0088,-0.014,0.18,-0.063,0.029,0.14,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00038,0.00062,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0054,0.038,0.039,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27390000,0.98,-0.01,-0.016,0.18,-0.071,0.037,0.46,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.028,-0.13,0.2,-1.6e-05,0.43,-0.00045,0.00054,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27490000,0.98,-0.012,-0.018,0.18,-0.075,0.042,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.039,-0.028,-0.13,0.2,-1.7e-05,0.43,-0.00042,0.0005,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27590000,0.98,-0.011,-0.017,0.18,-0.07,0.045,0.87,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.028,-0.13,0.2,-1.7e-05,0.43,-0.0004,0.00044,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27690000,0.98,-0.01,-0.014,0.18,-0.066,0.041,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.00043,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27790000,0.98,-0.0088,-0.013,0.18,-0.066,0.04,0.77,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.0004,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27890000,0.98,-0.0085,-0.013,0.18,-0.071,0.046,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.00039,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27990000,0.98,-0.0089,-0.013,0.18,-0.072,0.049,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00019,0.038,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00039,0.00033,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28090000,0.98,-0.0092,-0.013,0.18,-0.076,0.049,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00019,0.038,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.0004,0.00036,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28190000,0.98,-0.0086,-0.013,0.18,-0.077,0.047,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00018,0.038,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00038,0.00034,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28290000,0.98,-0.0081,-0.014,0.18,-0.081,0.05,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.038,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.00036,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28390000,0.98,-0.0081,-0.014,0.18,-0.082,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.038,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00044,0.00027,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28490000,0.98,-0.0084,-0.015,0.18,-0.085,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.038,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.0004,0.00025,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28590000,0.98,-0.0085,-0.015,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.037,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00037,0.00023,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28690000,0.98,-0.0082,-0.014,0.18,-0.078,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.037,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00035,0.00024,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.036,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00037,0.00013,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28890000,0.98,-0.0074,-0.014,0.18,-0.08,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00037,0.00017,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28990000,0.98,-0.0072,-0.014,0.18,-0.077,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-2e-05,0.43,-0.00041,1.5e-05,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29090000,0.98,-0.007,-0.014,0.18,-0.08,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00043,-4e-06,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.034,-0.029,-0.12,0.2,-2.1e-05,0.43,-0.00046,-0.00014,0.00028,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.034,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00045,-0.00016,0.00028,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29390000,0.98,-0.0076,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.034,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00049,-0.00029,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29490000,0.98,-0.0076,-0.013,0.18,-0.08,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-9.9e-05,0.034,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00051,-0.00026,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-8.2e-05,0.033,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00056,-0.00037,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-7.5e-05,0.033,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00057,-0.00033,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-5.9e-05,0.032,-0.029,-0.12,0.2,-2.6e-05,0.43,-0.00063,-0.00045,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29890000,0.98,-0.0068,-0.014,0.18,-0.08,0.055,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.3e-05,0.032,-0.028,-0.12,0.2,-2.6e-05,0.43,-0.00065,-0.00043,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29990000,0.98,-0.007,-0.014,0.18,-0.075,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-4.2e-05,0.031,-0.029,-0.12,0.2,-2.7e-05,0.43,-0.00063,-0.00048,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30090000,0.98,-0.0071,-0.014,0.18,-0.077,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.2e-05,0.031,-0.029,-0.12,0.2,-2.8e-05,0.43,-0.00058,-0.00055,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30190000,0.98,-0.0071,-0.014,0.18,-0.071,0.047,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.03,-0.029,-0.12,0.2,-3.1e-05,0.43,-0.00059,-0.00077,0.00036,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30290000,0.98,-0.0071,-0.014,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.3e-05,0.03,-0.029,-0.12,0.2,-3e-05,0.43,-0.00063,-0.00082,0.00037,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30390000,0.98,-0.0071,-0.013,0.18,-0.065,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-3.5e-05,0.03,-0.03,-0.12,0.2,-3.1e-05,0.43,-0.00077,-0.001,0.00037,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30490000,0.98,-0.0071,-0.014,0.18,-0.068,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-2.9e-05,0.03,-0.03,-0.12,0.2,-3.1e-05,0.43,-0.00081,-0.001,0.00035,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30590000,0.98,-0.0074,-0.014,0.17,-0.064,0.041,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.4e-05,0.029,-0.03,-0.12,0.2,-3.2e-05,0.43,-0.00092,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30690000,0.98,-0.0078,-0.014,0.18,-0.062,0.039,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.4e-05,0.029,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00089,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30790000,0.98,-0.0075,-0.014,0.17,-0.056,0.034,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-9.6e-06,0.029,-0.031,-0.12,0.2,-3.3e-05,0.43,-0.00097,-0.0013,0.00039,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30890000,0.98,-0.0069,-0.014,0.17,-0.056,0.03,0.79,0,0,-4.9e+02,-0.0014,-0.0058,-1.6e-05,0.029,-0.031,-0.12,0.2,-3.4e-05,0.43,-0.00087,-0.0012,0.00041,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30990000,0.98,-0.0071,-0.013,0.17,-0.048,0.025,0.8,0,0,-4.9e+02,-0.0013,-0.0057,-8.9e-06,0.028,-0.032,-0.12,0.2,-3.5e-05,0.43,-0.00091,-0.0013,0.00044,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31090000,0.98,-0.0073,-0.014,0.17,-0.048,0.024,0.79,0,0,-4.9e+02,-0.0013,-0.0057,-1.3e-05,0.028,-0.033,-0.12,0.2,-3.5e-05,0.43,-0.00086,-0.0013,0.00045,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31190000,0.98,-0.0075,-0.014,0.17,-0.043,0.02,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.6e-06,0.028,-0.033,-0.12,0.2,-3.6e-05,0.43,-0.00096,-0.0013,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.017,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.7e-06,0.029,-0.034,-0.12,0.2,-3.5e-05,0.43,-0.001,-0.0012,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.1e-06,0.028,-0.034,-0.12,0.2,-3.4e-05,0.43,-0.0011,-0.0016,0.00048,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0078,0.8,0,0,-4.9e+02,-0.0013,-0.0057,5.4e-06,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0011,-0.0018,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31590000,0.98,-0.0071,-0.014,0.17,-0.032,0.0059,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.5e-05,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0013,-0.0019,0.00052,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31690000,0.98,-0.0071,-0.015,0.17,-0.036,0.0053,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0014,-0.0019,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31790000,0.99,-0.0074,-0.015,0.17,-0.026,0.0027,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.8e-05,0.028,-0.034,-0.12,0.2,-3.2e-05,0.43,-0.0015,-0.002,0.00053,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31890000,0.99,-0.0071,-0.015,0.17,-0.025,0.00049,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3.2e-05,0.028,-0.034,-0.12,0.2,-3.1e-05,0.43,-0.0016,-0.0021,0.00055,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31990000,0.99,-0.0074,-0.015,0.17,-0.017,-0.00059,0.79,0,0,-4.9e+02,-0.0013,-0.0057,2.9e-05,0.029,-0.035,-0.12,0.2,-3.2e-05,0.43,-0.0015,-0.0023,0.0006,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32090000,0.99,-0.0077,-0.014,0.17,-0.018,-0.004,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3e-05,0.029,-0.034,-0.12,0.2,-3.1e-05,0.43,-0.0016,-0.0024,0.00062,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32190000,0.99,-0.008,-0.015,0.17,-0.013,-0.0076,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.029,-0.035,-0.12,0.2,-3e-05,0.43,-0.0016,-0.0025,0.00067,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32290000,0.99,-0.0079,-0.015,0.17,-0.012,-0.01,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3e-05,0.029,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0017,-0.0026,0.00068,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32390000,0.99,-0.008,-0.015,0.17,-0.0061,-0.012,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.7e-05,0.03,-0.035,-0.12,0.2,-2.8e-05,0.43,-0.0017,-0.0028,0.00074,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32490000,0.99,-0.011,-0.013,0.17,0.019,-0.078,-0.076,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0028,0.00073,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32590000,0.99,-0.011,-0.013,0.17,0.022,-0.079,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0024,0.0008,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32690000,0.99,-0.011,-0.013,0.17,0.017,-0.085,-0.08,0,0,-4.9e+02,-0.0013,-0.0057,2.1e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0025,0.00079,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32790000,0.99,-0.011,-0.013,0.17,0.018,-0.084,-0.081,0,0,-4.9e+02,-0.0013,-0.0057,2e-05,0.03,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0016,-0.0023,0.00086,0,0,-4.9e+02,0.00028,0.00027,0.035,0.011,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.03,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0016,-0.0022,0.00088,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.03,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0017,-0.0021,0.00094,0,0,-4.9e+02,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33090000,0.98,-0.011,-0.013,0.17,0.013,-0.094,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.03,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0017,-0.0021,0.00094,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,1.6e-05,0.031,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0016,-0.0021,0.00096,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33290000,0.99,-0.01,-0.013,0.17,0.0078,-0.096,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,2.7e-05,0.031,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0018,-0.0022,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33390000,0.98,-0.01,-0.013,0.17,0.0056,-0.091,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,2e-05,0.033,-0.033,-0.12,0.2,-2e-05,0.43,-0.0018,-0.0021,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 +33490000,0.99,-0.01,-0.013,0.17,0.0003,-0.094,-0.075,0,0,-4.9e+02,-0.0014,-0.0057,2.5e-05,0.033,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0019,-0.0023,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 +33590000,0.99,-0.01,-0.013,0.17,-0.0025,-0.092,-0.072,0,0,-4.9e+02,-0.0014,-0.0057,2.4e-05,0.034,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.002,-0.0023,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.083 +33690000,0.98,-0.01,-0.013,0.17,-0.0058,-0.095,-0.073,0,0,-4.9e+02,-0.0014,-0.0057,2.8e-05,0.034,-0.032,-0.12,0.2,-1.6e-05,0.43,-0.0019,-0.0022,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.11 +33790000,0.98,-0.01,-0.013,0.17,-0.0077,-0.093,-0.068,0,0,-4.9e+02,-0.0014,-0.0057,1.8e-05,0.036,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.0019,-0.002,0.0012,0,0,-4.9e+02,0.00027,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.13 +33890000,0.98,-0.01,-0.013,0.17,-0.012,-0.093,-0.067,0,0,-4.9e+02,-0.0014,-0.0057,2.8e-05,0.036,-0.032,-0.12,0.2,-1.3e-05,0.43,-0.0019,-0.002,0.0012,0,0,-4.9e+02,0.00027,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.4e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.16 +33990000,0.98,-0.0099,-0.013,0.17,-0.013,-0.089,-0.064,0,0,-4.9e+02,-0.0014,-0.0057,1.6e-05,0.038,-0.031,-0.12,0.2,-1e-05,0.43,-0.0019,-0.0019,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.18 +34090000,0.98,-0.0098,-0.013,0.17,-0.017,-0.093,-0.062,0,0,-4.9e+02,-0.0014,-0.0057,1.8e-05,0.038,-0.031,-0.12,0.2,-1.1e-05,0.43,-0.0019,-0.0018,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.21 +34190000,0.98,-0.0098,-0.013,0.17,-0.018,-0.088,-0.06,0,0,-4.9e+02,-0.0014,-0.0057,1.4e-05,0.04,-0.031,-0.12,0.2,-8.5e-06,0.43,-0.0018,-0.0017,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.23 +34290000,0.98,-0.0096,-0.014,0.17,-0.019,-0.092,-0.058,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.04,-0.031,-0.12,0.2,-7.8e-06,0.43,-0.0019,-0.0017,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.26 +34390000,0.98,-0.0095,-0.014,0.17,-0.021,-0.086,-0.054,0,0,-4.9e+02,-0.0014,-0.0057,1.4e-05,0.041,-0.031,-0.12,0.2,-5.7e-06,0.43,-0.0018,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.28 +34490000,0.98,-0.0095,-0.014,0.17,-0.024,-0.089,-0.052,0,0,-4.9e+02,-0.0014,-0.0056,2.2e-05,0.041,-0.031,-0.12,0.2,-5.4e-06,0.43,-0.0019,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.31 +34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.082,0.74,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.043,-0.03,-0.12,0.2,-2.9e-06,0.43,-0.0018,-0.0015,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 +34690000,0.98,-0.0094,-0.013,0.17,-0.031,-0.08,1.7,0,0,-4.9e+02,-0.0014,-0.0056,1.8e-05,0.043,-0.031,-0.12,0.2,-2.6e-06,0.43,-0.0019,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 +34790000,0.98,-0.0093,-0.013,0.17,-0.034,-0.071,2.7,0,0,-4.9e+02,-0.0014,-0.0056,1.2e-05,0.045,-0.031,-0.12,0.2,6.3e-08,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 +34890000,0.98,-0.0092,-0.013,0.17,-0.04,-0.069,3.7,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.045,-0.031,-0.12,0.2,2.3e-07,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 From aeb182a8edb5c399575c009fd00f3b625dc25ac2 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Nov 2024 10:35:17 +0100 Subject: [PATCH 053/211] ekf2: rename resetAltitudeTo to initialiseAltitudeTo This is to better show that the altitude is also used to set the origin. --- .../ekf2/EKF/aid_sources/barometer/baro_height_control.cpp | 2 +- src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp | 2 +- src/modules/ekf2/EKF/ekf.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 4 ++-- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 0f5bcd610c91..70cb041784bb 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -163,7 +163,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::BARO; _information_events.flags.reset_hgt_to_baro = true; - resetAltitudeTo(measurement, measurement_var); + initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); } else { diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index 78a6d12f90a3..d31587461570 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -129,7 +129,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) _information_events.flags.reset_hgt_to_gps = true; - resetAltitudeTo(measurement, measurement_var); + initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); } else { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index c3d4434a714d..a0df2a2ad710 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -296,7 +296,7 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl return false; } - resetAltitudeTo(altitude, sq(epv)); + initialiseAltitudeTo(altitude, sq(epv)); return true; } @@ -370,7 +370,7 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl const float obs_var = math::max(sq(epv), sq(0.01f)); ECL_INFO("reset height to external observation"); - resetAltitudeTo(gpos_corrected.altitude(), obs_var); + initialiseAltitudeTo(gpos_corrected.altitude(), obs_var); _last_known_gpos.setAltitude(gpos_corrected.altitude()); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 47c6e10f94b6..41728ea0084f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -649,7 +649,7 @@ class Ekf final : public EstimatorInterface bool setAltOrigin(float altitude, float vpos_var = NAN); bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN); - bool resetAltitudeTo(float altitude, float vpos_var = NAN); + bool initialiseAltitudeTo(float altitude, float vpos_var = NAN); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 33e63a6eb64a..e9332fdd901b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -160,7 +160,7 @@ bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, c } // altitude is optional - resetAltitudeTo(altitude, vpos_var); + initialiseAltitudeTo(altitude, vpos_var); return true; } @@ -216,7 +216,7 @@ bool Ekf::resetLatLonTo(const double latitude, const double longitude, const flo return true; } -bool Ekf::resetAltitudeTo(const float altitude, const float vpos_var) +bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; From a224d38e525c99d1a38ca11fadcb981cd85dc115 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 14 Nov 2024 15:43:54 +0100 Subject: [PATCH 054/211] lla: move implementation to cpp file This reduces flash usage --- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + .../ekf2/EKF/lat_lon_alt/CMakeLists.txt | 44 ++++++- .../ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp | 108 ++++++++++++++++++ .../ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp | 76 ++---------- .../ekf2/EKF/output_predictor/CMakeLists.txt | 2 +- 6 files changed, 164 insertions(+), 68 deletions(-) create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 62e6f2910330..112b920ba251 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -277,6 +277,7 @@ px4_add_module( world_magnetic_model ${EKF_LIBS} + lat_lon_alt bias_estimator output_predictor UNITY_BUILD diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 27804ebcd88a..f6dc615f4e36 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -155,6 +155,7 @@ target_link_libraries(ecl_EKF PRIVATE bias_estimator geo + lat_lon_alt output_predictor world_magnetic_model ${EKF_LIBS} diff --git a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt index bf2b3ebf536b..acd30f51f22f 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt +++ b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt @@ -1 +1,43 @@ -px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS geo) +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +add_library(lat_lon_alt + lat_lon_alt.cpp + lat_lon_alt.hpp +) + +add_dependencies(lat_lon_alt prebuild_targets) +target_include_directories(lat_lon_alt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo) diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp new file mode 100644 index 000000000000..93d347f2d73e --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "lat_lon_alt.hpp" + +using matrix::Vector3f; +using matrix::Vector2d; + +Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const +{ + double r_n; + double r_e; + computeRadiiOfCurvature(_latitude_rad, r_n, r_e); + return Vector3f( + v_ned(1) / (static_cast(r_e) + _altitude), + -v_ned(0) / (static_cast(r_n) + _altitude), + -v_ned(1) * tanf(_latitude_rad) / (static_cast(r_e) + _altitude)); +} + +Vector2d LatLonAlt::deltaLatLonToDeltaXY(const double latitude, const float altitude) +{ + double r_n; + double r_e; + computeRadiiOfCurvature(latitude, r_n, r_e); + const double dn_dlat = r_n + static_cast(altitude); + const double de_dlon = (r_e + static_cast(altitude)) * cos(latitude); + + return Vector2d(dn_dlat, de_dlon); +} + +void LatLonAlt::computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature, + double &transverse_radius_of_curvature) +{ + const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2); + const double sqrt_tmp = std::sqrt(tmp); + meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp); + transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp; +} + +LatLonAlt LatLonAlt::operator+(const matrix::Vector3f &delta_pos) const +{ + const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude()); + const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + const float altitude = _altitude - delta_pos(2); + + LatLonAlt lla_new; + lla_new.setLatLonRad(latitude_rad, longitude_rad); + lla_new.setAltitude(altitude); + return lla_new; +} + +void LatLonAlt::operator+=(const matrix::Vector3f &delta_pos) +{ + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + _altitude -= delta_pos(2); +} + +void LatLonAlt::operator+=(const matrix::Vector2f &delta_pos) +{ + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); +} + +matrix::Vector3f LatLonAlt::operator-(const LatLonAlt &lla) const +{ + const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad()); + const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad()); + const float delta_alt = _altitude - lla.altitude(); + + const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + return matrix::Vector3f(static_cast(delta_lat * d_lat_lon_to_d_xy(0)), + static_cast(delta_lon * d_lat_lon_to_d_xy(1)), + -delta_alt); +} diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp index bb6a384d6ac6..548f6ee30d46 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -73,33 +73,13 @@ class LatLonAlt void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); } - void operator+=(const matrix::Vector3f &delta_pos) - { - matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); - _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); - _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); - _altitude -= delta_pos(2); - } - - void operator+=(const matrix::Vector2f &delta_pos) - { - matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); - _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); - _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); - } - - LatLonAlt operator+(const matrix::Vector3f &delta_pos) const - { - matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude()); - const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); - const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); - const float altitude = _altitude - delta_pos(2); - - LatLonAlt lla_new; - lla_new.setLatLonRad(latitude_rad, longitude_rad); - lla_new.setAltitude(altitude); - return lla_new; - } + /* + * The plus and minus operators below use approximations and should only be used when the Cartesian component is small + */ + LatLonAlt operator+(const matrix::Vector3f &delta_pos) const; + void operator+=(const matrix::Vector3f &delta_pos); + void operator+=(const matrix::Vector2f &delta_pos); + matrix::Vector3f operator-(const LatLonAlt &lla) const; void operator=(const LatLonAlt &lla) { @@ -108,54 +88,18 @@ class LatLonAlt _altitude = lla.altitude(); } - matrix::Vector3f operator-(const LatLonAlt &lla) const - { - const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad()); - const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad()); - const float delta_alt = _altitude - lla.altitude(); - - const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); - return matrix::Vector3f(static_cast(delta_lat * d_lat_lon_to_d_xy(0)), - static_cast(delta_lon * d_lat_lon_to_d_xy(1)), - -delta_alt); - } - /* * Compute the angular rate of the local navigation frame at the current latitude and height * with respect to an inertial frame and resolved in the navigation frame */ - matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) - { - double r_n; - double r_e; - computeRadiiOfCurvature(_latitude_rad, r_n, r_e); - return matrix::Vector3f( - v_ned(1) / (static_cast(r_e) + _altitude), - -v_ned(0) / (static_cast(r_n) + _altitude), - -v_ned(1) * tanf(_latitude_rad) / (static_cast(r_e) + _altitude)); - } + matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const; private: // Convert between curvilinear and cartesian errors - static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude) - { - double r_n; - double r_e; - computeRadiiOfCurvature(latitude, r_n, r_e); - const double dn_dlat = r_n + static_cast(altitude); - const double de_dlon = (r_e + static_cast(altitude)) * cos(latitude); - - return matrix::Vector2d(dn_dlat, de_dlon); - } + static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude); static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature, - double &transverse_radius_of_curvature) - { - const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2); - const double sqrt_tmp = std::sqrt(tmp); - meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp); - transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp; - } + double &transverse_radius_of_curvature); struct Wgs84 { static constexpr double equatorial_radius = 6378137.0; diff --git a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt index 701a5cb13f65..8d7b3e0607b3 100644 --- a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt +++ b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt @@ -36,4 +36,4 @@ add_library(output_predictor output_predictor.h ) -add_dependencies(output_predictor prebuild_targets) +add_dependencies(output_predictor prebuild_targets lat_lon_alt) From 93c690f1331ed696613f9302bf4b70e9443451c0 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2024 12:03:11 +0100 Subject: [PATCH 055/211] ekf2: do not inline getPosition --- src/modules/ekf2/EKF/estimator_interface.cpp | 20 ++++++++++++++++++++ src/modules/ekf2/EKF/estimator_interface.h | 20 +------------------- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index f7a6c70dec39..84ced94c8004 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -568,6 +568,26 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) return true; } +Vector3f EstimatorInterface::getPosition() const +{ + LatLonAlt lla = _output_predictor.getLatLonAlt(); + float x; + float y; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + } + + const float z = -(lla.altitude() - getEkfGlobalOriginAltitude()); + + return Vector3f(x, y, z); +} + bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const { return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5322fe09976c..90c26d608392 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -242,25 +242,7 @@ class EstimatorInterface 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 - { - LatLonAlt lla = _output_predictor.getLatLonAlt(); - float x; - float y; - - if (_local_origin_lat_lon.isInitialized()) { - _local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y); - - } else { - MapProjection zero_ref; - zero_ref.initReference(0.0, 0.0); - zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y); - } - - const float z = -(lla.altitude() - getEkfGlobalOriginAltitude()); - - return Vector3f(x, y, z); - } + Vector3f getPosition() const; LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } From 14fe6c2167ce1712e9c0289c46a052d2b1f549a4 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2024 12:21:16 +0100 Subject: [PATCH 056/211] ekf2: resetHeightTo -> resetAltitudeTo The vertical position state is now an altitude, not just a local height --- .../ekf2/EKF/aid_sources/barometer/baro_height_control.cpp | 2 +- .../EKF/aid_sources/external_vision/ev_height_control.cpp | 6 +++--- src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp | 2 +- .../ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp | 2 +- .../EKF/aid_sources/range_finder/range_height_control.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 4 ++-- src/modules/ekf2/EKF/position_fusion.cpp | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 70cb041784bb..c645afd9d5c7 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -131,7 +131,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_baro = true; - resetHeightTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); + resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); // reset vertical velocity if no valid sources available diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index f62be7575fdc..232ecd375284 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -117,7 +117,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_height_sensor_ref == HeightSensor::EV) { _information_events.flags.reset_hgt_to_ev = true; - resetHeightTo(-measurement, measurement_var); + resetAltitudeTo(-measurement, measurement_var); bias_est.reset(); } else { @@ -146,7 +146,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetHeightTo(-measurement - bias_est.getBias(), measurement_var); + resetAltitudeTo(-measurement - bias_est.getBias(), measurement_var); bias_est.setBias(_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; @@ -170,7 +170,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_params.height_sensor_ref == static_cast(HeightSensor::EV)) { ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetHeightTo(-measurement, measurement_var); + resetAltitudeTo(-measurement, measurement_var); _height_sensor_ref = HeightSensor::EV; bias_est.reset(); diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index 4ff4e67d6cd9..8fa2dfb62592 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -110,7 +110,7 @@ void Ekf::resetHeightToLastKnown() { _information_events.flags.reset_pos_to_last_known = true; ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude()); - resetHeightTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise)); + resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise)); } void Ekf::stopFakeHgtFusion() diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index d31587461570..c936442b1405 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -105,7 +105,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetHeightTo(measurement, measurement_var); + resetAltitudeTo(measurement, measurement_var); bias_est.setBias(-_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 2598f3a3318c..fec1fd9d8e85 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -148,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::RANGE; _information_events.flags.reset_hgt_to_rng = true; - resetHeightTo(aid_src.observation, aid_src.observation_variance); + resetAltitudeTo(aid_src.observation, aid_src.observation_variance); _state.terrain = 0.f; _control_status.flags.rng_hgt = true; stopRngTerrFusion(); @@ -180,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; - resetHeightTo(aid_src.observation - _state.terrain); + resetAltitudeTo(aid_src.observation - _state.terrain); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 41728ea0084f..f42493652ecc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -725,7 +725,7 @@ class Ekf final : public EstimatorInterface bool isHeightResetRequired() const; - void resetHeightTo(float new_altitude, float new_vert_pos_var = NAN); + void resetAltitudeTo(float new_altitude, float new_vert_pos_var = NAN); void updateVerticalPositionResetStatus(const float delta_z); void resetVerticalVelocityToZero(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e9332fdd901b..9638abeb694c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -137,7 +137,7 @@ bool Ekf::setAltOrigin(const float altitude, const float vpos_var) if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) { const float local_alt_prev = _gpos.altitude(); _local_origin_alt = altitude; - resetHeightTo(local_alt_prev + _local_origin_alt); + resetAltitudeTo(local_alt_prev + _local_origin_alt); } else { const float delta_origin_alt = altitude - _local_origin_alt; @@ -235,7 +235,7 @@ bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var) ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt); } - resetHeightTo(altitude, vpos_var); + resetAltitudeTo(altitude, vpos_var); return true; } diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index e4c16b49415b..7d874e654081 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -182,7 +182,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos, resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var); } -void Ekf::resetHeightTo(const float new_altitude, float new_vert_pos_var) +void Ekf::resetAltitudeTo(const float new_altitude, float new_vert_pos_var) { const float old_altitude = _gpos.altitude(); _gpos.setAltitude(new_altitude); From 14468d49c1f0000a4eda55edd27031dd6f3a6579 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Fri, 22 Nov 2024 04:57:46 -0800 Subject: [PATCH 057/211] ci: flash analysis updates * updates comment instead of posting a new one each time * runs on dronecode infra --- .github/workflows/flash_analysis.yml | 76 ++++++++++++++++------------ 1 file changed, 43 insertions(+), 33 deletions(-) diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml index d7a2d5b17191..2b5209511844 100644 --- a/.github/workflows/flash_analysis.yml +++ b/.github/workflows/flash_analysis.yml @@ -10,8 +10,8 @@ on: jobs: analyze_flash: - name: FLASH usage analysis - runs-on: ubuntu-latest + name: Analyzing ${{ matrix.target }} + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] container: image: px4io/px4-dev-nuttx-focal strategy: @@ -29,7 +29,7 @@ jobs: - name: Git ownership workaround run: git config --system --add safe.directory '*' - - name: Build + - name: Build Target run: make ${{ matrix.target }} - name: Store the ELF with the change @@ -73,36 +73,46 @@ jobs: echo "$EOF" >> $GITHUB_OUTPUT post_pr_comment: - name: Post PR comment - runs-on: ubuntu-latest + name: Publish Results + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: [analyze_flash] + if: ${{ github.event.pull_request }} steps: - - name: If it's a PR add a comment with the bloaty output - if: ${{ github.event.pull_request }} - uses: actions/github-script@v6 + - name: Find Comment + uses: peter-evans/find-comment@v3 + id: fc + with: + issue-number: ${{ github.event.pull_request.number }} + comment-author: 'github-actions[bot]' + body-includes: FLASH Analysis + + - name: Set Build Time + id: bt + run: | + echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT + + - name: Create or update comment + uses: peter-evans/create-or-update-comment@v4 with: - script: | - const comment = [ - '## FLASH Analysis', - '
', - 'px4_fmu-v5x', - '', - '```', - `${{ needs.analyze_flash.outputs.px4_fmu-v5x }}`, - '```', - '
', - '', - '
', - 'px4_fmu-v6x', - '', - '```', - `${{ needs.analyze_flash.outputs.px4_fmu-v6x }}`, - '```', - '
' - ] - github.rest.issues.createComment({ - issue_number: context.issue.number, - owner: context.repo.owner, - repo: context.repo.repo, - body: comment.join('\n') - }) + comment-id: ${{ steps.fc.outputs.comment-id }} + issue-number: ${{ github.event.pull_request.number }} + body: | + ## FLASH Analysis +
+ px4_fmu-v5x + + ``` + ${{ needs.analyze_flash.outputs.px4_fmu-v5x }} + ``` +
+ +
+ px4_fmu-v6x + + ``` + ${{ needs.analyze_flash.outputs.px4_fmu-v6x }} + ``` +
+ + **Updated: _${{ steps.bt.outputs.timestamp }}_** + edit-mode: replace From fb42770131016394dc645f87a43ecab396c6d235 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Fri, 22 Nov 2024 05:30:45 -0800 Subject: [PATCH 058/211] Tools: instal ccache on ubuntu --- Tools/setup/ubuntu.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 2d2fa00c1e4c..e2f215dc2d66 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -64,6 +64,7 @@ sudo apt-get update -y --quiet sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ astyle \ build-essential \ + ccache \ cmake \ cppcheck \ file \ From f2bd3105adf934501a425458d31ffcd252abe30e Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Fri, 22 Nov 2024 05:36:31 -0800 Subject: [PATCH 059/211] ci: tag container main & main-date Make sure we always have a { branch name } container tag in addition to a { branch name + date } tag. This way we have a rolling { branch name } of the main and release branches --- .github/workflows/dev_container.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index d93a1292c3c6..c5b86caf4337 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -43,7 +43,8 @@ jobs: type=semver,pattern={{version}} type=semver,pattern={{major}}.{{minor}} type=semver,pattern={{major}} - type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}} + type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600 + type=ref,event=branch,suffix=,priority=500 type=ref,event=pr type=sha From 3240cf4dc744d922d7a6b754df9eecbc97533d1c Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Fri, 22 Nov 2024 05:39:48 -0800 Subject: [PATCH 060/211] ci: push px4-dev container to docker hub registry --- .github/workflows/dev_container.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index c5b86caf4337..f6644d8609da 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -25,6 +25,12 @@ jobs: submodules: false fetch-depth: 0 + - name: Login to Docker Hub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Login to GitHub Container Registry uses: docker/login-action@v3 with: @@ -38,6 +44,7 @@ jobs: with: images: | ghcr.io/PX4/px4-dev + px4io/px4-dev tags: | type=schedule type=semver,pattern={{version}} From 7462e98e160ee0c7a65e592f0eb16515d87bf9eb Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Fri, 22 Nov 2024 05:41:24 -0800 Subject: [PATCH 061/211] ci: publish pr images to registry --- .github/workflows/dev_container.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index f6644d8609da..cf88648207b7 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -90,7 +90,6 @@ jobs: - name: Push container image uses: docker/build-push-action@v6 - if: ${{ github.event_name == 'push' }} with: context: Tools/setup tags: ${{ steps.meta.outputs.tags }} From e194a52907083783f7e67ff93504016ca1275ce8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 15 Nov 2024 19:41:20 -0500 Subject: [PATCH 062/211] ekf2: derivation.py remove sideslip small angle approximation --- .../EKF/python/ekf_derivation/derivation.py | 4 +- .../generated/compute_sideslip_h_and_k.h | 223 +++++++++--------- .../compute_sideslip_innov_and_innov_var.h | 133 ++++++----- 3 files changed, 178 insertions(+), 182 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 36e2f3437351..86651f9d656c 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -337,9 +337,7 @@ def predict_sideslip( vel_rel = state["vel"] - wind relative_wind_body = state["quat_nominal"].inverse() * vel_rel - # Small angle approximation of side slip model - # Protect division by zero using epsilon - sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon) + sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon) return sideslip_pred diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h index 002be9bbe8e9..6181936f8fa2 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h @@ -30,66 +30,65 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar innov_var, const Scalar epsilon, matrix::Matrix* const H = nullptr, matrix::Matrix* const K = nullptr) { - // Total ops: 513 + // Total ops: 518 // Input arrays - // Intermediate terms (43) - const Scalar _tmp0 = -state(23, 0) + state(5, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = -state(22, 0) + state(4, 0); - const Scalar _tmp3 = 4 * _tmp2; - const Scalar _tmp4 = 2 * state(6, 0); - const Scalar _tmp5 = _tmp4 * state(0, 0); - const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp8 = 2 * state(0, 0); - const Scalar _tmp9 = _tmp8 * state(3, 0); - const Scalar _tmp10 = 2 * state(2, 0); - const Scalar _tmp11 = _tmp10 * state(1, 0); - const Scalar _tmp12 = _tmp11 + _tmp9; - const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0); - const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7; - const Scalar _tmp15 = - _tmp14 + epsilon * (2 * math::min(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1); - const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp17 = _tmp11 - _tmp9; - const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0); - const Scalar _tmp19 = - (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2)); - const Scalar _tmp20 = _tmp4 * state(3, 0); - const Scalar _tmp21 = Scalar(1.0) / (_tmp15); - const Scalar _tmp22 = - -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) + - (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20); - const Scalar _tmp23 = 4 * _tmp0; - const Scalar _tmp24 = - -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) + - (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5); - const Scalar _tmp25 = 2 * state(3, 0); - const Scalar _tmp26 = _tmp4 * state(2, 0); - const Scalar _tmp27 = _tmp4 * state(1, 0); - const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) + - (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27); - const Scalar _tmp29 = - -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) + - (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26); + // Intermediate terms (50) + const Scalar _tmp0 = -state(22, 0) + state(4, 0); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = 2 * state(3, 0); + const Scalar _tmp3 = _tmp2 * state(6, 0); + const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp6 = _tmp2 * state(0, 0); + const Scalar _tmp7 = 2 * state(1, 0); + const Scalar _tmp8 = _tmp7 * state(2, 0); + const Scalar _tmp9 = _tmp6 + _tmp8; + const Scalar _tmp10 = -state(23, 0) + state(5, 0); + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0); + const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0); + const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); + const Scalar _tmp15 = Scalar(1.0) / (_tmp14); + const Scalar _tmp16 = 2 * _tmp10; + const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0); + const Scalar _tmp18 = std::pow(_tmp14, Scalar(2)); + const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp20 = -_tmp6 + _tmp8; + const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0); + const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0); + const Scalar _tmp23 = _tmp22 / _tmp18; + const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) - + _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17); + const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2))); + const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25; + const Scalar _tmp27 = _tmp26 * state(3, 0); + const Scalar _tmp28 = _tmp7 * state(6, 0); + const Scalar _tmp29 = _tmp11 * state(6, 0); const Scalar _tmp30 = - -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0); - const Scalar _tmp31 = - _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0); - const Scalar _tmp32 = - _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0); - const Scalar _tmp33 = _tmp19 * _tmp7; - const Scalar _tmp34 = _tmp17 * _tmp21; - const Scalar _tmp35 = -_tmp33 + _tmp34; - const Scalar _tmp36 = _tmp12 * _tmp19; - const Scalar _tmp37 = _tmp16 * _tmp21; - const Scalar _tmp38 = -_tmp36 + _tmp37; - const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21; - const Scalar _tmp40 = _tmp33 - _tmp34; - const Scalar _tmp41 = _tmp36 - _tmp37; - const Scalar _tmp42 = Scalar(1.0) / (math::max(epsilon, innov_var)); + _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29); + const Scalar _tmp31 = _tmp26 * state(1, 0); + const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) - + _tmp23 * (_tmp16 * state(2, 0) + _tmp3); + const Scalar _tmp33 = _tmp26 * state(0, 0); + const Scalar _tmp34 = 4 * state(3, 0); + const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) - + _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28); + const Scalar _tmp36 = _tmp26 * state(2, 0); + const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36; + const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35; + const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35; + const Scalar _tmp40 = _tmp23 * _tmp5; + const Scalar _tmp41 = _tmp15 * _tmp20; + const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41); + const Scalar _tmp43 = _tmp15 * _tmp19; + const Scalar _tmp44 = _tmp23 * _tmp9; + const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44); + const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21); + const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41); + const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44); + const Scalar _tmp49 = Scalar(1.0) / (math::max(epsilon, innov_var)); // Output terms (2) if (H != nullptr) { @@ -97,91 +96,91 @@ void ComputeSideslipHAndK(const matrix::Matrix& state, _h.setZero(); - _h(0, 0) = _tmp30; - _h(1, 0) = _tmp31; - _h(2, 0) = _tmp32; - _h(3, 0) = _tmp35; - _h(4, 0) = _tmp38; - _h(5, 0) = _tmp39; - _h(21, 0) = _tmp40; - _h(22, 0) = _tmp41; + _h(0, 0) = _tmp37; + _h(1, 0) = _tmp38; + _h(2, 0) = _tmp39; + _h(3, 0) = _tmp42; + _h(4, 0) = _tmp45; + _h(5, 0) = _tmp46; + _h(21, 0) = _tmp47; + _h(22, 0) = _tmp48; } if (K != nullptr) { matrix::Matrix& _k = (*K); _k(0, 0) = - _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 + - P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39); + _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 + + P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46); _k(1, 0) = - _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 + - P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39); + _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 + + P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46); _k(2, 0) = - _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 + - P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39); + _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 + + P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46); _k(3, 0) = - _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 + - P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39); + _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 + + P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46); _k(4, 0) = - _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 + - P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39); + _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 + + P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46); _k(5, 0) = - _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 + - P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39); + _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 + + P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46); _k(6, 0) = - _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 + - P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39); + _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 + + P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46); _k(7, 0) = - _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 + - P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39); + _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 + + P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46); _k(8, 0) = - _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 + - P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39); + _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 + + P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46); _k(9, 0) = - _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 + - P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39); + _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 + + P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46); _k(10, 0) = - _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 + - P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39); + _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 + + P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46); _k(11, 0) = - _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 + - P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39); + _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 + + P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46); _k(12, 0) = - _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 + - P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39); + _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 + + P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46); _k(13, 0) = - _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 + - P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39); + _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 + + P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46); _k(14, 0) = - _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 + - P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39); + _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 + + P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46); _k(15, 0) = - _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 + - P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39); + _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 + + P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46); _k(16, 0) = - _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 + - P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39); + _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 + + P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46); _k(17, 0) = - _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 + - P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39); + _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 + + P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46); _k(18, 0) = - _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 + - P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39); + _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 + + P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46); _k(19, 0) = - _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 + - P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39); + _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 + + P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46); _k(20, 0) = - _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 + - P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39); + _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 + + P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46); _k(21, 0) = - _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 + - P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39); + _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 + + P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46); _k(22, 0) = - _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 + - P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39); + _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 + + P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46); _k(23, 0) = - _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 + - P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39); + _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 + + P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h index 6918bddfc366..65a2e4df950c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -30,70 +30,69 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { - // Total ops: 265 + // Total ops: 266 // Input arrays - // Intermediate terms (42) + // Intermediate terms (49) const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp2 = -state(22, 0) + state(4, 0); - const Scalar _tmp3 = 2 * state(0, 0); - const Scalar _tmp4 = _tmp3 * state(3, 0); + const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = -state(23, 0) + state(5, 0); + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = _tmp3 * 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 = -state(23, 0) + state(5, 0); + const Scalar _tmp7 = -_tmp4 + _tmp6; + const Scalar _tmp8 = -state(22, 0) + state(4, 0); const Scalar _tmp9 = 2 * state(2, 0); - const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0); + const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0); const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8; - const Scalar _tmp12 = - _tmp11 + epsilon * (2 * math::min(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1); - const Scalar _tmp13 = Scalar(1.0) / (_tmp12); - const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp15 = -_tmp4 + _tmp6; - const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0); - const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0); - const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2)); - const Scalar _tmp19 = _tmp18 * _tmp7; - const Scalar _tmp20 = _tmp13 * _tmp14; - const Scalar _tmp21 = _tmp19 - _tmp20; - const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16; - const Scalar _tmp23 = _tmp1 * _tmp18; - const Scalar _tmp24 = _tmp13 * _tmp15; - const Scalar _tmp25 = -_tmp23 + _tmp24; - const Scalar _tmp26 = 2 * state(6, 0); - const Scalar _tmp27 = _tmp26 * state(0, 0); - const Scalar _tmp28 = _tmp26 * state(3, 0); - const Scalar _tmp29 = - (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) - - Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8); - const Scalar _tmp30 = - (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) - - Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9); - const Scalar _tmp31 = 2 * state(3, 0); - const Scalar _tmp32 = _tmp26 * state(2, 0); - const Scalar _tmp33 = _tmp26 * state(1, 0); - const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) - - Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32); - const Scalar _tmp35 = 4 * state(3, 0); - const Scalar _tmp36 = - (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) - - Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33); - const Scalar _tmp37 = - _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0); - const Scalar _tmp38 = - -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0); - const Scalar _tmp39 = - _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0); - const Scalar _tmp40 = _tmp23 - _tmp24; - const Scalar _tmp41 = -_tmp19 + _tmp20; + const Scalar _tmp12 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp13 = _tmp4 + _tmp6; + const Scalar _tmp14 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0); + const Scalar _tmp15 = _tmp12 * _tmp8 + _tmp13 * _tmp2 + _tmp14 * state(6, 0); + const Scalar _tmp16 = _tmp15 + epsilon * ((((_tmp15) > 0) - ((_tmp15) < 0)) + Scalar(0.5)); + const Scalar _tmp17 = Scalar(1.0) / (_tmp16); + const Scalar _tmp18 = _tmp1 * _tmp17; + const Scalar _tmp19 = std::pow(_tmp16, Scalar(2)); + const Scalar _tmp20 = _tmp11 / _tmp19; + const Scalar _tmp21 = _tmp13 * _tmp20; + const Scalar _tmp22 = _tmp19 / (std::pow(_tmp11, Scalar(2)) + _tmp19); + const Scalar _tmp23 = _tmp22 * (-_tmp18 + _tmp21); + const Scalar _tmp24 = _tmp12 * _tmp20; + const Scalar _tmp25 = _tmp17 * _tmp7; + const Scalar _tmp26 = _tmp22 * (-_tmp24 + _tmp25); + const Scalar _tmp27 = _tmp22 * (_tmp10 * _tmp17 - _tmp14 * _tmp20); + const Scalar _tmp28 = _tmp3 * state(6, 0); + const Scalar _tmp29 = 4 * _tmp8; + const Scalar _tmp30 = 2 * state(0, 0); + const Scalar _tmp31 = _tmp30 * state(6, 0); + const Scalar _tmp32 = + _tmp17 * (_tmp28 + _tmp5 * _tmp8) - _tmp20 * (_tmp2 * _tmp5 - _tmp29 * state(2, 0) - _tmp31); + const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp34 = _tmp33 * state(1, 0); + const Scalar _tmp35 = _tmp5 * state(6, 0); + const Scalar _tmp36 = _tmp9 * state(6, 0); + const Scalar _tmp37 = _tmp17 * (-_tmp3 * _tmp8 + _tmp35) - _tmp20 * (_tmp2 * _tmp3 - _tmp36); + const Scalar _tmp38 = _tmp33 * state(3, 0); + const Scalar _tmp39 = 4 * _tmp2; + const Scalar _tmp40 = + _tmp17 * (_tmp31 - _tmp39 * state(1, 0) + _tmp8 * _tmp9) - _tmp20 * (_tmp2 * _tmp9 + _tmp28); + const Scalar _tmp41 = _tmp33 * state(2, 0); + const Scalar _tmp42 = _tmp17 * (-_tmp30 * _tmp8 + _tmp36 - _tmp39 * state(3, 0)) - + _tmp20 * (_tmp2 * _tmp30 - _tmp29 * state(3, 0) + _tmp35); + const Scalar _tmp43 = _tmp33 * state(0, 0); + const Scalar _tmp44 = _tmp32 * _tmp34 - _tmp37 * _tmp38 - _tmp40 * _tmp41 + _tmp42 * _tmp43; + const Scalar _tmp45 = -_tmp32 * _tmp38 - _tmp34 * _tmp37 + _tmp40 * _tmp43 + _tmp41 * _tmp42; + const Scalar _tmp46 = _tmp32 * _tmp43 - _tmp34 * _tmp42 - _tmp37 * _tmp41 + _tmp38 * _tmp40; + const Scalar _tmp47 = _tmp22 * (_tmp24 - _tmp25); + const Scalar _tmp48 = _tmp22 * (_tmp18 - _tmp21); // Output terms (2) if (innov != nullptr) { Scalar& _innov = (*innov); - _innov = _tmp13 * _tmp17; + _innov = std::atan2(_tmp11, _tmp16); } if (innov_var != nullptr) { @@ -101,22 +100,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, _innov_var = R + - _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 + - P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) + - _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 + - P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) + - _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 + - P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) + - _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 + - P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) + - _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 + - P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) + - _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 + - P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) + - _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 + - P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) + - _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 + - P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22); + _tmp23 * (P(0, 22) * _tmp45 + P(1, 22) * _tmp46 + P(2, 22) * _tmp44 + P(21, 22) * _tmp47 + + P(22, 22) * _tmp23 + P(3, 22) * _tmp26 + P(4, 22) * _tmp48 + P(5, 22) * _tmp27) + + _tmp26 * (P(0, 3) * _tmp45 + P(1, 3) * _tmp46 + P(2, 3) * _tmp44 + P(21, 3) * _tmp47 + + P(22, 3) * _tmp23 + P(3, 3) * _tmp26 + P(4, 3) * _tmp48 + P(5, 3) * _tmp27) + + _tmp27 * (P(0, 5) * _tmp45 + P(1, 5) * _tmp46 + P(2, 5) * _tmp44 + P(21, 5) * _tmp47 + + P(22, 5) * _tmp23 + P(3, 5) * _tmp26 + P(4, 5) * _tmp48 + P(5, 5) * _tmp27) + + _tmp44 * (P(0, 2) * _tmp45 + P(1, 2) * _tmp46 + P(2, 2) * _tmp44 + P(21, 2) * _tmp47 + + P(22, 2) * _tmp23 + P(3, 2) * _tmp26 + P(4, 2) * _tmp48 + P(5, 2) * _tmp27) + + _tmp45 * (P(0, 0) * _tmp45 + P(1, 0) * _tmp46 + P(2, 0) * _tmp44 + P(21, 0) * _tmp47 + + P(22, 0) * _tmp23 + P(3, 0) * _tmp26 + P(4, 0) * _tmp48 + P(5, 0) * _tmp27) + + _tmp46 * (P(0, 1) * _tmp45 + P(1, 1) * _tmp46 + P(2, 1) * _tmp44 + P(21, 1) * _tmp47 + + P(22, 1) * _tmp23 + P(3, 1) * _tmp26 + P(4, 1) * _tmp48 + P(5, 1) * _tmp27) + + _tmp47 * (P(0, 21) * _tmp45 + P(1, 21) * _tmp46 + P(2, 21) * _tmp44 + P(21, 21) * _tmp47 + + P(22, 21) * _tmp23 + P(3, 21) * _tmp26 + P(4, 21) * _tmp48 + P(5, 21) * _tmp27) + + _tmp48 * (P(0, 4) * _tmp45 + P(1, 4) * _tmp46 + P(2, 4) * _tmp44 + P(21, 4) * _tmp47 + + P(22, 4) * _tmp23 + P(3, 4) * _tmp26 + P(4, 4) * _tmp48 + P(5, 4) * _tmp27); } } // NOLINT(readability/fn_size) From e047972cde2bbd4678a0a25ffc701dde7eedaac8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 17:31:28 +0100 Subject: [PATCH 063/211] Add new C++ PID library --- src/lib/pid/CMakeLists.txt | 12 +++- src/lib/pid/PID.cpp | 65 +++++++++++++++++++ src/lib/pid/PID.hpp | 64 ++++++++++++++++++ src/lib/pid/PIDTest.cpp | 129 +++++++++++++++++++++++++++++++++++++ 4 files changed, 268 insertions(+), 2 deletions(-) create mode 100644 src/lib/pid/PID.cpp create mode 100644 src/lib/pid/PID.hpp create mode 100644 src/lib/pid/PIDTest.cpp diff --git a/src/lib/pid/CMakeLists.txt b/src/lib/pid/CMakeLists.txt index b8ed30b7a20a..044e1b39b4de 100644 --- a/src/lib/pid/CMakeLists.txt +++ b/src/lib/pid/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2024 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,4 +31,12 @@ # ############################################################################ -px4_add_library(pid pid.cpp) +px4_add_library(PID + PID.cpp + PID.hpp +) +target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID) + +px4_add_library(pid pid.cpp) # TODO: remove deprecated pid library diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp new file mode 100644 index 000000000000..3f0d9b4b9385 --- /dev/null +++ b/src/lib/pid/PID.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2024 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 "PID.hpp" +#include "lib/mathlib/math/Functions.hpp" + +void PID::setGains(const float P, const float I, const float D) +{ + _gain_proportional = P; + _gain_integral = I; + _gain_derivative = D; +} + +float PID::update(const float feedback, const float dt, const bool update_integral) +{ + const float error = _setpoint - feedback; + const float feedback_change = std::isfinite(_last_feedback) ? (feedback - _last_feedback) / dt : 0.f; + const float output = (_gain_proportional * error) + _integral + (_gain_derivative * feedback_change); + + if (update_integral) { + updateIntegral(error, dt); + } + + _last_feedback = feedback; + return math::constrain(output, -_limit_output, _limit_output); +} + +void PID::updateIntegral(float error, const float dt) +{ + const float integral_new = _integral + _gain_integral * error * dt; + + if (std::isfinite(integral_new)) { + _integral = math::constrain(integral_new, -_limit_integral, _limit_integral); + } +} diff --git a/src/lib/pid/PID.hpp b/src/lib/pid/PID.hpp new file mode 100644 index 000000000000..cabdd3701b8a --- /dev/null +++ b/src/lib/pid/PID.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +class PID +{ +public: + PID() = default; + virtual ~PID() = default; + void setOutputLimit(const float limit) { _limit_output = limit; } + void setIntegralLimit(const float limit) { _limit_integral = limit; } + void setGains(const float P, const float I, const float D); + void setSetpoint(const float setpoint) { _setpoint = setpoint; } + float update(const float feedback, const float dt, const bool update_integral = true); + float getIntegral() { return _integral; } + void resetIntegral() { _integral = 0.f; }; + void resetDerivative() { _last_feedback = NAN; }; +private: + void updateIntegral(float error, const float dt); + + float _setpoint{0.f}; ///< current setpoint to track + float _integral{0.f}; ///< integral state + float _last_feedback{NAN}; + + // Gains, Limits + float _gain_proportional{0.f}; + float _gain_integral{0.f}; + float _gain_derivative{0.f}; + float _limit_integral{0.f}; + float _limit_output{0.f}; +}; diff --git a/src/lib/pid/PIDTest.cpp b/src/lib/pid/PIDTest.cpp new file mode 100644 index 000000000000..ed3c204e6ef8 --- /dev/null +++ b/src/lib/pid/PIDTest.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include + +TEST(PIDTest, AllZeroCase) +{ + PID pid; + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f); +} + +TEST(PIDTest, OutputLimit) +{ + PID pid; + pid.setOutputLimit(.01f); + pid.setGains(.1f, 0.f, 0.f); + pid.setSetpoint(1.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f); + EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f); + EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f); + EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f); + EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f); + EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f); +} + +TEST(PIDTest, ProportinalOnly) +{ + PID pid; + pid.setOutputLimit(1.f); + pid.setGains(.1f, 0.f, 0.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f); + pid.setSetpoint(1.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f); + + float plant = 0.f; + float output = 10000.f; + int i; // need function scope to check how many steps + + for (i = 1000; i > 0; i--) { + const float output_new = pid.update(plant, 0.f, false); + plant += output_new; + + // expect the output to get smaller with each iteration + if (output_new >= output) { + break; + } + + output = output_new; + } + + EXPECT_FLOAT_EQ(plant, 1.f); + EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge +} + +TEST(PIDTest, InteralOpenLoop) +{ + PID pid; + pid.setOutputLimit(1.f); + pid.setGains(0.f, .1f, 0.f); + pid.setIntegralLimit(.05f); + pid.setSetpoint(1.f); + + // Zero error + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f); + EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f); + + // Open loop ramp up + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + + // Open loop ramp down + pid.setSetpoint(-1.f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f); + EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); + pid.resetIntegral(); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f); +} From b89c53d28cf02abc88355adfbc5dca027288c1b4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 18:05:11 +0100 Subject: [PATCH 064/211] Replace old pid library with new one --- src/lib/pid/CMakeLists.txt | 2 - src/lib/pid/pid.cpp | 185 ------------------ src/lib/pid/pid.h | 91 --------- .../RoverAckermannControl/CMakeLists.txt | 2 +- .../RoverAckermannControl.cpp | 36 ++-- .../RoverAckermannControl.hpp | 6 +- .../RoverAckermannGuidance.hpp | 2 +- .../RoverDifferentialControl/CMakeLists.txt | 2 +- .../RoverDifferentialControl.cpp | 60 +++--- .../RoverDifferentialControl.hpp | 12 +- src/modules/rover_mecanum/RoverMecanum.hpp | 2 +- .../RoverMecanumControl/CMakeLists.txt | 2 +- .../RoverMecanumControl.cpp | 78 ++++---- .../RoverMecanumControl.hpp | 10 +- src/modules/rover_pos_control/CMakeLists.txt | 2 +- .../RoverPositionControl.cpp | 25 +-- .../RoverPositionControl.hpp | 6 +- .../uuv_att_control/uuv_att_control.hpp | 1 - .../uuv_pos_control/uuv_pos_control.hpp | 1 - 19 files changed, 102 insertions(+), 423 deletions(-) delete mode 100644 src/lib/pid/pid.cpp delete mode 100644 src/lib/pid/pid.h diff --git a/src/lib/pid/CMakeLists.txt b/src/lib/pid/CMakeLists.txt index 044e1b39b4de..8adba376b8bc 100644 --- a/src/lib/pid/CMakeLists.txt +++ b/src/lib/pid/CMakeLists.txt @@ -38,5 +38,3 @@ px4_add_library(PID target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID) - -px4_add_library(pid pid.cpp) # TODO: remove deprecated pid library diff --git a/src/lib/pid/pid.cpp b/src/lib/pid/pid.cpp deleted file mode 100644 index 076c1afeedb2..000000000000 --- a/src/lib/pid/pid.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes - * - * 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 pid.cpp - * - * Implementation of generic PID controller. - * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin - * @author Julian Oes - */ - -#include "pid.h" -#include -#include - -#define SIGMA 0.000001f - -__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min) -{ - pid->mode = mode; - pid->dt_min = dt_min; - pid->kp = 0.0f; - pid->ki = 0.0f; - pid->kd = 0.0f; - pid->integral = 0.0f; - pid->integral_limit = 0.0f; - pid->output_limit = 0.0f; - pid->error_previous = 0.0f; - pid->last_output = 0.0f; -} - -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) -{ - int ret = 0; - - if (PX4_ISFINITE(kp)) { - pid->kp = kp; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(ki)) { - pid->ki = ki; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(kd)) { - pid->kd = kd; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(integral_limit)) { - pid->integral_limit = integral_limit; - - } else { - ret = 1; - } - - if (PX4_ISFINITE(output_limit)) { - pid->output_limit = output_limit; - - } else { - ret = 1; - } - - return ret; -} - -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) -{ - if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) { - return pid->last_output; - } - - float i, d; - - /* current error value */ - float error = sp - val; - - /* current error derivative */ - if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; - - } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else if (pid->mode == PID_MODE_DERIVATIV_SET) { - d = -val_dot; - - } else { - d = 0.0f; - } - - if (!PX4_ISFINITE(d)) { - d = 0.0f; - } - - /* calculate PD output */ - float output = (error * pid->kp) + (d * pid->kd); - - if (pid->ki > SIGMA) { - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); - - /* check for saturation */ - if (PX4_ISFINITE(i)) { - if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && - fabsf(i) <= pid->integral_limit) { - /* not saturated, use new integral value */ - pid->integral = i; - } - } - - /* add I component to output */ - output += pid->integral * pid->ki; - } - - /* limit output */ - if (PX4_ISFINITE(output)) { - if (pid->output_limit > SIGMA) { - if (output > pid->output_limit) { - output = pid->output_limit; - - } else if (output < -pid->output_limit) { - output = -pid->output_limit; - } - } - - pid->last_output = output; - } - - return pid->last_output; -} - - -__EXPORT void pid_reset_integral(PID_t *pid) -{ - pid->integral = 0.0f; -} diff --git a/src/lib/pid/pid.h b/src/lib/pid/pid.h deleted file mode 100644 index e8b1aac4fd95..000000000000 --- a/src/lib/pid/pid.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes - * - * 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 pid.h - * - * Definition of generic PID controller. - * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin - * @author Julian Oes - */ - -#ifndef PID_H_ -#define PID_H_ - -#include - -__BEGIN_DECLS - -typedef enum PID_MODE { - /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ - PID_MODE_DERIVATIV_NONE = 0, - /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, - * val_dot in pid_calculate() will be ignored */ - PID_MODE_DERIVATIV_CALC, - /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, - * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ - PID_MODE_DERIVATIV_CALC_NO_SP, - /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ - PID_MODE_DERIVATIV_SET -} pid_mode_t; - -typedef struct { - pid_mode_t mode; - float dt_min; - float kp; - float ki; - float kd; - float integral; - float integral_limit; - float output_limit; - float error_previous; - float last_output; -} PID_t; - -__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); -__EXPORT void pid_reset_integral(PID_t *pid); - -__END_DECLS - -#endif /* PID_H_ */ diff --git a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt b/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt index 9645c7acf930..e0574682f18a 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt +++ b/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverAckermannControl RoverAckermannControl.cpp ) -target_link_libraries(RoverAckermannControl PUBLIC pid) +target_link_libraries(RoverAckermannControl PUBLIC PID) target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp index 5da47d449c8e..ad02e7914192 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp +++ b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp @@ -41,27 +41,19 @@ RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParam { updateParams(); _rover_ackermann_status_pub.advertise(); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - } void RoverAckermannControl::updateParams() { ModuleParams::updateParams(); - pid_set_parameters(&_pid_throttle, - _param_ra_speed_p.get(), // Proportional gain - _param_ra_speed_i.get(), // Integral gain - 0, // Derivative gain - _param_ra_speed_i.get() > FLT_EPSILON ? 1.f / _param_ra_speed_i.get() : 0.f, // Integral limit - 1); // Output limit + _pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f); + _pid_throttle.setIntegralLimit(1.f); + _pid_throttle.setOutputLimit(1.f); - pid_set_parameters(&_pid_lat_accel, - _param_ra_lat_accel_p.get(), // Proportional gain - _param_ra_lat_accel_i.get(), // Integral gain - 0, // Derivative gain - _param_ra_lat_accel_i.get() > FLT_EPSILON ? 1.f / _param_ra_lat_accel_i.get() : 0.f, // Integral limit - 1); // Output limit + _pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f); + _pid_lat_accel.setIntegralLimit(1.f); + _pid_lat_accel.setOutputLimit(1.f); // Update slew rates if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) { @@ -117,8 +109,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe if (sign(vehicle_forward_speed_temp) == 1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability) - steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint, - vehicle_lateral_acceleration, 0, dt); + _pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint); + steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt); } _rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(), @@ -156,8 +148,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe _rover_ackermann_status.steering_setpoint_normalized = steering_normalized; _rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState(); _rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration; - _rover_ackermann_status.pid_throttle_integral = _pid_throttle.integral * _param_ra_speed_i.get(); - _rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.integral * _param_ra_lat_accel_i.get(); + _rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral(); + _rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral(); _rover_ackermann_status_pub.publish(_rover_ackermann_status); // Publish to motor @@ -218,8 +210,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe -1.f, 1.f); } - forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(), - vehicle_forward_speed, 0, dt); // Feedback + _pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt); } return math::constrain(forward_speed_normalized, -1.f, 1.f); @@ -228,8 +220,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe void RoverAckermannControl::resetControllers() { - pid_reset_integral(&_pid_throttle); - pid_reset_integral(&_pid_lat_accel); + _pid_throttle.resetIntegral(); + _pid_lat_accel.resetIntegral(); _forward_speed_setpoint_with_accel_limit.setForcedValue(0.f); _steering_with_rate_limit.setForcedValue(0.f); diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp index a781ea79ca48..7f23caff4caf 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp +++ b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp @@ -46,7 +46,7 @@ #include // Standard libraries -#include +#include #include #include #include @@ -114,8 +114,8 @@ class RoverAckermannControl : public ModuleParams hrt_abstime _timestamp{0}; // Controllers - PID_t _pid_throttle; // The PID controller for the closed loop speed control - PID_t _pid_lat_accel; // The PID controller for the closed loop speed control + PID _pid_throttle; // The PID controller for the closed loop speed control + PID _pid_lat_accel; // The PID controller for the closed loop speed control SlewRate _steering_with_rate_limit{0.f}; SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 752b5b3417bf..cf6fa56fc17a 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include using namespace matrix; diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt index 58a5239f8cc7..7464de4f5b9f 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverDifferentialControl RoverDifferentialControl.cpp ) -target_link_libraries(RoverDifferentialControl PUBLIC pid) +target_link_libraries(RoverDifferentialControl PUBLIC PID) target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index deb0b9e37b82..e864e3e31f43 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -42,9 +42,6 @@ RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : Modul { updateParams(); _rover_differential_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); } void RoverDifferentialControl::updateParams() @@ -54,24 +51,15 @@ void RoverDifferentialControl::updateParams() _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; // Update PID - pid_set_parameters(&_pid_yaw_rate, - _param_rd_yaw_rate_p.get(), // Proportional gain - _param_rd_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_throttle, - _param_rd_p_gain_speed.get(), // Proportional gain - _param_rd_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_yaw, - _param_rd_p_gain_yaw.get(), // Proportional gain - _param_rd_i_gain_yaw.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit + _pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f); + _pid_throttle.setIntegralLimit(1.f); + _pid_throttle.setOutputLimit(1.f); + _pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); // Update slew rates if (_max_yaw_rate > FLT_EPSILON) { @@ -99,8 +87,10 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw); - _rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt); + _pid_yaw.setSetpoint( + matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - + vehicle_yaw)); // error as setpoint to take care of wrapping + _rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; } else { @@ -142,9 +132,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con _rover_differential_status.measured_forward_speed = vehicle_forward_speed; _rover_differential_status.measured_yaw = vehicle_yaw; _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - _rover_differential_status.pid_throttle_integral = _pid_throttle.integral; - _rover_differential_status.pid_yaw_integral = _pid_yaw.integral; + _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral(); + _rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral(); _rover_differential_status_pub.publish(_rover_differential_status); // Publish to motors @@ -158,7 +148,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID_t &pid_yaw_rate, const bool normalized) + PID &pid_yaw_rate, const bool normalized) { float slew_rate_normalization{1.f}; @@ -190,8 +180,8 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set max_thr_yaw_r, -1.f, 1.f); } - speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0, - dt); // Feedback + pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); } return math::constrain(speed_diff_normalized, -1.f, 1.f); @@ -200,7 +190,7 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, - PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) + PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) { float slew_rate_normalization{1.f}; @@ -242,8 +232,8 @@ float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_ -1.f, 1.f); } - forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(), - vehicle_forward_speed, 0, dt); // Feedback + pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt); } return math::constrain(forward_speed_normalized, -1.f, 1.f); @@ -267,7 +257,7 @@ matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forwar void RoverDifferentialControl::resetControllers() { - pid_reset_integral(&_pid_throttle); - pid_reset_integral(&_pid_yaw_rate); - pid_reset_integral(&_pid_yaw); + _pid_throttle.resetIntegral(); + _pid_yaw_rate.resetIntegral(); + _pid_yaw.resetIntegral(); } diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp index 5a445e788234..e08153356b73 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -47,7 +47,7 @@ #include // Standard libraries -#include +#include #include #include #include @@ -100,7 +100,7 @@ class RoverDifferentialControl : public ModuleParams * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. */ float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized); + float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); /** * @brief Compute normalized forward speed setpoint and apply slew rates. * to the forward speed setpoint and doing closed loop speed control if enabled. @@ -116,7 +116,7 @@ class RoverDifferentialControl : public ModuleParams * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. */ float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel, + SlewRate &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, float dt, bool normalized); /** @@ -142,9 +142,9 @@ class RoverDifferentialControl : public ModuleParams float _max_yaw_accel{0.f}; // Controllers - PID_t _pid_throttle; // Closed loop speed control - PID_t _pid_yaw; // Closed loop yaw control - PID_t _pid_yaw_rate; // Closed loop yaw rate control + PID _pid_throttle; // Closed loop speed control + PID _pid_yaw; // Closed loop yaw control + PID _pid_yaw_rate; // Closed loop yaw rate control SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; SlewRate _yaw_rate_with_accel_limit{0.f}; SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index e76124aca9a1..b2d89efbd31a 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -53,7 +53,7 @@ #include // Standard libraries -#include +#include #include // Local includes diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt index 9777ae5d18e4..61b64980f139 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt +++ b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverMecanumControl RoverMecanumControl.cpp ) -target_link_libraries(RoverMecanumControl PUBLIC pid) +target_link_libraries(RoverMecanumControl PUBLIC PID) target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index 9f52ad175625..87c9f86079c4 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -42,40 +42,28 @@ RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(pa { updateParams(); _rover_mecanum_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_forward_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); } void RoverMecanumControl::updateParams() { ModuleParams::updateParams(); + + _pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + + _pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); + _pid_forward_throttle.setIntegralLimit(1.f); + _pid_forward_throttle.setOutputLimit(1.f); + + _pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); + _pid_lateral_throttle.setIntegralLimit(1.f); + _pid_lateral_throttle.setOutputLimit(1.f); + _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; - pid_set_parameters(&_pid_yaw_rate, - _param_rm_yaw_rate_p.get(), // Proportional gain - _param_rm_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_forward_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_lateral_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_yaw, - _param_rm_p_gain_yaw.get(), // Proportional gain - _param_rm_i_gain_yaw.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit + _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); } void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, @@ -91,11 +79,12 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl // Closed loop yaw control if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { - const float heading_error = matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw); - _rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0.f, 0.f, dt); + _pid_yaw.setSetpoint( + matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw)); // error as setpoint to take care of wrapping + _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); } else { - pid_reset_integral(&_pid_yaw); + _pid_yaw.resetIntegral(); } // Yaw rate control @@ -108,8 +97,9 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); } + _pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint); speed_diff_normalized = math::constrain(speed_diff_normalized + - pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt), + _pid_yaw_rate.update(vehicle_yaw_rate, dt), -1.f, 1.f); // Feedback } else { // Use normalized setpoint @@ -129,8 +119,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); } - forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint, - vehicle_forward_speed, 0, dt); // Feedback + _pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint); + forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt); // Closed loop lateral speed control if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward @@ -138,8 +128,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); } - lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint, - vehicle_lateral_speed, 0, dt); // Feedback + _pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint); + lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt); } else { // Use normalized setpoint forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain( @@ -156,10 +146,10 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; rover_mecanum_status.measured_yaw = vehicle_yaw; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral; - rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral; - rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral; + rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); + rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); + rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); _rover_mecanum_status_pub.publish(rover_mecanum_status); // Publish to motors @@ -213,8 +203,8 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr void RoverMecanumControl::resetControllers() { - pid_reset_integral(&_pid_forward_throttle); - pid_reset_integral(&_pid_lateral_throttle); - pid_reset_integral(&_pid_yaw_rate); - pid_reset_integral(&_pid_yaw); + _pid_forward_throttle.resetIntegral(); + _pid_lateral_throttle.resetIntegral(); + _pid_yaw_rate.resetIntegral(); + _pid_yaw.resetIntegral(); } diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp index 27a084934a0c..42cec627a077 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -46,7 +46,7 @@ // Standard libraries -#include +#include #include #include #include @@ -112,10 +112,10 @@ class RoverMecanumControl : public ModuleParams float _max_yaw_rate{0.f}; // Controllers - PID_t _pid_forward_throttle; // PID for the closed loop forward speed control - PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control - PID_t _pid_yaw; // PID for the closed loop yaw control - PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control + PID _pid_forward_throttle; // PID for the closed loop forward speed control + PID _pid_lateral_throttle; // PID for the closed loop lateral speed control + PID _pid_yaw; // PID for the closed loop yaw control + PID _pid_yaw_rate; // PID for the closed loop yaw rate control // Parameters DEFINE_PARAMETERS( diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 5e093862e768..ee85e3cc3942 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -38,5 +38,5 @@ px4_add_module( RoverPositionControl.hpp DEPENDS l1 - pid + PID ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 928ce13c6edb..7466077a9334 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -92,13 +92,9 @@ void RoverPositionControl::parameters_update(bool force) _gnd_control.set_l1_damping(_param_l1_damping.get()); _gnd_control.set_l1_period(_param_l1_period.get()); - pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f); - pid_set_parameters(&_speed_ctrl, - _param_speed_p.get(), - _param_speed_i.get(), - _param_speed_d.get(), - _param_speed_imax.get(), - _param_gndspeed_max.get()); + _speed_ctrl.setGains(_param_speed_p.get(), _param_speed_i.get(), _param_speed_d.get()); + _speed_ctrl.setIntegralLimit(_param_speed_imax.get()); + _speed_ctrl.setOutputLimit(_param_gndspeed_max.get()); } } @@ -239,12 +235,9 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); - const float x_vel = vel(0); - const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; - // Compute airspeed control out and just scale it as a constant - mission_throttle = _param_throttle_speed_scaler.get() - * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt); + _speed_ctrl.setSetpoint(mission_target_speed); + mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt); // Constrain throttle between min and max mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get()); @@ -327,10 +320,8 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2)); - const float x_vel = vel(0); - const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; - - const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); + _speed_ctrl.setSetpoint(desired_speed); + const float control_throttle = _speed_ctrl.update(vel(0), dt); //Constrain maximum throttle to mission throttle _throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle); @@ -392,8 +383,6 @@ RoverPositionControl::Run() vehicle_attitude_poll(); manual_control_setpoint_poll(); - _vehicle_acceleration_sub.update(); - /* update parameters from storage */ parameters_update(); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index ee1c65efa03d..d3a26438e21d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -128,8 +128,6 @@ class RoverPositionControl final : public ModuleBase, publ uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; - uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**, publ /* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/ - PID_t _speed_ctrl{}; + PID _speed_ctrl{}; // estimator reset counters uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index d04f21cb1103..7559ef4701f4 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index 4d94ac9cd956..192365a98d53 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.hpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include From f9bcbc31aed8aa21d2a5372b50098307b0d9d8cc Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 26 Nov 2024 13:49:30 +0100 Subject: [PATCH 065/211] PID: protect from division by zero because of dt Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> --- src/lib/pid/PID.cpp | 14 ++++++++++++-- src/lib/pid/PID.hpp | 1 + 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp index 3f0d9b4b9385..6fbb34635178 100644 --- a/src/lib/pid/PID.cpp +++ b/src/lib/pid/PID.cpp @@ -44,8 +44,7 @@ void PID::setGains(const float P, const float I, const float D) float PID::update(const float feedback, const float dt, const bool update_integral) { const float error = _setpoint - feedback; - const float feedback_change = std::isfinite(_last_feedback) ? (feedback - _last_feedback) / dt : 0.f; - const float output = (_gain_proportional * error) + _integral + (_gain_derivative * feedback_change); + const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt)); if (update_integral) { updateIntegral(error, dt); @@ -63,3 +62,14 @@ void PID::updateIntegral(float error, const float dt) _integral = math::constrain(integral_new, -_limit_integral, _limit_integral); } } + +float PID::updateDerivative(float feedback, const float dt) +{ + float feedback_change = 0.f; + + if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) { + feedback_change = (feedback - _last_feedback) / dt; + } + + return feedback_change; +} diff --git a/src/lib/pid/PID.hpp b/src/lib/pid/PID.hpp index cabdd3701b8a..615fd60b220f 100644 --- a/src/lib/pid/PID.hpp +++ b/src/lib/pid/PID.hpp @@ -50,6 +50,7 @@ class PID void resetDerivative() { _last_feedback = NAN; }; private: void updateIntegral(float error, const float dt); + float updateDerivative(float feedback, const float dt); float _setpoint{0.f}; ///< current setpoint to track float _integral{0.f}; ///< integral state From a280d67be830465b74fc757ed39ff08d09ad9550 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 26 Nov 2024 14:03:31 +0100 Subject: [PATCH 066/211] PID: Fix test to respect integral updates being applied in the next iteration Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> --- src/lib/pid/PIDTest.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/pid/PIDTest.cpp b/src/lib/pid/PIDTest.cpp index ed3c204e6ef8..ae7d847bde72 100644 --- a/src/lib/pid/PIDTest.cpp +++ b/src/lib/pid/PIDTest.cpp @@ -125,5 +125,6 @@ TEST(PIDTest, InteralOpenLoop) EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f); pid.resetIntegral(); + EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f); EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f); } From 1dad25b7636fac86cf84175ae327ef64ba9ad754 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Nov 2024 14:46:23 +0100 Subject: [PATCH 067/211] SIH: do not assume being a tailsitter when creating airspeed measurement --- src/modules/simulation/simulator_sih/sih.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 2ca4987338e8..aa1de1e5e968 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -450,8 +450,7 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; - // airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter - airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f); + airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; From 7236ef2d171f15c37fa35aeafccb2dbf6fb2a5b8 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Nov 2024 15:31:30 +0100 Subject: [PATCH 068/211] SIH-FW: fix aileron and elevator signs This broken when changing from mixer files to the control allocation module. --- src/modules/simulation/simulator_sih/sih.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index aa1de1e5e968..3210c33ed0b3 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -327,9 +327,9 @@ void Sih::generate_fw_aerodynamics() { _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] float altitude = _H0 - _p_I(2); - _wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX); - _wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); - _tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]); + _wing_l.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); + _wing_r.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX); + _tailplane.update_aero(_v_B, _w_B, altitude, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); _fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]); _fuselage.update_aero(_v_B, _w_B, altitude); From 8a9bac29a2962b36570e2991343d9f3e8d18a963 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Nov 2024 15:34:46 +0100 Subject: [PATCH 069/211] SIH-FW: allow pitching up during takeoff Otherwise difficult to get lift --- src/modules/simulation/simulator_sih/sih.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 3210c33ed0b3..a184f1ac9912 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -409,11 +409,9 @@ void Sih::equations_of_motion(const float dt) // integration: Euler forward _p_I = _p_I + _p_I_dot * dt; _v_I = _v_I + _v_I_dot * dt; - Eulerf RPY = Eulerf(_q); - RPY(0) = 0.0f; // no roll - RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift - _q = Quatf(RPY); - _w_B.setZero(); + _q = _q * _dq; + _q.normalize(); + _w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); _grounded = true; } From 85bc8ef885a197d04230a70b02809ba7f39463f3 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 18:10:15 +0100 Subject: [PATCH 070/211] SIH-plane: fix actuator mapping --- .../init.d-posix/airframes/10041_sihsim_airplane | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index fa132e6d6d11..000ac9766b94 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -45,7 +45,7 @@ param set-default CA_SV_CS1_TRQ_P 1 param set-default CA_SV_CS1_TYPE 3 param set-default CA_SV_CS2_TRQ_Y 1 param set-default CA_SV_CS2_TYPE 4 -param set-default PWM_MAIN_FUNC3 201 -param set-default PWM_MAIN_FUNC4 202 -param set-default PWM_MAIN_FUNC5 203 -param set-default PWM_MAIN_FUNC6 101 +param set-default PWM_MAIN_FUNC1 201 +param set-default PWM_MAIN_FUNC2 202 +param set-default PWM_MAIN_FUNC3 203 +param set-default PWM_MAIN_FUNC4 101 From f2bbb6f4073ae5b0076d869617cf784504062e95 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 25 Nov 2024 09:45:47 -0800 Subject: [PATCH 071/211] ci: disable publishing PR images to docker hub Docker hub is rate limiting our API access, as a result tests are failing for no apparent reason. This change will decrease the API calls by at least 80% We have applied for an Open Source account with greater API limits, I will come back to this and update as necessary when and if they grant us access to their program. --- .github/workflows/dev_container.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index cf88648207b7..fa0092565959 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -27,6 +27,7 @@ jobs: - name: Login to Docker Hub uses: docker/login-action@v3 + if: github.event_name != 'pull_request' with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} @@ -44,7 +45,7 @@ jobs: with: images: | ghcr.io/PX4/px4-dev - px4io/px4-dev + ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }} tags: | type=schedule type=semver,pattern={{version}} From 22c1f07f0ca7badb8143fdce42a2dd329de1ba3f Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 27 Nov 2024 08:59:30 -0800 Subject: [PATCH 072/211] container: use PX4 tags whiel tagging images --- .github/workflows/dev_container.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index fa0092565959..6e5c1cad32ee 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -25,6 +25,11 @@ jobs: submodules: false fetch-depth: 0 + - name: Set PX4 Tag + id: px4-tag + run: | + echo "tag=$(git describe --tags --match 'v[0-9]*')" >> $GITHUB_OUTPUT + - name: Login to Docker Hub uses: docker/login-action@v3 if: github.event_name != 'pull_request' @@ -47,14 +52,13 @@ jobs: ghcr.io/PX4/px4-dev ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }} tags: | - type=schedule type=semver,pattern={{version}} type=semver,pattern={{major}}.{{minor}} type=semver,pattern={{major}} - type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600 + type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700 + type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600 type=ref,event=branch,suffix=,priority=500 type=ref,event=pr - type=sha - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 From 68cbbaab92e8409c7d88ba33f1e9a40f7ae5a16f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 27 Nov 2024 16:48:11 -0500 Subject: [PATCH 073/211] Tools/astyle: check_code_style_all.sh skip pre-commit hook if non-interactive --- Tools/astyle/check_code_style_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/astyle/check_code_style_all.sh b/Tools/astyle/check_code_style_all.sh index 5ace9ea52b10..c4f4669003a3 100755 --- a/Tools/astyle/check_code_style_all.sh +++ b/Tools/astyle/check_code_style_all.sh @@ -43,7 +43,7 @@ fi # install git pre-commit hook HOOK_FILE="$DIR/../../.git/hooks/pre-commit" -if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then +if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then echo "" echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m" echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]" From 990b067b25bfeb5680c1a7e2253a0fbcd00db20a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 27 Nov 2024 16:56:02 -0500 Subject: [PATCH 074/211] uxrce_dds_client: update cmake requirements to match Micro-XRCE-DDS-Client submodule --- src/modules/uxrce_dds_client/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uxrce_dds_client/CMakeLists.txt b/src/modules/uxrce_dds_client/CMakeLists.txt index ed6c576fe5cc..acbc56c5a6b8 100644 --- a/src/modules/uxrce_dds_client/CMakeLists.txt +++ b/src/modules/uxrce_dds_client/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -if(${CMAKE_VERSION} VERSION_LESS "3.11") +if(${CMAKE_VERSION} VERSION_LESS_EQUAL "3.15") message(WARNING "skipping uxrce_dds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}") else() From 1b6215fcf3517c70ea4880473cdd815013f70338 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Fri, 22 Nov 2024 13:42:57 -0800 Subject: [PATCH 075/211] readme: update ci badge --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e2e62c11c60e..aef9e8f8a592 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot) -[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22) +[![Build Targets](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml/badge.svg?branch=main)](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22) [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) From f7b62961cbb40a854ec72f7ff95dfbe692013175 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Nov 2024 11:23:25 +1300 Subject: [PATCH 076/211] drivers: Copy SPL06 to SPA06 --- src/drivers/barometer/Kconfig | 1 + src/drivers/barometer/goertek/CMakeLists.txt | 3 +- .../barometer/goertek/spa06/CMakeLists.txt | 45 ++++ src/drivers/barometer/goertek/spa06/Kconfig | 5 + src/drivers/barometer/goertek/spa06/SPA06.cpp | 248 ++++++++++++++++++ src/drivers/barometer/goertek/spa06/SPA06.hpp | 120 +++++++++ .../barometer/goertek/spa06/SPA06_I2C.cpp | 102 +++++++ .../barometer/goertek/spa06/SPA06_SPI.cpp | 104 ++++++++ .../barometer/goertek/spa06/parameters.c | 41 +++ src/drivers/barometer/goertek/spa06/spa06.h | 108 ++++++++ .../barometer/goertek/spa06/spa06_main.cpp | 141 ++++++++++ src/drivers/drv_sensor.h | 1 + 12 files changed, 918 insertions(+), 1 deletion(-) create mode 100644 src/drivers/barometer/goertek/spa06/CMakeLists.txt create mode 100644 src/drivers/barometer/goertek/spa06/Kconfig create mode 100644 src/drivers/barometer/goertek/spa06/SPA06.cpp create mode 100644 src/drivers/barometer/goertek/spa06/SPA06.hpp create mode 100644 src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp create mode 100644 src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp create mode 100644 src/drivers/barometer/goertek/spa06/parameters.c create mode 100644 src/drivers/barometer/goertek/spa06/spa06.h create mode 100644 src/drivers/barometer/goertek/spa06/spa06_main.cpp diff --git a/src/drivers/barometer/Kconfig b/src/drivers/barometer/Kconfig index da9c2dd7143a..868b55262131 100644 --- a/src/drivers/barometer/Kconfig +++ b/src/drivers/barometer/Kconfig @@ -11,6 +11,7 @@ menu "barometer" select DRIVERS_BAROMETER_MS5611 select DRIVERS_BAROMETER_MAIERTEK_MPC2520 select DRIVERS_BAROMETER_GOERTEK_SPL06 + select DRIVERS_BAROMETER_GOERTEK_SPA06 select DRIVERS_BAROMETER_INVENSENSE_ICP101XX select DRIVERS_BAROMETER_INVENSENSE_ICP201XX ---help--- diff --git a/src/drivers/barometer/goertek/CMakeLists.txt b/src/drivers/barometer/goertek/CMakeLists.txt index 867eb64fcaa0..b626766ddd9a 100644 --- a/src/drivers/barometer/goertek/CMakeLists.txt +++ b/src/drivers/barometer/goertek/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# Copyright (c) 2022-2024 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 @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(spl06) +add_subdirectory(spa06) diff --git a/src/drivers/barometer/goertek/spa06/CMakeLists.txt b/src/drivers/barometer/goertek/spa06/CMakeLists.txt new file mode 100644 index 000000000000..572b5dab2a1b --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2024 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__barometer__spa06 + MAIN spa06 + SRCS + SPA06.cpp + SPA06.hpp + SPA06_I2C.cpp + SPA06_SPI.cpp + spa06_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/goertek/spa06/Kconfig b/src/drivers/barometer/goertek/spa06/Kconfig new file mode 100644 index 000000000000..7190a27defaa --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_GOERTEK_SPA06 + bool "spa06" + default n + ---help--- + Enable support for spa06 diff --git a/src/drivers/barometer/goertek/spa06/SPA06.cpp b/src/drivers/barometer/goertek/spa06/SPA06.cpp new file mode 100644 index 000000000000..fb81db54825e --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06.cpp @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SPA06.hpp" + +SPA06::SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface) : + I2CSPIDriver(config), + _interface(interface), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) +{ +} + +SPA06::~SPA06() +{ + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + + delete _interface; +} + +/* +float +SPA06::scale_factor(int oversampling_rate) +{ + float k; + + switch (oversampling_rate) { + case 1: + k = 524288.0f; + break; + + case 2: + k = 1572864.0f; + break; + + case 4: + k = 3670016.0f; + break; + + case 8: + k = 7864320.0f; + break; + + case 16: + k = 253952.0f; + break; + + case 32: + k = 516096.0f; + break; + + case 64: + k = 1040384.0f; + break; + + case 128: + k = 2088960.0f; + break; + + default: + k = 0; + break; + } + + return k; +} +*/ + +int +SPA06::calibrate() +{ + uint8_t buf[18]; + + _interface->read(SPA06_ADDR_CAL, buf, sizeof(buf)); + + _cal.c0 = (uint16_t)buf[0] << 4 | (uint16_t)buf[1] >> 4; + _cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0; + + _cal.c1 = (uint16_t)(buf[1] & 0x0f) << 8 | (uint16_t)buf[2]; + _cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1; + + _cal.c00 = (uint32_t)buf[3] << 12 | (uint32_t)buf[4] << 4 | (uint16_t)buf[5] >> 4; + _cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00; + + _cal.c10 = (uint32_t)(buf[5] & 0x0f) << 16 | (uint32_t)buf[6] << 8 | (uint32_t)buf[7]; + _cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10; + + _cal.c01 = (uint16_t)buf[8] << 8 | buf[9]; + _cal.c11 = (uint16_t)buf[10] << 8 | buf[11]; + _cal.c20 = (uint16_t)buf[12] << 8 | buf[13]; + _cal.c21 = (uint16_t)buf[14] << 8 | buf[15]; + _cal.c30 = (uint16_t)buf[16] << 8 | buf[17]; + + // PX4_INFO("c0:%d \nc1:%d \nc00:%d \nc10:%d \nc01:%d \nc11:%d \nc20:%d \nc21:%d \nc30:%d\n", + // _cal.c0,_cal.c1, + // _cal.c00,_cal.c10, + // _cal.c01,_cal.c11,_cal.c20,_cal.c21,_cal.c30 + // ); + //PX4_DEBUG("c0:%f",_cal.c0); + return OK; +} +int +SPA06::init() +{ + int8_t tries = 5; + // reset sensor + _interface->set_reg(SPA06_VALUE_RESET, SPA06_ADDR_RESET); + usleep(10000); + + // check id + if (_interface->get_reg(SPA06_ADDR_ID) != SPA06_VALUE_ID) { + PX4_DEBUG("id of your baro is not: 0x%02x", SPA06_VALUE_ID); + return -EIO; + } + + while (tries--) { + uint8_t meas_cfg = _interface->get_reg(SPA06_ADDR_MEAS_CFG); + + if (meas_cfg & (1 << 7) && meas_cfg & (1 << 6)) { + break; + } + + usleep(10000); + } + + if (tries < 0) { + PX4_DEBUG("spa06 cal failed"); + return -EIO; + } + + // get calibration and pre process them + calibrate(); + + // set config, recommended settings + _interface->set_reg(_curr_prs_cfg, SPA06_ADDR_PRS_CFG); + kp = 253952.0f; // refer to scale_factor() + _interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG); + kt = 524288.0f; + + + _interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG); + _interface->set_reg(7, SPA06_ADDR_MEAS_CFG); + + Start(); + + return OK; +} + +void +SPA06::Start() +{ + // schedule a cycle to start things + ScheduleNow(); +} + +void +SPA06::RunImpl() +{ + collect(); + + ScheduleDelayed(_measure_interval); +} +int +SPA06::collect() +{ + perf_begin(_sample_perf); + + // this should be fairly close to the end of the conversion, so the best approximation of the time + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (_interface->read(SPA06_ADDR_DATA, (uint8_t *)&_data, sizeof(_data)) != OK) { + perf_count(_comms_errors); + perf_cancel(_sample_perf); + return -EIO; + } + + int32_t temp_raw = (uint32_t)_data.t_msb << 16 | (uint32_t)_data.t_lsb << 8 | (uint32_t)_data.t_xlsb; + temp_raw = (temp_raw & 1 << 23) ? (0xff000000 | temp_raw) : temp_raw; + + int32_t press_raw = (uint32_t)_data.p_msb << 16 | (uint32_t) _data.p_lsb << 8 | (uint32_t) _data.p_xlsb; + press_raw = (press_raw & 1 << 23) ? (0xff000000 | press_raw) : press_raw; + + // calculate + float ftsc = (float)temp_raw / kt; + float fpsc = (float)press_raw / kp; + float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * (float)_cal.c30); + float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * (float)_cal.c21); + + float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3; + float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc; + + + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = fp; + sensor_baro.temperature = temperature; + sensor_baro.error_count = perf_event_count(_comms_errors); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + perf_end(_sample_perf); + + return OK; +} + +void +SPA06::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_measure_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/barometer/goertek/spa06/SPA06.hpp b/src/drivers/barometer/goertek/spa06/SPA06.hpp new file mode 100644 index 000000000000..565e572a3e79 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "spa06.h" + +#include +#include +#include +#include +#include +#include +#include + +class SPA06 : public I2CSPIDriver +{ +public: + SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface); + virtual ~SPA06(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + int init(); + void print_status(); + + void RunImpl(); +private: + void Start(); + // float scale_factor(int oversampling_rate); + + int collect(); //get results and publish + int calibrate(); + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + + spa06::ISPA06 *_interface; + spa06::data_s _data; + spa06::calibration_s _cal{}; + + // set config, recommended settings + // + // oversampling rate : single | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // scale factor(KP/KT): 524288 | 1572864 | 3670016 | 7864320 | 253952 | 516096 | 1040384 | 2088960 + + // configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC) + // + // bit[7]: reserved + // + // PM_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // note: applicable for measurements in background mode only + // + // PM_PRC[3:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // measurement time(ms): 3.6 | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8 + // precision(PaRMS) : 5.0 | | 2.5 | | 1.2 | 0.9 | 0.5 | + // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register + static constexpr uint8_t _curr_prs_cfg{4 << 4 | 4}; + + // configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC) + // + // temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element) + // TMP_EXT[7] : 0 | 1 + // note: it is highly recommended to use the same temperature sensor as the source of the calibration coefficients wihch can be read from reg 0x28 + // + // TMP_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // note: applicable for measurements in background mode only + // + // bit[3]: reserved + // + // TMP_PRC[2:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant + // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register + static constexpr uint8_t _curr_tmp_cfg{1 << 7 | 4 << 4 | 0}; + + bool _collect_phase{false}; + float kp; + float kt; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + + static constexpr uint32_t _sample_rate{16}; + static constexpr uint32_t _measure_interval{1000000 / _sample_rate / 2}; +}; diff --git a/src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp b/src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp new file mode 100644 index 000000000000..e3e6044037c9 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 SPA06_I2C.cpp + * + * SPI interface for Goertek SPA06 + */ + +#include "spa06.h" + +#include +#include + +#if defined(CONFIG_I2C) + +class SPA06_I2C: public device::I2C, public spa06::ISPA06 +{ +public: + SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency); + virtual ~SPA06_I2C() override = default; + + int init() override { return I2C::init(); } + + uint8_t get_reg(uint8_t addr) override; + int set_reg(uint8_t value, uint8_t addr) override; + + int read(uint8_t addr, uint8_t *buf, uint8_t len) override; + //spa06::data_s *get_data(uint8_t addr) override; + //spa06::calibration_s *get_calibration(uint8_t addr) override; + + uint32_t get_device_id() const override { return device::I2C::get_device_id(); } + + uint8_t get_device_address() const override { return device::I2C::get_device_address(); } +private: + spa06::calibration_s _cal{}; + spa06::data_s _data{}; +}; + +spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency) +{ + return new SPA06_I2C(busnum, device, bus_frequency); +} + +SPA06_I2C::SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency) : + I2C(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, bus_frequency) +{ +} + +uint8_t +SPA06_I2C::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), 0}; + transfer(&cmd[0], 1, &cmd[1], 1); + + return cmd[1]; +} + +int +SPA06_I2C::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), value}; + return transfer(cmd, sizeof(cmd), nullptr, 0); +} + +int +SPA06_I2C::read(uint8_t addr, uint8_t *buf, uint8_t len) +{ + return transfer(&addr, 1, buf, len); +} + +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp b/src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp new file mode 100644 index 000000000000..80c6112090fb --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 SPA06_SPI.cpp + * + * SPI interface for Goertek SPA06 + */ + +#include "spa06.h" + +#include +#include + +#if defined(CONFIG_SPI) + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) //for set +#define DIR_WRITE ~(1<<7) //for clear + +class SPA06_SPI: public device::SPI, public spa06::ISPA06 +{ +public: + SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + virtual ~SPA06_SPI() override = default; + + int init() override { return SPI::init(); } + + uint8_t get_reg(uint8_t addr) override; + int set_reg(uint8_t value, uint8_t addr) override; + + int read(uint8_t addr, uint8_t *buf, uint8_t len) override; + + uint32_t get_device_id() const override { return device::SPI::get_device_id(); } + + uint8_t get_device_address() const override { return device::SPI::get_device_address(); } +}; + +spa06::ISPA06 * +spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode) +{ + return new SPA06_SPI(busnum, device, bus_frequency, spi_mode); +} + +SPA06_SPI::SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, spi_mode, bus_frequency) +{ +} + +uint8_t +SPA06_SPI::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; // set MSB bit + transfer(&cmd[0], &cmd[0], 2); + + return cmd[1]; +} + +int +SPA06_SPI::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; // clear MSB bit + return transfer(&cmd[0], nullptr, 2); +} + +int +SPA06_SPI::read(uint8_t addr, uint8_t *buf, uint8_t len) +{ + uint8_t tx_buf[len + 1] = {(uint8_t)(addr | DIR_READ)}; // GCC support VLA, let's use it + + return transfer(tx_buf, buf, len); +} + +#endif // CONFIG_SPI diff --git a/src/drivers/barometer/goertek/spa06/parameters.c b/src/drivers/barometer/goertek/spa06/parameters.c new file mode 100644 index 000000000000..a7853291310f --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * Goertek SPA06 Barometer (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SPA06, 0); diff --git a/src/drivers/barometer/goertek/spa06/spa06.h b/src/drivers/barometer/goertek/spa06/spa06.h new file mode 100644 index 000000000000..b31276af8761 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/spa06.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 spa06.h + * + * Shared defines for the spa06 driver. + */ +#pragma once + +#include + +#define SPA06_ADDR_ID 0x0d +#define SPA06_ADDR_RESET 0x0c // set to reset +#define SPA06_ADDR_CAL 0x10 +#define SPA06_ADDR_PRS_CFG 0x06 +#define SPA06_ADDR_TMP_CFG 0x07 +#define SPA06_ADDR_MEAS_CFG 0x08 +#define SPA06_ADDR_CFG_REG 0x09 +#define SPA06_ADDR_DATA 0x00 + + +#define SPA06_VALUE_RESET 9 +#define SPA06_VALUE_ID 0x10 + +namespace spa06 +{ + +#pragma pack(push,1) +struct calibration_s { + int16_t c0, c1; + int32_t c00, c10; + int16_t c01, c11, c20, c21, c30; +}; //calibration data + +struct data_s { + uint8_t p_msb; + uint8_t p_lsb; + uint8_t p_xlsb; + + uint8_t t_msb; + uint8_t t_lsb; + uint8_t t_xlsb; +}; // data +#pragma pack(pop) + +class ISPA06 +{ +public: + virtual ~ISPA06() = default; + + virtual int init() = 0; + + // read reg value + virtual uint8_t get_reg(uint8_t addr) = 0; + + // write reg value + virtual int set_reg(uint8_t value, uint8_t addr) = 0; + + // bulk read of data into buffer, return same pointer + virtual int read(uint8_t addr, uint8_t *buf, uint8_t len) = 0; + // bulk read of calibration data into buffer, return same pointer + + virtual uint32_t get_device_id() const = 0; + + virtual uint8_t get_device_address() const = 0; +}; + +} /* namespace */ + + +/* interface factories */ +#if defined(CONFIG_SPI) +extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) +extern spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency); +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/goertek/spa06/spa06_main.cpp b/src/drivers/barometer/goertek/spa06/spa06_main.cpp new file mode 100644 index 000000000000..848b1f19a712 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/spa06_main.cpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +#include "SPA06.hpp" + +#include + +extern "C" { __EXPORT int spa06_main(int argc, char *argv[]); } + +void +SPA06::print_usage() +{ + PRINT_MODULE_USAGE_NAME("spa06", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); +#if defined(CONFIG_I2C) + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); +#else + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); +#endif + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *SPA06::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + spa06::ISPA06 *interface = nullptr; + +#if defined(CONFIG_I2C) + + if (config.bus_type == BOARD_I2C_BUS) { + interface = spa06_i2c_interface(config.bus, config.i2c_address, config.bus_frequency); + + } + +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + + if (config.bus_type == BOARD_SPI_BUS) { + interface = spa06_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); + } + +#endif // CONFIG_SPI + + if (interface == nullptr) { + PX4_ERR("failed creating interface for bus %i", config.bus); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i", config.bus); + return nullptr; + } + + SPA06 *dev = new SPA06(config, interface); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +int +spa06_main(int argc, char *argv[]) +{ + using ThisDriver = SPA06; + BusCLIArguments cli{true, true}; +#if defined(CONFIG_I2C) + cli.i2c_address = 0x76; + cli.default_i2c_frequency = 100 * 1000; +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + cli.default_spi_frequency = 10 * 1000 * 1000; +#endif // CONFIG_SPI + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_SPA06); + + 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/drv_sensor.h b/src/drivers/drv_sensor.h index 904d63aeba7b..8b33acb0f1ad 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -247,6 +247,7 @@ #define DRV_DIFF_PRESS_DEVTYPE_AUAV 0xE6 #define DRV_BARO_DEVTYPE_AUAV 0xE7 +#define DRV_BARO_DEVTYPE_SPA06 0xE8 #define DRV_DEVTYPE_UNUSED 0xff From def6ab5a6b54bc9dd9c81082f669667862f2697a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Nov 2024 15:02:32 +1300 Subject: [PATCH 077/211] drivers: add SPA06 --- ROMFS/px4fmu_common/init.d/rc.sensors | 7 +++ src/drivers/barometer/goertek/spa06/SPA06.cpp | 53 +++++++++++-------- src/drivers/barometer/goertek/spa06/SPA06.hpp | 27 +++++----- src/drivers/barometer/goertek/spa06/spa06.h | 13 +++-- 4 files changed, 57 insertions(+), 43 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index b85f36e09de3..7f479a3b1f02 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -211,6 +211,13 @@ then spl06 -X -a 0x77 start fi +# SPA06 sensor external I2C +if param compare -s SENS_EN_SPA06 1 +then + spa06 -X start + spa06 -X -a 0x77 start +fi + # PCF8583 counter (RPM sensor) if param compare -s SENS_EN_PCF8583 1 then diff --git a/src/drivers/barometer/goertek/spa06/SPA06.cpp b/src/drivers/barometer/goertek/spa06/SPA06.cpp index fb81db54825e..a1b1b4118fc5 100644 --- a/src/drivers/barometer/goertek/spa06/SPA06.cpp +++ b/src/drivers/barometer/goertek/spa06/SPA06.cpp @@ -103,34 +103,44 @@ SPA06::scale_factor(int oversampling_rate) int SPA06::calibrate() { - uint8_t buf[18]; + uint8_t buf[21]; _interface->read(SPA06_ADDR_CAL, buf, sizeof(buf)); - _cal.c0 = (uint16_t)buf[0] << 4 | (uint16_t)buf[1] >> 4; + _cal.c0 = (uint16_t)(buf[0]) << 4 | (uint16_t)(buf[1]) >> 4; + // If value is negative, we need to fill the missing bits. _cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0; - _cal.c1 = (uint16_t)(buf[1] & 0x0f) << 8 | (uint16_t)buf[2]; + _cal.c1 = (uint16_t)(buf[1] & 0x0F) << 8 | buf[2]; _cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1; - _cal.c00 = (uint32_t)buf[3] << 12 | (uint32_t)buf[4] << 4 | (uint16_t)buf[5] >> 4; + _cal.c00 = (uint32_t)(buf[3]) << 12 | (uint32_t)(buf[4]) << 4 | (buf[5]) >> 4; _cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00; - _cal.c10 = (uint32_t)(buf[5] & 0x0f) << 16 | (uint32_t)buf[6] << 8 | (uint32_t)buf[7]; + _cal.c10 = (uint32_t)(buf[5] & 0x0F) << 16 | (uint32_t)(buf[6]) << 8 | buf[7]; _cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10; - _cal.c01 = (uint16_t)buf[8] << 8 | buf[9]; - _cal.c11 = (uint16_t)buf[10] << 8 | buf[11]; - _cal.c20 = (uint16_t)buf[12] << 8 | buf[13]; - _cal.c21 = (uint16_t)buf[14] << 8 | buf[15]; - _cal.c30 = (uint16_t)buf[16] << 8 | buf[17]; - - // PX4_INFO("c0:%d \nc1:%d \nc00:%d \nc10:%d \nc01:%d \nc11:%d \nc20:%d \nc21:%d \nc30:%d\n", - // _cal.c0,_cal.c1, - // _cal.c00,_cal.c10, - // _cal.c01,_cal.c11,_cal.c20,_cal.c21,_cal.c30 - // ); - //PX4_DEBUG("c0:%f",_cal.c0); + _cal.c01 = (uint16_t)(buf[8]) << 8 | buf[9]; + + _cal.c11 = (uint16_t)(buf[10]) << 8 | buf[11]; + + _cal.c20 = (uint16_t)(buf[12]) << 8 | buf[13]; + + _cal.c21 = (uint16_t)(buf[14]) << 8 | buf[15]; + + _cal.c30 = (uint16_t)(buf[16]) << 8 | buf[17]; + + _cal.c31 = (uint16_t)(buf[18]) << 4 | (uint16_t)(buf[19] & 0xF0) >> 4; + _cal.c31 = (_cal.c31 & 1 << 11) ? (0xf000 | _cal.c31) : _cal.c31; + + _cal.c40 = (uint16_t)(buf[19] & 0x0F) << 8 | buf[20]; + _cal.c40 = (_cal.c40 & 1 << 11) ? (0xf000 | _cal.c40) : _cal.c40; + + PX4_DEBUG("c0:%d\nc1:%d\nc00:%ld\nc10:%ld\nc01:%d\nc11:%d\nc20:%d\nc21:%d\nc30:%d\nc31:%d\nc40:%d\n", + _cal.c0, _cal.c1, + _cal.c00, _cal.c10, + _cal.c01, _cal.c11, _cal.c20, _cal.c21, _cal.c30, _cal.c31, _cal.c40); + return OK; } int @@ -158,7 +168,7 @@ SPA06::init() } if (tries < 0) { - PX4_DEBUG("spa06 cal failed"); + PX4_DEBUG("spa06 sensor or coef not ready"); return -EIO; } @@ -171,8 +181,9 @@ SPA06::init() _interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG); kt = 524288.0f; - + // Enable FIFO _interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG); + // Continuous pressure and temperature mesasurement. _interface->set_reg(7, SPA06_ADDR_MEAS_CFG); Start(); @@ -217,8 +228,8 @@ SPA06::collect() // calculate float ftsc = (float)temp_raw / kt; float fpsc = (float)press_raw / kp; - float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * (float)_cal.c30); - float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * (float)_cal.c21); + float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * ((float)_cal.c30) + fpsc * (float)_cal.c40); + float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * ((float)_cal.c21) + fpsc * (float)_cal.c31); float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3; float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc; diff --git a/src/drivers/barometer/goertek/spa06/SPA06.hpp b/src/drivers/barometer/goertek/spa06/SPA06.hpp index 565e572a3e79..255da708195c 100644 --- a/src/drivers/barometer/goertek/spa06/SPA06.hpp +++ b/src/drivers/barometer/goertek/spa06/SPA06.hpp @@ -76,10 +76,8 @@ class SPA06 : public I2CSPIDriver // configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC) // - // bit[7]: reserved - // - // PM_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 - // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // PM_RATE[7:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200 // note: applicable for measurements in background mode only // // PM_PRC[3:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 @@ -87,25 +85,24 @@ class SPA06 : public I2CSPIDriver // measurement time(ms): 3.6 | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8 // precision(PaRMS) : 5.0 | | 2.5 | | 1.2 | 0.9 | 0.5 | // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register - static constexpr uint8_t _curr_prs_cfg{4 << 4 | 4}; + // + // -> 32 measurements per second, 16 oversampling + static constexpr uint8_t _curr_prs_cfg{5 << 4 | 4}; // configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC) // // temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element) - // TMP_EXT[7] : 0 | 1 - // note: it is highly recommended to use the same temperature sensor as the source of the calibration coefficients wihch can be read from reg 0x28 - // - // TMP_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 - // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // PM_RATE[7:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200 // note: applicable for measurements in background mode only // - // bit[3]: reserved - // // TMP_PRC[2:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 // oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128 // note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register - static constexpr uint8_t _curr_tmp_cfg{1 << 7 | 4 << 4 | 0}; + + // -> 32 measurements per second, single oversampling + static constexpr uint8_t _curr_tmp_cfg{5 << 4 | 0}; bool _collect_phase{false}; float kp; @@ -115,6 +112,6 @@ class SPA06 : public I2CSPIDriver perf_counter_t _measure_perf; perf_counter_t _comms_errors; - static constexpr uint32_t _sample_rate{16}; - static constexpr uint32_t _measure_interval{1000000 / _sample_rate / 2}; + static constexpr uint32_t _sample_rate{32}; + static constexpr uint32_t _measure_interval{1000000 / _sample_rate}; }; diff --git a/src/drivers/barometer/goertek/spa06/spa06.h b/src/drivers/barometer/goertek/spa06/spa06.h index b31276af8761..4412974f351f 100644 --- a/src/drivers/barometer/goertek/spa06/spa06.h +++ b/src/drivers/barometer/goertek/spa06/spa06.h @@ -34,7 +34,7 @@ /** * @file spa06.h * - * Shared defines for the spa06 driver. + * Shared defines for the SPA06 driver. */ #pragma once @@ -51,7 +51,7 @@ #define SPA06_VALUE_RESET 9 -#define SPA06_VALUE_ID 0x10 +#define SPA06_VALUE_ID 0x11 namespace spa06 { @@ -60,8 +60,8 @@ namespace spa06 struct calibration_s { int16_t c0, c1; int32_t c00, c10; - int16_t c01, c11, c20, c21, c30; -}; //calibration data + int16_t c01, c11, c20, c21, c30, c31, c40; +}; struct data_s { uint8_t p_msb; @@ -71,7 +71,7 @@ struct data_s { uint8_t t_msb; uint8_t t_lsb; uint8_t t_xlsb; -}; // data +}; #pragma pack(pop) class ISPA06 @@ -96,10 +96,9 @@ class ISPA06 virtual uint8_t get_device_address() const = 0; }; -} /* namespace */ +} // namespace spa06 -/* interface factories */ #if defined(CONFIG_SPI) extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); #endif // CONFIG_SPI From 95b58599130da5971410a565365004a21b5eedb9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 14:57:07 +1300 Subject: [PATCH 078/211] boards: add SPA06 to KakuteH7/H7mini/H7v2 boards --- boards/holybro/kakuteh7/init/rc.board_sensors | 5 ++++- boards/holybro/kakuteh7mini/init/rc.board_sensors | 5 ++++- boards/holybro/kakuteh7v2/init/rc.board_sensors | 5 ++++- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/boards/holybro/kakuteh7/init/rc.board_sensors b/boards/holybro/kakuteh7/init/rc.board_sensors index eb0413343b01..2d26d786e0c4 100644 --- a/boards/holybro/kakuteh7/init/rc.board_sensors +++ b/boards/holybro/kakuteh7/init/rc.board_sensors @@ -11,4 +11,7 @@ then icm42688p -R 6 -s start fi -bmp280 -X start +if ! bmp280 -X start +then + spa06 -X start +fi diff --git a/boards/holybro/kakuteh7mini/init/rc.board_sensors b/boards/holybro/kakuteh7mini/init/rc.board_sensors index 4577115c1d7f..58e58905af12 100644 --- a/boards/holybro/kakuteh7mini/init/rc.board_sensors +++ b/boards/holybro/kakuteh7mini/init/rc.board_sensors @@ -12,4 +12,7 @@ then fi fi -bmp280 -X start +if ! bmp280 -X start +then + spa06 -X start +fi diff --git a/boards/holybro/kakuteh7v2/init/rc.board_sensors b/boards/holybro/kakuteh7v2/init/rc.board_sensors index ad7025102a30..25387e78478b 100644 --- a/boards/holybro/kakuteh7v2/init/rc.board_sensors +++ b/boards/holybro/kakuteh7v2/init/rc.board_sensors @@ -9,4 +9,7 @@ then icm42688p -R 0 -s start fi -bmp280 -X start +if ! bmp280 -X start +then + spa06 -X start +fi From ab320017ccf5efb6818fbc41d8995b413eb995d4 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Thu, 28 Nov 2024 10:44:44 -0600 Subject: [PATCH 079/211] boards: emlid_navio - support 64-bit OS on Raspberry Pi 4,5 (#24006) adds the `emlid_navio2_arm64` build target - supports 64-bit OS on Raspberry Pi 4,5 (#24006) --- boards/emlid/navio2/arm64.px4board | 1 + 1 file changed, 1 insertion(+) create mode 100644 boards/emlid/navio2/arm64.px4board diff --git a/boards/emlid/navio2/arm64.px4board b/boards/emlid/navio2/arm64.px4board new file mode 100644 index 000000000000..35e129220273 --- /dev/null +++ b/boards/emlid/navio2/arm64.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" From 8b3c78a0a4d1789319a932ed6983054e9797e8ea Mon Sep 17 00:00:00 2001 From: Stefano Colli Date: Thu, 14 Nov 2024 16:23:44 +0100 Subject: [PATCH 080/211] Navigator: add optional delay after gimbal mission items --- src/modules/navigator/mission_block.cpp | 26 ++++++++++++++++++++++-- src/modules/navigator/mission_block.h | 17 ++++++++++++++++ src/modules/navigator/mission_params.c | 13 ++++++++++++ src/modules/navigator/navigator.h | 11 +++++----- src/modules/navigator/navigator_main.cpp | 3 +++ 5 files changed, 63 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 02e7352be2bf..66f17a9766fc 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -81,7 +81,6 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: - case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: @@ -160,6 +159,19 @@ MissionBlock::is_mission_item_reached_or_completed() return false; } + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: { + const float gimbal_command_elasped_time_s = (now - _gimbal_command_time) * 1E-6f; + if (gimbal_command_elasped_time_s > _gimbal_wait_time) { + PX4_DEBUG("Waited %.2f seconds for the gimbal to reach the desired orientation, resuming mission!", (double) _gimbal_wait_time); + return true; + + } + + // We are still waiting the desired delay for the gimbal + return false; + } + + case NAV_CMD_DO_GRIPPER: { const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; @@ -595,6 +607,13 @@ MissionBlock::issue_command(const mission_item_s &item) } } + // Register the Gimbal control command (NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) timestamp so to wait + // for the gimbal to reach the desired position + if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) + { + _gimbal_command_time = hrt_absolute_time(); + } + _navigator->publish_vehicle_cmd(&vcmd); } } @@ -619,10 +638,13 @@ MissionBlock::get_time_inside(const mission_item_s &item) const // and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER // or so, and have a function named 'item_is_conditional' // Reference: https://mavlink.io/en/services/mission.html#mavlink_commands +// A similar condition applies to DO_GIMBAL_MANAGER_PITCHYAW bool MissionBlock::item_has_timeout(const mission_item_s &item) { - return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER; + return item.nav_cmd == NAV_CMD_DO_WINCH || + item.nav_cmd == NAV_CMD_DO_GRIPPER || + item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW; } bool diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 241acdca4fd0..8628a16cf083 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -133,6 +133,19 @@ class MissionBlock : public NavigatorMode _payload_deploy_timeout_s = timeout_s; } + /** + * @brief Set the gimbal wait time + * + * Accessed in Navigator to set the appropriate time to wait for the gimbal to reach + * the desired orientation before resuming the mission + * + * @param wait_time Wait time in seconds + */ + void set_gimbal_wait_time(const float wait_time) + { + _gimbal_wait_time = wait_time; + } + /** * Copies position from setpoint if valid, otherwise copies current position */ @@ -252,6 +265,10 @@ class MissionBlock : public NavigatorMode hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + /* Gimbal commands time variables are used in case of NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW to wait for the gimbal movement */ + hrt_abstime _gimbal_command_time{0}; // Last gimbal command timestamp + float _gimbal_wait_time{0.0f}; // Time to wait for the gimbal to reach the desired configuration + private: void updateMaxHaglFailsafe(); diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index d02932317f2f..8420f8253a15 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -155,3 +155,16 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); * @group Mission */ PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30); + +/** + * Time in seconds allowed for the gimbal to reach its commanded attitude before the mission resumes. + * + * Time allocated for a gimbal command to execute within a mission before resuming. + * This ensures the gimbal has reached the commanded orientation before beginning to take pictures. + * + * @unit s + * @min 0 + * @decimal 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_GIM_WAIT_T, 0.0f); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 0da0c72e9f48..a33c7cca8478 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -424,10 +424,11 @@ class Navigator : public ModuleBase, public ModuleParams _param_nav_min_gnd_dist, /**< minimum distance to ground (Mission and RTL)*/ // non-navigator parameters: Mission (MIS_*) - (ParamFloat) _param_mis_takeoff_alt, - (ParamFloat) _param_mis_yaw_tmt, - (ParamFloat) _param_mis_yaw_err, - (ParamFloat) _param_mis_payload_delivery_timeout, - (ParamInt) _param_mis_lnd_abrt_alt + (ParamFloat) _param_mis_takeoff_alt, + (ParamFloat) _param_mis_yaw_tmt, + (ParamFloat) _param_mis_yaw_err, + (ParamFloat) _param_mis_payload_delivery_timeout, + (ParamInt) _param_mis_lnd_abrt_alt, + (ParamFloat) _param_mis_gimbal_wait_t ) }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b1a920a46769..482e440ac97e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -117,6 +117,7 @@ Navigator::Navigator() : // Update the timeout used in mission_block (which can't hold it's own parameters) _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); + _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), _param_nav_traff_a_ver.get(), @@ -150,6 +151,8 @@ void Navigator::params_update() } _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); + _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); + } void Navigator::run() From ec1cf04bc9d2667c2918ed5b12e6b0fab1232eca Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 18 Nov 2024 14:09:11 +0100 Subject: [PATCH 081/211] mission_block: fix style, shorten debug message strings --- src/modules/navigator/mission_block.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 66f17a9766fc..703e689e374c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -146,11 +146,11 @@ MissionBlock::is_mission_item_reached_or_completed() 1E-6f; // TODO: Add proper microseconds_to_seconds function if (_payload_deploy_ack_successful) { - PX4_DEBUG("Winch Deploy Ack received! Resuming mission"); + PX4_DEBUG("Winch has acknowledged, resume mission"); return true; } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { - PX4_DEBUG("Winch Deploy Timed out, resuming mission!"); + PX4_DEBUG("Winch timed out, resume mission"); return true; } @@ -161,26 +161,25 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: { const float gimbal_command_elasped_time_s = (now - _gimbal_command_time) * 1E-6f; + if (gimbal_command_elasped_time_s > _gimbal_wait_time) { - PX4_DEBUG("Waited %.2f seconds for the gimbal to reach the desired orientation, resuming mission!", (double) _gimbal_wait_time); + PX4_DEBUG("Gimbal aligned, resume mission"); return true; - } // We are still waiting the desired delay for the gimbal return false; } - case NAV_CMD_DO_GRIPPER: { const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; if (_payload_deploy_ack_successful) { - PX4_DEBUG("Gripper Deploy Ack received! Resuming mission"); + PX4_DEBUG("Gripper has acknowledged, resume mission"); return true; } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { - PX4_DEBUG("Gripper Deploy Timed out, resuming mission!"); + PX4_DEBUG("Gripper timed out, resume mission"); return true; } @@ -609,8 +608,7 @@ MissionBlock::issue_command(const mission_item_s &item) // Register the Gimbal control command (NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) timestamp so to wait // for the gimbal to reach the desired position - if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) - { + if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { _gimbal_command_time = hrt_absolute_time(); } From 17c24bafbcd1a3c1cc8c5b8486bdffe80824c713 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 18 Nov 2024 14:45:44 +0100 Subject: [PATCH 082/211] mission_block: simplify timeout check --- src/modules/navigator/mission_block.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 703e689e374c..1d48e3acbb2a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -142,14 +142,11 @@ MissionBlock::is_mission_item_reached_or_completed() break; case NAV_CMD_DO_WINCH: { - const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * - 1E-6f; // TODO: Add proper microseconds_to_seconds function - if (_payload_deploy_ack_successful) { PX4_DEBUG("Winch has acknowledged, resume mission"); return true; - } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { + } else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) { PX4_DEBUG("Winch timed out, resume mission"); return true; @@ -160,9 +157,7 @@ MissionBlock::is_mission_item_reached_or_completed() } case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: { - const float gimbal_command_elasped_time_s = (now - _gimbal_command_time) * 1E-6f; - - if (gimbal_command_elasped_time_s > _gimbal_wait_time) { + if (now > _gimbal_command_time + (_gimbal_wait_time * 1_s)) { PX4_DEBUG("Gimbal aligned, resume mission"); return true; } @@ -172,13 +167,11 @@ MissionBlock::is_mission_item_reached_or_completed() } case NAV_CMD_DO_GRIPPER: { - const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; - if (_payload_deploy_ack_successful) { PX4_DEBUG("Gripper has acknowledged, resume mission"); return true; - } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { + } else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) { PX4_DEBUG("Gripper timed out, resume mission"); return true; From ce3fcd503f473652a76b66a4f4d372a4ca0eab32 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 18 Nov 2024 16:30:05 +0100 Subject: [PATCH 083/211] navigator: unify timeout waiting for payload to execute mission item command Used for winch, gripper, gimbal to reach the desired state before continuing the mission. Ideally we'd have feedback from all these components and not just a feed-forward delay. --- src/modules/navigator/mission_block.cpp | 121 +++++++---------------- src/modules/navigator/mission_block.h | 50 ++-------- src/modules/navigator/mission_params.c | 24 ++--- src/modules/navigator/navigator.h | 3 +- src/modules/navigator/navigator_main.cpp | 18 +--- 5 files changed, 54 insertions(+), 162 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1d48e3acbb2a..b07542984dec 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -141,45 +141,14 @@ MissionBlock::is_mission_item_reached_or_completed() break; - case NAV_CMD_DO_WINCH: { - if (_payload_deploy_ack_successful) { - PX4_DEBUG("Winch has acknowledged, resume mission"); - return true; - - } else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) { - PX4_DEBUG("Winch timed out, resume mission"); - return true; - - } - - // We are still waiting for the acknowledgement / execution of deploy - return false; - } - - case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: { - if (now > _gimbal_command_time + (_gimbal_wait_time * 1_s)) { - PX4_DEBUG("Gimbal aligned, resume mission"); - return true; - } - - // We are still waiting the desired delay for the gimbal - return false; + case NAV_CMD_DO_WINCH: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GRIPPER: + if (now > _timestamp_command_timeout + (_command_timeout * 1_s)) { + return true; } - case NAV_CMD_DO_GRIPPER: { - if (_payload_deploy_ack_successful) { - PX4_DEBUG("Gripper has acknowledged, resume mission"); - return true; - - } else if (now > _payload_deployed_time + (_payload_deploy_timeout_s * 1_s)) { - PX4_DEBUG("Gripper timed out, resume mission"); - return true; - - } - - // We are still waiting for the acknowledgement / execution of deploy - return false; - } + return false; // Still waiting default: /* do nothing, this is a 3D waypoint */ @@ -449,7 +418,7 @@ MissionBlock::is_mission_item_reached_or_completed() /* check if the MAV was long enough inside the waypoint orbit */ if ((get_time_inside(_mission_item) < FLT_EPSILON) || - (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + (now >= (hrt_abstime)(get_time_inside(_mission_item) * 1_s) + _time_wp_reached)) { time_inside_reached = true; } @@ -553,59 +522,37 @@ MissionBlock::issue_command(const mission_item_s &item) return; } - if (item.nav_cmd == NAV_CMD_DO_WINCH || - item.nav_cmd == NAV_CMD_DO_GRIPPER) { - // Initiate Payload Deployment - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - _navigator->publish_vehicle_cmd(&vcmd); - - // Reset payload deploy flag & data to get ready to receive deployment ack result - _payload_deploy_ack_successful = false; - _payload_deployed_time = hrt_absolute_time(); - - } else { - - // This is to support legacy DO_MOUNT_CONTROL as part of a mission. - if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { - _navigator->acquire_gimbal_control(); - } + // This is to support legacy DO_MOUNT_CONTROL as part of a mission. + if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { + _navigator->acquire_gimbal_control(); + } - // Mission item's NAV_CMD enums directly map to the according vehicle command - // So set the raw value directly (MAV_FRAME_MISSION mission item) - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - vcmd.param7 = item.params[6]; - - if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { - // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision - vcmd.param5 = item.lat; - vcmd.param6 = item.lon; - - if (item.altitude_is_relative) { - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; - } + // Mission item's NAV_CMD enums directly map to the according vehicle command + // So set the raw value directly (MAV_FRAME_MISSION mission item) + vehicle_command_s vcmd = {}; + vcmd.command = item.nav_cmd; + vcmd.param1 = item.params[0]; + vcmd.param2 = item.params[1]; + vcmd.param3 = item.params[2]; + vcmd.param4 = item.params[3]; + vcmd.param5 = static_cast(item.params[4]); + vcmd.param6 = static_cast(item.params[5]); + vcmd.param7 = item.params[6]; + + if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { + // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision + vcmd.param5 = item.lat; + vcmd.param6 = item.lon; + + if (item.altitude_is_relative) { + vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; } + } - // Register the Gimbal control command (NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) timestamp so to wait - // for the gimbal to reach the desired position - if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { - _gimbal_command_time = hrt_absolute_time(); - } + _navigator->publish_vehicle_cmd(&vcmd); - _navigator->publish_vehicle_cmd(&vcmd); + if (item_has_timeout(item)) { + _timestamp_command_timeout = hrt_absolute_time(); } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8628a16cf083..52f626a2fe60 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -108,43 +108,11 @@ class MissionBlock : public NavigatorMode static bool item_contains_marker(const mission_item_s &item); /** - * @brief Set the payload deployment successful flag object + * Set the item_has_timeout() command timeout * - * Function is accessed in Navigator (navigator_main.cpp) to flag when a successful - * payload deployment ack command has been received. Which in turn allows the mission item - * to continue to the next in the 'is_mission_item_reached_or_completed' function below + * @param timeout Timeout in seconds */ - void set_payload_depolyment_successful_flag(bool payload_deploy_result) - { - _payload_deploy_ack_successful = payload_deploy_result; - } - - /** - * @brief Set the payload deployment timeout - * - * Accessed in Navigator to set the appropriate timeout to wait for while waiting for a successful - * payload delivery acknowledgement. If the payload deployment takes longer than timeout, mission will - * continue into the next item automatically. - * - * @param timeout_s Timeout in seconds - */ - void set_payload_deployment_timeout(const float timeout_s) - { - _payload_deploy_timeout_s = timeout_s; - } - - /** - * @brief Set the gimbal wait time - * - * Accessed in Navigator to set the appropriate time to wait for the gimbal to reach - * the desired orientation before resuming the mission - * - * @param wait_time Wait time in seconds - */ - void set_gimbal_wait_time(const float wait_time) - { - _gimbal_wait_time = wait_time; - } + void set_command_timeout(const float timeout) { _command_timeout = timeout; } /** * Copies position from setpoint if valid, otherwise copies current position @@ -260,16 +228,10 @@ class MissionBlock : public NavigatorMode hrt_abstime _time_wp_reached{0}; - /* Payload Deploy internal states are used by two NAV_CMDs: DO_WINCH and DO_GRIPPER */ - bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment - hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts - float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received - - /* Gimbal commands time variables are used in case of NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW to wait for the gimbal movement */ - hrt_abstime _gimbal_command_time{0}; // Last gimbal command timestamp - float _gimbal_wait_time{0.0f}; // Time to wait for the gimbal to reach the desired configuration + // Mission items that have a timeout to allow the payload e.g. gripper, winch, gimbal executing the command see item_has_timeout() + hrt_abstime _timestamp_command_timeout{0}; ///< Timestamp when the current item_has_timeout() command was started + float _command_timeout{0.f}; ///< Time in seconds any item_has_timeout() command should be waited for before continuing the mission private: void updateMaxHaglFailsafe(); - }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8420f8253a15..c0bbf5c2541b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -132,17 +132,6 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); */ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); -/** - * Timeout for a successful payload deployment acknowledgement - * - * @unit s - * @min 0 - * @decimal 1 - * @increment 1 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); - /** * Landing abort min altitude * @@ -157,14 +146,19 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30); /** - * Time in seconds allowed for the gimbal to reach its commanded attitude before the mission resumes. + * Timeout to allow the payload to execute the mission command * - * Time allocated for a gimbal command to execute within a mission before resuming. - * This ensures the gimbal has reached the commanded orientation before beginning to take pictures. + * Ensure: + * gripper: NAV_CMD_DO_GRIPPER + * has released before continuing mission. + * winch: CMD_DO_WINCH + * has delivered before continuing mission. + * gimbal: CMD_DO_GIMBAL_MANAGER_PITCHYAW + * has reached the commanded orientation before beginning to take pictures. * * @unit s * @min 0 * @decimal 1 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_GIM_WAIT_T, 0.0f); +PARAM_DEFINE_FLOAT(MIS_COMMAND_TOUT, 0.f); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a33c7cca8478..b3057e53753f 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -427,8 +427,7 @@ class Navigator : public ModuleBase, public ModuleParams (ParamFloat) _param_mis_takeoff_alt, (ParamFloat) _param_mis_yaw_tmt, (ParamFloat) _param_mis_yaw_err, - (ParamFloat) _param_mis_payload_delivery_timeout, (ParamInt) _param_mis_lnd_abrt_alt, - (ParamFloat) _param_mis_gimbal_wait_t + (ParamFloat) _param_mis_command_tout ) }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 482e440ac97e..5f397786fbe4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -115,14 +115,6 @@ Navigator::Navigator() : _distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF; _distance_sensor_mode_change_request_pub.update(); - // Update the timeout used in mission_block (which can't hold it's own parameters) - _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); - _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); - - _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), - _param_nav_traff_a_ver.get(), - _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); - reset_triplets(); } @@ -150,9 +142,10 @@ void Navigator::params_update() param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor); } - _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); - _mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get()); - + _mission.set_command_timeout(_param_mis_command_tout.get()); + _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), + _param_nav_traff_a_ver.get(), + _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); } void Navigator::run() @@ -1270,9 +1263,7 @@ void Navigator::run_fake_traffic() void Navigator::check_traffic() { - if (_traffic_sub.updated()) { - _traffic_sub.copy(&_adsb_conflict._transponder_report); uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | @@ -1291,7 +1282,6 @@ void Navigator::check_traffic() } _adsb_conflict.remove_expired_conflicts(); - } bool Navigator::abort_landing() From 5d7b734bc931b85b5e292c293106815ef5240e45 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 14 Nov 2024 09:29:51 +0100 Subject: [PATCH 084/211] SIH: ellipsoidal earth model SIH: use projection functions and constants from geo lib SIH: remove unnecessary member variable SIH: clarify names of rotation matrices and frames SIH: do not store DCM corresponding to quaternion attitude Using DCM is more efficient when more than 1 rotation needs to be done, which is not the case here. SIH: don't store local variable as member SIH: use Wgs84 constants everywhere SIH: do not store delta_quaternion Converting an AxisAngle to a Quaternion uses the exponenial SIH: organise ECEF member variables SIH: add earth spin rate to gyro data Co-authored-by: bresch --- .../simulation/simulator_sih/CMakeLists.txt | 1 + src/modules/simulation/simulator_sih/sih.cpp | 259 +++++++++++++----- src/modules/simulation/simulator_sih/sih.hpp | 51 +++- 3 files changed, 221 insertions(+), 90 deletions(-) diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index f0328024ff0b..b8d3bf1232b3 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mathlib drivers_accelerometer drivers_gyroscope + geo ) if(PX4_PLATFORM MATCHES "posix") diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index a184f1ac9912..2759844341f6 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -68,8 +68,8 @@ void Sih::run() _px4_accel.set_temperature(T1_C); _px4_gyro.set_temperature(T1_C); - parameters_updated(); init_variables(); + parameters_updated(); const hrt_abstime task_start = hrt_absolute_time(); _last_run = task_start; @@ -241,16 +241,30 @@ void Sih::parameters_updated() _L_PITCH = _sih_l_pitch.get(); _KDV = _sih_kdv.get(); _KDW = _sih_kdw.get(); - _H0 = _sih_h0.get(); - _LAT0 = (double)_sih_lat0.get(); - _LON0 = (double)_sih_lon0.get(); - _COS_LAT0 = cosl((long double)radians(_LAT0)); + if (!_lpos_ref.isInitialized() + || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON) + || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON) + || (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) { + _lpos_ref.initReference(static_cast(_sih_lat0.get()), static_cast(_sih_lon0.get())); + _lpos_ref_alt = _sih_h0.get(); + + // Reset earth position, velocity and attitude + _lat = radians(static_cast(_sih_lat0.get())); + _lon = radians(static_cast(_sih_lon0.get())); + _alt = static_cast(_lpos_ref_alt); + _p_E = llaToEcef(_lat, _lon, _alt); + + const Dcmf R_E2N = computeRotEcefToNed(_lat, _lon, _alt); + _R_N2E = R_E2N.transpose(); + _v_E = _R_N2E * _v_N; + + _q_E = Quatf(_R_N2E) * _q; + _q_E.normalize(); + } _MASS = _sih_mass.get(); - _W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G); - _I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get())); _I(0, 1) = _I(1, 0) = _sih_ixy.get(); _I(0, 2) = _I(2, 0) = _sih_ixz.get(); @@ -270,9 +284,12 @@ void Sih::init_variables() { srand(1234); // initialize the random seed once before calling generate_wgn() - _p_I = Vector3f(0.0f, 0.0f, 0.0f); - _v_I = Vector3f(0.0f, 0.0f, 0.0f); + _lpos = Vector3f(0.0f, 0.0f, 0.0f); + _v_N = Vector3f(0.0f, 0.0f, 0.0f); + _p_E = Vector3d(Wgs84::equatorial_radius, 0.0, 0.0); + _v_E = Vector3f(0.0f, 0.0f, 0.0f); _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + _q_E = Quatf(Eulerf(0.f, -M_PI_2_F, 0.f)); _w_B = Vector3f(0.0f, 0.0f, 0.0f); _u[0] = _u[1] = _u[2] = _u[3] = 0.0f; @@ -304,7 +321,7 @@ void Sih::generate_force_and_torques() _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); - _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft _Ma_B = -_KDW * _w_B; // first order angular damper } else if (_vehicle == VehicleType::FW) { @@ -318,24 +335,24 @@ void Sih::generate_force_and_torques() _Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0])); generate_ts_aerodynamics(); - // _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + // _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft // _Ma_B = -_KDW * _w_B; // first order angular damper } } void Sih::generate_fw_aerodynamics() { - _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] - float altitude = _H0 - _p_I(2); - _wing_l.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); - _wing_r.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX); - _tailplane.update_aero(_v_B, _w_B, altitude, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); - _fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]); - _fuselage.update_aero(_v_B, _w_B, altitude); + const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); + _wing_l.update_aero(v_B, _w_B, _alt, -_u[0]*FLAP_MAX); + _wing_r.update_aero(v_B, _w_B, _alt, _u[0]*FLAP_MAX); + _tailplane.update_aero(v_B, _w_B, _alt, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); + _fin.update_aero(v_B, _w_B, _alt, _u[2]*FLAP_MAX, _T_MAX * _u[3]); + _fuselage.update_aero(v_B, _w_B, _alt); // sum of aerodynamic forces - _Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa()) - _KDV - * _v_I; + const Vector3f Fa_B = _wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa() - + _KDV * v_B; + _Fa_E = _q_E.rotateVector(Fa_B); // aerodynamic moments _Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() + _fuselage.get_Ma() - _KDW * _w_B; @@ -344,12 +361,12 @@ void Sih::generate_fw_aerodynamics() void Sih::generate_ts_aerodynamics() { // velocity in body frame [m/s] - _v_B = _C_IB.transpose() * _v_I; + const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); // the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly) - Vector3f v_ts = _C_BS.transpose() * _v_B; - Vector3f w_ts = _C_BS.transpose() * _w_B; - float altitude = _H0 - _p_I(2); + Vector3f v_ts = _R_S2B.transpose() * v_B; + Vector3f w_ts = _R_S2B.transpose() * _w_B; + float altitude = _lpos_ref_alt - _lpos(2); Vector3f Fa_ts{}; Vector3f Ma_ts{}; @@ -366,49 +383,63 @@ void Sih::generate_ts_aerodynamics() Ma_ts += _ts[i].get_Ma(); } - _Fa_I = _C_IB * _C_BS * Fa_ts - _KDV * _v_I; // sum of aerodynamic forces - _Ma_B = _C_BS * Ma_ts - _KDW * _w_B; // aerodynamic moments + const Vector3f Fa_B = _R_S2B * Fa_ts - _KDV * v_B; // sum of aerodynamic forces + _Fa_E = _q_E.rotateVector(Fa_B); + _Ma_B = _R_S2B * Ma_ts - _KDW * _w_B; // aerodynamic moments +} + +float Sih::computeGravity(const double lat) +{ + // Somigliana formula for gravitational acceleration + const double sin_lat = sin(lat); + const double g = Wgs84::gravity_equator * (1.0 + 0.001931851353 * sin_lat * sin_lat) / sqrt( + 1.0 - Wgs84::eccentricity2 * sin_lat * sin_lat); + return static_cast(g); } void Sih::equations_of_motion(const float dt) { - _C_IB = matrix::Dcm(_q); // body to inertial transformation + _gravity_E = Vector3f(_R_N2E.col(2)) * computeGravity(_lat); // assume gravity along the Down axis + _coriolis_E = -2.f * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE).cross(_v_E); - // Equations of motion of a rigid body - _p_I_dot = _v_I; // position differential - _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum - // _q_dot = _q.derivative1(_w_B); // attitude differential - _dq = Quatf::expq(0.5f * dt * _w_B); - _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum + _v_E_dot = _gravity_E + _coriolis_E + (_Fa_E + _q_E.rotateVector(_T_B)) / _MASS; + _v_N_dot = _R_N2E.transpose() * _v_E_dot; //TODO: add transport rate // fake ground, avoid free fall - if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) { + double vertical_acc = Vector3d(_v_E_dot(0), _v_E_dot(1), _v_E_dot(2)).dot(_p_E) / _p_E.norm(); + + if ((static_cast(_alt) - _lpos_ref_alt) < 0.f && (vertical_acc <= 0.0 || _v_N(2) > 0.f)) { if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { if (!_grounded) { // if we just hit the floor // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_I_dot = -_v_I / dt; + _v_N_dot = -_v_N / dt; + _v_E_dot = -_v_E / dt; } else { - _v_I_dot.setZero(); + _v_N_dot.setZero(); + _v_E_dot.setZero(); } - _v_I.setZero(); + _v_N.setZero(); + _v_E.setZero(); _w_B.setZero(); _grounded = true; } else if (_vehicle == VehicleType::FW) { if (!_grounded) { // if we just hit the floor // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_I_dot(2) = -_v_I(2) / dt; + _v_N_dot(2) = -_v_N(2) / dt; } else { // we only allow negative acceleration in order to takeoff - _v_I_dot(2) = fminf(_v_I_dot(2), 0.0f); + _v_N_dot(2) = fminf(_v_N_dot(2), 0.0f); } // integration: Euler forward - _p_I = _p_I + _p_I_dot * dt; - _v_I = _v_I + _v_I_dot * dt; + Vector3d temp_p_E_dot = Vector3d(_v_E(0), _v_E(1), _v_E(2)); + _p_E = _p_E + temp_p_E_dot * dt; + _v_E = _v_E + _v_E_dot * dt; + _q = _q * _dq; _q.normalize(); _w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); @@ -416,16 +447,86 @@ void Sih::equations_of_motion(const float dt) } } else { - // integration: Euler forward - _p_I = _p_I + _p_I_dot * dt; - _v_I = _v_I + _v_I_dot * dt; - _q = _q * _dq; - _q.normalize(); - // integration Runge-Kutta 4 - // rk4_update(_p_I, _v_I, _q, _w_B); - _w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); + // forward Euler velocity intergation + Vector3d v_E_prev(_v_E(0), _v_E(1), _v_E(2)); + _v_E = _v_E + _v_E_dot * dt; + // trapezoidal position integration + _p_E = _p_E + (Vector3d(_v_E(0), _v_E(1), _v_E(2)) + v_E_prev) * 0.5 * dt; + + const Quatf dq(AxisAnglef(_w_B * dt)); + + _q_E = _q_E * dq; + _q_E.normalize(); + + const Vector3f w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum + _w_B = constrain(_w_B + w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); _grounded = false; } + + ecefToNed(); + + _lpos_ref.project(degrees(_lat), degrees(_lon), _lpos(0), _lpos(1)); + _lpos(2) = -(static_cast(_alt) - _lpos_ref_alt); +} + +void Sih::ecefToNed() +{ + // Convert position using Borkowski closed-form exact solution + const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(_p_E(2)); + const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius; + const double beta = sqrt(_p_E(0) * _p_E(0) + _p_E(1) * _p_E(1)); + const double E = (k1 - k2) / beta; + const double F = (k1 + k2) / beta; + + const double P = 4.0 / 3.0 * (E * F + 1); + const double Q = 2 * (E * E - F * F); + const double D = P * P * P + Q * Q; + const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0); + const double G = 0.5 * (sqrt(E * E + V) + E); + const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G; + + _lon = atan2(_p_E(1), _p_E(0)); + _lat = sign(_p_E(2)) * atan((1 - T * T) / (2 * T * sqrt(1 - Wgs84::eccentricity2))); + _alt = (beta - Wgs84::equatorial_radius * T) * cos(_lat) + + (_p_E(2) - sign(_p_E(2)) * Wgs84::equatorial_radius * sqrt(1 - Wgs84::eccentricity2)) * sin(_lat); + + const Dcmf C_SE = computeRotEcefToNed(_lat, _lon, _alt); + _R_N2E = C_SE.transpose(); + + // Transform velocity to NED frame + _v_N = C_SE * _v_E; + _q = Quatf(C_SE) * _q_E; + _q.normalize(); +} + +Vector3d Sih::llaToEcef(const double lat, const double lon, const double alt) +{ + const double r_e = Wgs84::equatorial_radius / sqrt(1 - std::pow(Wgs84::eccentricity * sin(lat), 2)); + + const double cos_lat = cos(lat); + const double sin_lat = sin(lat); + const double cos_lon = cos(lon); + const double sin_lon = sin(lon); + + return Vector3d((r_e + alt) * cos_lat * cos_lon, + (r_e + alt) * cos_lat * sin_lon, + ((1.0 - Wgs84::eccentricity2) * r_e + alt) * sin_lat); +} + +Dcmf Sih::computeRotEcefToNed(const double lat, const double lon, const double alt) +{ + // Calculate the ECEF to NED coordinate transformation matrix + const double cos_lat = cos(lat); + const double sin_lat = sin(lat); + const double cos_lon = cos(lon); + const double sin_lon = sin(lon); + + const float val[] = {(float)(-sin_lat * cos_lon), (float)(-sin_lat * sin_lon), (float)cos_lat, + (float) - sin_lon, (float)cos_lon, 0.f, + (float)(-cos_lat * cos_lon), (float)(-cos_lat * sin_lon), (float) - sin_lat + }; + + return Dcmf(val); } void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) @@ -435,8 +536,13 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. // IMU - Vector3f acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f); - Vector3f gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f); + const Dcmf R_E2B(_q_E.inversed()); + + Vector3f specific_force_B = R_E2B * (_v_E_dot - _gravity_E - _coriolis_E); + Vector3f acc = specific_force_B + noiseGauss3f(0.5f, 1.7f, 1.4f); + + const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); + Vector3f gyro = _w_B + earth_spin_rate_B + noiseGauss3f(0.14f, 0.07f, 0.03f); // update IMU every iteration _px4_accel.update(time_now_us, acc(0), acc(1), acc(2)); @@ -448,7 +554,7 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; - airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B.norm() + generate_wgn() * 0.2f); + airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; @@ -477,7 +583,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) distance_sensor.current_distance = _distance_snsr_override; } else { - distance_sensor.current_distance = -_p_I(2) / _C_IB(2, 2); + distance_sensor.current_distance = -_lpos(2) / _q.dcm_z()(2); if (distance_sensor.current_distance > _distance_snsr_max) { // this is based on lightware lw20 behaviour @@ -522,25 +628,26 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.v_xy_valid = true; local_position.v_z_valid = true; - local_position.x = _p_I(0); - local_position.y = _p_I(1); - local_position.z = _p_I(2); + local_position.x = _lpos(0); + local_position.y = _lpos(1); + local_position.z = _lpos(2); + + local_position.vx = _v_N(0); + local_position.vy = _v_N(1); + local_position.vz = _v_N(2); - local_position.vx = _v_I(0); - local_position.vy = _v_I(1); - local_position.vz = _v_I(2); - local_position.z_deriv = _v_I(2); + local_position.z_deriv = _v_N(2); - local_position.ax = _v_I_dot(0); - local_position.ay = _v_I_dot(1); - local_position.az = _v_I_dot(2); + local_position.ax = _v_N_dot(0); + local_position.ay = _v_N_dot(1); + local_position.az = _v_N_dot(2); local_position.xy_global = true; local_position.z_global = true; local_position.ref_timestamp = _last_run; - local_position.ref_lat = _LAT0; - local_position.ref_lon = _LON0; - local_position.ref_alt = _H0; + local_position.ref_lat = _lpos_ref.getProjectionReferenceLat(); + local_position.ref_lon = _lpos_ref.getProjectionReferenceLon(); + local_position.ref_alt = _lpos_ref_alt; local_position.heading = Eulerf(_q).psi(); local_position.heading_good_for_control = true; @@ -554,11 +661,11 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) // publish global position groundtruth vehicle_global_position_s global_position{}; global_position.timestamp_sample = time_now_us; - global_position.lat = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH);; - global_position.lon = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;; - global_position.alt = _H0 - _p_I(2);; + global_position.lat = degrees(_lat); + global_position.lon = degrees(_lon); + global_position.alt = _alt; global_position.alt_ellipsoid = global_position.alt; - global_position.terrain_alt = -_p_I(2); + global_position.terrain_alt = -_lpos(2); global_position.timestamp = hrt_absolute_time(); _global_position_ground_truth_pub.publish(global_position); } @@ -619,10 +726,10 @@ int Sih::print_status() } PX4_INFO("vehicle landed: %d", _grounded); - PX4_INFO("inertial position NED (m)"); - _p_I.print(); - PX4_INFO("inertial velocity NED (m/s)"); - _v_I.print(); + PX4_INFO("local position NED (m)"); + _lpos.print(); + PX4_INFO("local velocity NED (m/s)"); + _v_N.print(); PX4_INFO("attitude roll-pitch-yaw (deg)"); (Eulerf(_q) * 180.0f / M_PI_F).print(); PX4_INFO("angular acceleration roll-pitch-yaw (deg/s)"); @@ -630,8 +737,8 @@ int Sih::print_status() PX4_INFO("actuator signals"); Vector u = Vector(_u); u.transpose().print(); - PX4_INFO("Aerodynamic forces NED inertial (N)"); - _Fa_I.print(); + PX4_INFO("Aerodynamic forces NED (N)"); + (_R_N2E.transpose() * _Fa_E).print(); PX4_INFO("Aerodynamic moments body frame (Nm)"); _Ma_B.print(); PX4_INFO("Thruster moments in body frame (Nm)"); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 8988474e01b1..d4ae8a023dee 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -65,6 +65,7 @@ #include // to get the real time #include #include +#include #include #include #include @@ -162,6 +163,19 @@ class Sih : public ModuleBase, public ModuleParams void generate_fw_aerodynamics(); void generate_ts_aerodynamics(); void sensor_step(); + float computeGravity(double lat); + + void ecefToNed(); + static matrix::Vector3d llaToEcef(double lat, double lon, double alt); + matrix::Dcmf computeRotEcefToNed(const double lat, const double lon, const double alt); + + struct Wgs84 { + static constexpr double equatorial_radius = 6378137.0; + static constexpr double eccentricity = 0.0818191908425; + static constexpr double eccentricity2 = eccentricity * eccentricity; + static constexpr double gravity_equator = 9.7803253359; + }; + #if defined(ENABLE_LOCKSTEP_SCHEDULER) void lockstep_loop(); @@ -185,19 +199,28 @@ class Sih : public ModuleBase, public ModuleParams bool _grounded{true};// whether the vehicle is on the ground matrix::Vector3f _T_B{}; // thrust force in body frame [N] - matrix::Vector3f _Fa_I{}; // aerodynamic force in inertial frame [N] matrix::Vector3f _Mt_B{}; // thruster moments in the body frame [Nm] matrix::Vector3f _Ma_B{}; // aerodynamic moments in the body frame [Nm] - matrix::Vector3f _p_I{}; // inertial position [m] - matrix::Vector3f _v_I{}; // inertial velocity [m/s] - matrix::Vector3f _v_B{}; // body frame velocity [m/s] - matrix::Vector3f _p_I_dot{}; // inertial position differential - matrix::Vector3f _v_I_dot{}; // inertial velocity differential - matrix::Quatf _q{}; // quaternion attitude - matrix::Dcmf _C_IB{}; // body to inertial transformation + matrix::Vector3f _lpos{}; // position in a local tangent-plane frame [m] + matrix::Vector3f _v_N{}; // velocity in local navigation frame (NED, body-fixed) [m/s] + matrix::Vector3f _v_N_dot{}; // time derivative of velocity in local navigation frame [m/s2] + matrix::Quatf _q{}; // quaternion attitude in local navigation frame matrix::Vector3f _w_B{}; // body rates in body frame [rad/s] - matrix::Quatf _dq{}; // quaternion differential - matrix::Vector3f _w_B_dot{}; // body rates differential + + double _lat{0.0}; + double _lon{0.0}; + double _alt{0.0}; + + // Quantities in Earth-centered-Earth-fixed coordinates + matrix::Vector3f _Fa_E{}; // aerodynamic force in ECEF frame [N] + matrix::Vector3f _gravity_E{}; + matrix::Vector3f _coriolis_E{}; + matrix::Quatf _q_E{}; + matrix::Vector3d _p_E{}; + matrix::Vector3f _v_E{}; + matrix::Vector3f _v_E_dot{}; + matrix::Dcmf _R_N2E; // local navigation to ECEF frame rotation matrix + float _u[NB_MOTORS] {}; // thruster signals enum class VehicleType {MC, FW, TS}; @@ -218,7 +241,7 @@ class Sih : public ModuleBase, public ModuleParams static constexpr const float TS_CM = 0.115f; // longitudinal position of the CM from trailing edge static constexpr const float TS_RP = 0.0625f; // propeller radius [m] static constexpr const float TS_DEF_MAX = math::radians(39.0f); // max deflection - matrix::Dcmf _C_BS = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch + matrix::Dcmf _R_S2B = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch AeroSeg _ts[NB_TS_SEG] = { AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, -0.239f, 0.0f), 0.0f, TS_AR), AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, -0.208f, 0.0f), 0.0f, TS_AR, 0.063f), @@ -248,9 +271,9 @@ class Sih : public ModuleBase, public ModuleParams // }; // parameters - float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0, _T_TAU; - double _LAT0, _LON0, _COS_LAT0; - matrix::Vector3f _W_I; // weight of the vehicle in inertial frame [N] + MapProjection _lpos_ref{}; + float _lpos_ref_alt; + float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU; matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix From 9b172d36a25ab0e495c828d74b0864bc02b573e7 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 09:27:57 +0100 Subject: [PATCH 085/211] matrix: allow casting float<->double --- src/lib/matrix/matrix/Matrix.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index 9e60b9ebef38..d17447b5b24b 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -55,6 +55,16 @@ class Matrix } } + template + Matrix(const Matrix &aa) + { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + _data[i][j] = static_cast(aa(i, j)); + } + } + } + template Matrix(const Slice &in_slice) { From 5d33971712cf269182f0d8a0d320324542a09071 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2024 16:31:59 +0100 Subject: [PATCH 086/211] SIH: refactor MC ground contact --- src/modules/simulation/simulator_sih/sih.cpp | 62 +++++++++++--------- src/modules/simulation/simulator_sih/sih.hpp | 3 +- 2 files changed, 34 insertions(+), 31 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 2759844341f6..92fe08ee55c0 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -399,30 +399,26 @@ float Sih::computeGravity(const double lat) void Sih::equations_of_motion(const float dt) { - _gravity_E = Vector3f(_R_N2E.col(2)) * computeGravity(_lat); // assume gravity along the Down axis - _coriolis_E = -2.f * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE).cross(_v_E); + const Vector3f gravity_acceleration_E = Vector3f(_R_N2E.col(2)) * computeGravity(_lat); // gravity along the Down axis + const Vector3f coriolis_acceleration_E = -2.f * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE).cross(_v_E); - _v_E_dot = _gravity_E + _coriolis_E + (_Fa_E + _q_E.rotateVector(_T_B)) / _MASS; - _v_N_dot = _R_N2E.transpose() * _v_E_dot; //TODO: add transport rate + const Vector3f weight_E = _MASS * gravity_acceleration_E; + Vector3f sum_of_forces_E = _Fa_E + _q_E.rotateVector(_T_B) + weight_E; // fake ground, avoid free fall - double vertical_acc = Vector3d(_v_E_dot(0), _v_E_dot(1), _v_E_dot(2)).dot(_p_E) / _p_E.norm(); + const float force_down = Vector3f(_R_N2E.transpose() * sum_of_forces_E)(2); + Vector3f ground_force_E; - if ((static_cast(_alt) - _lpos_ref_alt) < 0.f && (vertical_acc <= 0.0 || _v_N(2) > 0.f)) { + if ((static_cast(_alt) - _lpos_ref_alt) < 0.f && force_down > 0.f) { if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { - if (!_grounded) { // if we just hit the floor - // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_N_dot = -_v_N / dt; - _v_E_dot = -_v_E / dt; + ground_force_E = -sum_of_forces_E; - } else { - _v_N_dot.setZero(); - _v_E_dot.setZero(); + if (!_grounded) { + // if we just hit the floor + // compute the force that will stop the vehicle in one time step + ground_force_E += -_v_E / dt * _MASS; } - _v_N.setZero(); - _v_E.setZero(); - _w_B.setZero(); _grounded = true; } else if (_vehicle == VehicleType::FW) { @@ -447,21 +443,29 @@ void Sih::equations_of_motion(const float dt) } } else { - // forward Euler velocity intergation - Vector3d v_E_prev(_v_E(0), _v_E(1), _v_E(2)); - _v_E = _v_E + _v_E_dot * dt; - // trapezoidal position integration - _p_E = _p_E + (Vector3d(_v_E(0), _v_E(1), _v_E(2)) + v_E_prev) * 0.5 * dt; + _grounded = false; + } - const Quatf dq(AxisAnglef(_w_B * dt)); + sum_of_forces_E += ground_force_E; + const Vector3f acceleration_E = sum_of_forces_E / _MASS; + _specific_force_E = acceleration_E - gravity_acceleration_E; - _q_E = _q_E * dq; - _q_E.normalize(); + _v_E_dot = acceleration_E + coriolis_acceleration_E; + _v_N_dot = _R_N2E.transpose() * _v_E_dot; //TODO: add transport rate - const Vector3f w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum - _w_B = constrain(_w_B + w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); - _grounded = false; - } + // forward Euler velocity intergation + Vector3f v_E_prev = _v_E; + _v_E = _v_E + _v_E_dot * dt; + // trapezoidal position integration + _p_E = _p_E + Vector3d(_v_E + v_E_prev) * 0.5 * static_cast(dt); + + const Quatf dq(AxisAnglef(_w_B * dt)); + + _q_E = _q_E * dq; + _q_E.normalize(); + + const Vector3f w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum + _w_B = constrain(_w_B + w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); ecefToNed(); @@ -538,7 +542,7 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) // IMU const Dcmf R_E2B(_q_E.inversed()); - Vector3f specific_force_B = R_E2B * (_v_E_dot - _gravity_E - _coriolis_E); + Vector3f specific_force_B = R_E2B * _specific_force_E; Vector3f acc = specific_force_B + noiseGauss3f(0.5f, 1.7f, 1.4f); const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index d4ae8a023dee..3e71a32f3b35 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -213,8 +213,7 @@ class Sih : public ModuleBase, public ModuleParams // Quantities in Earth-centered-Earth-fixed coordinates matrix::Vector3f _Fa_E{}; // aerodynamic force in ECEF frame [N] - matrix::Vector3f _gravity_E{}; - matrix::Vector3f _coriolis_E{}; + matrix::Vector3f _specific_force_E{}; matrix::Quatf _q_E{}; matrix::Vector3d _p_E{}; matrix::Vector3f _v_E{}; From 7ee69d616d793a78d99836b326383e0eefa65b68 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2024 17:30:06 +0100 Subject: [PATCH 087/211] SIH: rework FW ground contact --- src/modules/simulation/simulator_sih/sih.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 92fe08ee55c0..086de38ec19e 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -422,23 +422,15 @@ void Sih::equations_of_motion(const float dt) _grounded = true; } else if (_vehicle == VehicleType::FW) { - if (!_grounded) { // if we just hit the floor - // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_N_dot(2) = -_v_N(2) / dt; + Vector3f down_u = _R_N2E.col(2); + ground_force_E = -down_u * sum_of_forces_E * down_u; - } else { - // we only allow negative acceleration in order to takeoff - _v_N_dot(2) = fminf(_v_N_dot(2), 0.0f); + if (!_grounded) { + // if we just hit the floor + // compute the force that will stop the vehicle in one time step + ground_force_E += down_u * (-_v_N(2) / dt) * _MASS; } - // integration: Euler forward - Vector3d temp_p_E_dot = Vector3d(_v_E(0), _v_E(1), _v_E(2)); - _p_E = _p_E + temp_p_E_dot * dt; - _v_E = _v_E + _v_E_dot * dt; - - _q = _q * _dq; - _q.normalize(); - _w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); _grounded = true; } From 7cf42727fb23bc12844b7e2dbc52da3087feaeb5 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 10:35:46 +0100 Subject: [PATCH 088/211] lla: add functions to convert from and to ECEF --- .../ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp | 44 +++++++++++++++++++ .../ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp | 7 ++- 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp index 93d347f2d73e..d9ac442e3a8f 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp @@ -34,8 +34,52 @@ #include "lat_lon_alt.hpp" using matrix::Vector3f; +using matrix::Vector3d; using matrix::Vector2d; +LatLonAlt LatLonAlt::fromEcef(const Vector3d &p_ecef) +{ + // Convert position using Borkowski closed-form exact solution + // P. D. Groves, "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd edition (appendix C) + const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(p_ecef(2)); + const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius; + const double beta = sqrt(p_ecef(0) * p_ecef(0) + p_ecef(1) * p_ecef(1)); + const double E = (k1 - k2) / beta; + const double F = (k1 + k2) / beta; + + const double P = 4.0 / 3.0 * (E * F + 1); + const double Q = 2 * (E * E - F * F); + const double D = P * P * P + Q * Q; + const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0); + const double G = 0.5 * (sqrt(E * E + V) + E); + const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G; + + const double lon = atan2(p_ecef(1), p_ecef(0)); + const double lat = matrix::sign(p_ecef(2)) * atan((1.0 - T * T) / (2.0 * T * sqrt(1.0 - Wgs84::eccentricity2))); + const double alt = (beta - Wgs84::equatorial_radius * T) * cos(lat) + + (p_ecef(2) - matrix::sign(p_ecef(2)) * Wgs84::equatorial_radius * sqrt(1.0 - Wgs84::eccentricity2)) * sin(lat); + + LatLonAlt lla; + lla.setLatLonRad(lat, lon); + lla.setAltitude(static_cast(alt)); + return lla; +} + +Vector3d LatLonAlt::toEcef() const +{ + const double cos_lat = cos(_latitude_rad); + const double sin_lat = sin(_latitude_rad); + const double cos_lon = cos(_longitude_rad); + const double sin_lon = sin(_longitude_rad); + + const double r_e = Wgs84::equatorial_radius / sqrt(1.0 - std::pow(Wgs84::eccentricity * sin_lat, 2.0)); + const double r_total = r_e + static_cast(_altitude); + + return Vector3d(r_total * cos_lat * cos_lon, + r_total * cos_lat * sin_lon, + ((1.0 - Wgs84::eccentricity2) * r_total) * sin_lat); +} + Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const { double r_n; diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp index 548f6ee30d46..085143c6f950 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -54,6 +54,10 @@ class LatLonAlt _altitude = altitude_m; } + + static LatLonAlt fromEcef(const matrix::Vector3d &p_ecef); + matrix::Vector3d toEcef() const; + void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; } double latitude_deg() const { return math::degrees(latitude_rad()); } @@ -104,7 +108,8 @@ class LatLonAlt struct Wgs84 { static constexpr double equatorial_radius = 6378137.0; static constexpr double eccentricity = 0.0818191908425; - static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity); + static constexpr double eccentricity2 = eccentricity * eccentricity; + static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2); }; double _latitude_rad{0.0}; From b6658df16905fe398570576b3b8af17ffc0b2cb4 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 10:46:39 +0100 Subject: [PATCH 089/211] lla: move to lib directory --- src/lib/CMakeLists.txt | 1 + src/{modules/ekf2/EKF => lib}/lat_lon_alt/CMakeLists.txt | 0 src/{modules/ekf2/EKF => lib}/lat_lon_alt/lat_lon_alt.cpp | 0 src/{modules/ekf2/EKF => lib}/lat_lon_alt/lat_lon_alt.hpp | 0 src/{modules/ekf2/EKF => lib}/lat_lon_alt/test_lat_lon_alt.cpp | 0 src/modules/ekf2/EKF/CMakeLists.txt | 1 - src/modules/ekf2/EKF/ekf.h | 2 -- src/modules/ekf2/EKF/estimator_interface.h | 2 +- src/modules/ekf2/EKF/output_predictor/output_predictor.h | 2 +- 9 files changed, 3 insertions(+), 5 deletions(-) rename src/{modules/ekf2/EKF => lib}/lat_lon_alt/CMakeLists.txt (100%) rename src/{modules/ekf2/EKF => lib}/lat_lon_alt/lat_lon_alt.cpp (100%) rename src/{modules/ekf2/EKF => lib}/lat_lon_alt/lat_lon_alt.hpp (100%) rename src/{modules/ekf2/EKF => lib}/lat_lon_alt/test_lat_lon_alt.cpp (100%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 7a7b5973fd4a..69ab23aacc0e 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -54,6 +54,7 @@ add_subdirectory(geo EXCLUDE_FROM_ALL) add_subdirectory(heatshrink EXCLUDE_FROM_ALL) add_subdirectory(hysteresis EXCLUDE_FROM_ALL) add_subdirectory(l1 EXCLUDE_FROM_ALL) +add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) diff --git a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt b/src/lib/lat_lon_alt/CMakeLists.txt similarity index 100% rename from src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt rename to src/lib/lat_lon_alt/CMakeLists.txt diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp b/src/lib/lat_lon_alt/lat_lon_alt.cpp similarity index 100% rename from src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp rename to src/lib/lat_lon_alt/lat_lon_alt.cpp diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/lib/lat_lon_alt/lat_lon_alt.hpp similarity index 100% rename from src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp rename to src/lib/lat_lon_alt/lat_lon_alt.hpp diff --git a/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp similarity index 100% rename from src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp rename to src/lib/lat_lon_alt/test_lat_lon_alt.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index f6dc615f4e36..2c4c898d4113 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,7 +33,6 @@ add_subdirectory(bias_estimator) add_subdirectory(output_predictor) -add_subdirectory(lat_lon_alt) set(EKF_LIBS) set(EKF_SRCS) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f42493652ecc..aa4597b25d9c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -66,8 +66,6 @@ # include "aid_sources/aux_global_position/aux_global_position.hpp" #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION -#include "lat_lon_alt/lat_lon_alt.hpp" - enum class Likelihood { LOW, MEDIUM, HIGH }; class ExternalVisionVel; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 90c26d608392..e878d100dc9a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -42,7 +42,6 @@ #ifndef EKF_ESTIMATOR_INTERFACE_H #define EKF_ESTIMATOR_INTERFACE_H -#include "lat_lon_alt/lat_lon_alt.hpp" #if defined(MODULE_NAME) #include # define ECL_INFO PX4_DEBUG @@ -73,6 +72,7 @@ #endif // CONFIG_EKF2_RANGE_FINDER #include +#include #include #include #include diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 33b7f127bc05..602993b53309 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -37,9 +37,9 @@ #include #include "../RingBuffer.h" -#include "../lat_lon_alt/lat_lon_alt.hpp" #include +#include class OutputPredictor { From 189122d5538ffeaab902748affbf48e66f9e5144 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 11:12:00 +0100 Subject: [PATCH 090/211] lla: add gravity constant at equator --- src/lib/lat_lon_alt/lat_lon_alt.hpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/lib/lat_lon_alt/lat_lon_alt.hpp b/src/lib/lat_lon_alt/lat_lon_alt.hpp index 085143c6f950..02614c186238 100644 --- a/src/lib/lat_lon_alt/lat_lon_alt.hpp +++ b/src/lib/lat_lon_alt/lat_lon_alt.hpp @@ -98,20 +98,21 @@ class LatLonAlt */ matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const; -private: - // Convert between curvilinear and cartesian errors - static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude); - - static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature, - double &transverse_radius_of_curvature); - struct Wgs84 { static constexpr double equatorial_radius = 6378137.0; static constexpr double eccentricity = 0.0818191908425; static constexpr double eccentricity2 = eccentricity * eccentricity; static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2); + static constexpr double gravity_equator = 9.7803253359; }; +private: + // Convert between curvilinear and cartesian errors + static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude); + + static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature, + double &transverse_radius_of_curvature); + double _latitude_rad{0.0}; double _longitude_rad{0.0}; float _altitude{0.0}; From 674aa474e7b7b9cedaec81fa14487709f26f07ff Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 11:13:28 +0100 Subject: [PATCH 091/211] SIH: use LatLonAlt class --- src/modules/simulation/simulator_sih/sih.cpp | 87 +++++++------------- src/modules/simulation/simulator_sih/sih.hpp | 10 +-- 2 files changed, 33 insertions(+), 64 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 086de38ec19e..58cb8b51503c 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -250,12 +250,12 @@ void Sih::parameters_updated() _lpos_ref_alt = _sih_h0.get(); // Reset earth position, velocity and attitude - _lat = radians(static_cast(_sih_lat0.get())); - _lon = radians(static_cast(_sih_lon0.get())); - _alt = static_cast(_lpos_ref_alt); - _p_E = llaToEcef(_lat, _lon, _alt); + _lla.setLatitudeDeg(static_cast(_sih_lat0.get())); + _lla.setLongitudeDeg(static_cast(_sih_lon0.get())); + _lla.setAltitude(_lpos_ref_alt); + _p_E = _lla.toEcef(); - const Dcmf R_E2N = computeRotEcefToNed(_lat, _lon, _alt); + const Dcmf R_E2N = computeRotEcefToNed(_lla); _R_N2E = R_E2N.transpose(); _v_E = _R_N2E * _v_N; @@ -343,11 +343,12 @@ void Sih::generate_force_and_torques() void Sih::generate_fw_aerodynamics() { const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); - _wing_l.update_aero(v_B, _w_B, _alt, -_u[0]*FLAP_MAX); - _wing_r.update_aero(v_B, _w_B, _alt, _u[0]*FLAP_MAX); - _tailplane.update_aero(v_B, _w_B, _alt, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); - _fin.update_aero(v_B, _w_B, _alt, _u[2]*FLAP_MAX, _T_MAX * _u[3]); - _fuselage.update_aero(v_B, _w_B, _alt); + const float &alt = _lla.altitude(); + _wing_l.update_aero(v_B, _w_B, alt, -_u[0]*FLAP_MAX); + _wing_r.update_aero(v_B, _w_B, alt, _u[0]*FLAP_MAX); + _tailplane.update_aero(v_B, _w_B, alt, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); + _fin.update_aero(v_B, _w_B, alt, _u[2]*FLAP_MAX, _T_MAX * _u[3]); + _fuselage.update_aero(v_B, _w_B, alt); // sum of aerodynamic forces const Vector3f Fa_B = _wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa() - @@ -392,14 +393,15 @@ float Sih::computeGravity(const double lat) { // Somigliana formula for gravitational acceleration const double sin_lat = sin(lat); - const double g = Wgs84::gravity_equator * (1.0 + 0.001931851353 * sin_lat * sin_lat) / sqrt( - 1.0 - Wgs84::eccentricity2 * sin_lat * sin_lat); + const double g = LatLonAlt::Wgs84::gravity_equator * (1.0 + 0.001931851353 * sin_lat * sin_lat) / sqrt( + 1.0 - LatLonAlt::Wgs84::eccentricity2 * sin_lat * sin_lat); return static_cast(g); } void Sih::equations_of_motion(const float dt) { - const Vector3f gravity_acceleration_E = Vector3f(_R_N2E.col(2)) * computeGravity(_lat); // gravity along the Down axis + const Vector3f gravity_acceleration_E = Vector3f(_R_N2E.col(2)) * computeGravity( + _lla.latitude_rad()); // gravity along the Down axis const Vector3f coriolis_acceleration_E = -2.f * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE).cross(_v_E); const Vector3f weight_E = _MASS * gravity_acceleration_E; @@ -409,7 +411,7 @@ void Sih::equations_of_motion(const float dt) const float force_down = Vector3f(_R_N2E.transpose() * sum_of_forces_E)(2); Vector3f ground_force_E; - if ((static_cast(_alt) - _lpos_ref_alt) < 0.f && force_down > 0.f) { + if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) { if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { ground_force_E = -sum_of_forces_E; @@ -461,32 +463,15 @@ void Sih::equations_of_motion(const float dt) ecefToNed(); - _lpos_ref.project(degrees(_lat), degrees(_lon), _lpos(0), _lpos(1)); - _lpos(2) = -(static_cast(_alt) - _lpos_ref_alt); + _lpos_ref.project(_lla.latitude_deg(), _lla.longitude_deg(), _lpos(0), _lpos(1)); + _lpos(2) = -(_lla.altitude() - _lpos_ref_alt); } void Sih::ecefToNed() { - // Convert position using Borkowski closed-form exact solution - const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(_p_E(2)); - const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius; - const double beta = sqrt(_p_E(0) * _p_E(0) + _p_E(1) * _p_E(1)); - const double E = (k1 - k2) / beta; - const double F = (k1 + k2) / beta; - - const double P = 4.0 / 3.0 * (E * F + 1); - const double Q = 2 * (E * E - F * F); - const double D = P * P * P + Q * Q; - const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0); - const double G = 0.5 * (sqrt(E * E + V) + E); - const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G; - - _lon = atan2(_p_E(1), _p_E(0)); - _lat = sign(_p_E(2)) * atan((1 - T * T) / (2 * T * sqrt(1 - Wgs84::eccentricity2))); - _alt = (beta - Wgs84::equatorial_radius * T) * cos(_lat) + - (_p_E(2) - sign(_p_E(2)) * Wgs84::equatorial_radius * sqrt(1 - Wgs84::eccentricity2)) * sin(_lat); - - const Dcmf C_SE = computeRotEcefToNed(_lat, _lon, _alt); + _lla = LatLonAlt::fromEcef(_p_E); + + const Dcmf C_SE = computeRotEcefToNed(_lla); _R_N2E = C_SE.transpose(); // Transform velocity to NED frame @@ -495,27 +480,13 @@ void Sih::ecefToNed() _q.normalize(); } -Vector3d Sih::llaToEcef(const double lat, const double lon, const double alt) -{ - const double r_e = Wgs84::equatorial_radius / sqrt(1 - std::pow(Wgs84::eccentricity * sin(lat), 2)); - - const double cos_lat = cos(lat); - const double sin_lat = sin(lat); - const double cos_lon = cos(lon); - const double sin_lon = sin(lon); - - return Vector3d((r_e + alt) * cos_lat * cos_lon, - (r_e + alt) * cos_lat * sin_lon, - ((1.0 - Wgs84::eccentricity2) * r_e + alt) * sin_lat); -} - -Dcmf Sih::computeRotEcefToNed(const double lat, const double lon, const double alt) +Dcmf Sih::computeRotEcefToNed(const LatLonAlt &lla) { // Calculate the ECEF to NED coordinate transformation matrix - const double cos_lat = cos(lat); - const double sin_lat = sin(lat); - const double cos_lon = cos(lon); - const double sin_lon = sin(lon); + const double cos_lat = cos(lla.latitude_rad()); + const double sin_lat = sin(lla.latitude_rad()); + const double cos_lon = cos(lla.longitude_rad()); + const double sin_lon = sin(lla.longitude_rad()); const float val[] = {(float)(-sin_lat * cos_lon), (float)(-sin_lat * sin_lon), (float)cos_lat, (float) - sin_lon, (float)cos_lon, 0.f, @@ -657,9 +628,9 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) // publish global position groundtruth vehicle_global_position_s global_position{}; global_position.timestamp_sample = time_now_us; - global_position.lat = degrees(_lat); - global_position.lon = degrees(_lon); - global_position.alt = _alt; + global_position.lat = _lla.latitude_deg(); + global_position.lon = _lla.longitude_deg(); + global_position.alt = _lla.altitude(); global_position.alt_ellipsoid = global_position.alt; global_position.terrain_alt = -_lpos(2); global_position.timestamp = hrt_absolute_time(); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 3e71a32f3b35..588d231855ae 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -163,11 +164,10 @@ class Sih : public ModuleBase, public ModuleParams void generate_fw_aerodynamics(); void generate_ts_aerodynamics(); void sensor_step(); - float computeGravity(double lat); + static float computeGravity(double lat); void ecefToNed(); - static matrix::Vector3d llaToEcef(double lat, double lon, double alt); - matrix::Dcmf computeRotEcefToNed(const double lat, const double lon, const double alt); + static matrix::Dcmf computeRotEcefToNed(const LatLonAlt &lla); struct Wgs84 { static constexpr double equatorial_radius = 6378137.0; @@ -207,9 +207,7 @@ class Sih : public ModuleBase, public ModuleParams matrix::Quatf _q{}; // quaternion attitude in local navigation frame matrix::Vector3f _w_B{}; // body rates in body frame [rad/s] - double _lat{0.0}; - double _lon{0.0}; - double _alt{0.0}; + LatLonAlt _lla{}; // Quantities in Earth-centered-Earth-fixed coordinates matrix::Vector3f _Fa_E{}; // aerodynamic force in ECEF frame [N] From cd18138b1cc6b4c92b838c4dc3c4d3ad3b188194 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 11:45:46 +0100 Subject: [PATCH 092/211] SIH: add transport rate acceleration to local acceleration --- src/modules/simulation/simulator_sih/sih.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 58cb8b51503c..a6d6f7c91f26 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -445,7 +445,11 @@ void Sih::equations_of_motion(const float dt) _specific_force_E = acceleration_E - gravity_acceleration_E; _v_E_dot = acceleration_E + coriolis_acceleration_E; - _v_N_dot = _R_N2E.transpose() * _v_E_dot; //TODO: add transport rate + + // add fictitious transport rate acceleration as the local navigation frame rotates + // to stay tangent to the ellipsoid + const Vector3f transport_rate = -_lla.computeAngularRateNavFrame(_v_N).cross(_v_N); + _v_N_dot = _R_N2E.transpose() * _v_E_dot + transport_rate; // forward Euler velocity intergation Vector3f v_E_prev = _v_E; From b30ea40c6d77ed2d14f9e3a4e1094a4132a9fe67 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 11:46:50 +0100 Subject: [PATCH 093/211] SIH: set GNSS delay to 0 as delay is not simulated --- ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx | 2 ++ .../px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane | 2 ++ ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert | 1 + 3 files changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx index 7c04d446509e..077d908ec5e8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx @@ -38,4 +38,6 @@ param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 +param set-default EKF2_GPS_DELAY 0 + param set SIH_VEHICLE_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index 000ac9766b94..36a69b7714a8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -49,3 +49,5 @@ param set-default PWM_MAIN_FUNC1 201 param set-default PWM_MAIN_FUNC2 202 param set-default PWM_MAIN_FUNC3 203 param set-default PWM_MAIN_FUNC4 101 + +param set-default EKF2_GPS_DELAY 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index 4910dbc8502f..d82b92c1ef98 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -11,6 +11,7 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert} +param set-default EKF2_GPS_DELAY 0 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters param set-default SENS_EN_GPSSIM 1 From 8b1975cb989d6e979ae66f39b6170c3e97044d07 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 26 Nov 2024 12:05:45 +0100 Subject: [PATCH 094/211] SIH: lower IMU noise before takeoff This speeds-up the EKF alignment --- src/modules/simulation/simulator_sih/sih.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index a6d6f7c91f26..ea6425f21a31 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -508,15 +508,27 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) // IMU const Dcmf R_E2B(_q_E.inversed()); + Vector3f accel_noise; + Vector3f gyro_noise; + + if (_T_B.longerThan(FLT_EPSILON)) { + accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f); + gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f); + + } else { + // Lower noise when not armed + accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f); + gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f); + } Vector3f specific_force_B = R_E2B * _specific_force_E; - Vector3f acc = specific_force_B + noiseGauss3f(0.5f, 1.7f, 1.4f); + Vector3f accel = specific_force_B + accel_noise; const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); - Vector3f gyro = _w_B + earth_spin_rate_B + noiseGauss3f(0.14f, 0.07f, 0.03f); + Vector3f gyro = _w_B + earth_spin_rate_B + gyro_noise; // update IMU every iteration - _px4_accel.update(time_now_us, acc(0), acc(1), acc(2)); + _px4_accel.update(time_now_us, accel(0), accel(1), accel(2)); _px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2)); } From db0160bf7cd2098b84ec4060865355c006ada715 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 28 Nov 2024 17:40:06 +0100 Subject: [PATCH 095/211] fix sih hitl plane airframe --- .../init.d/airframes/1101_rc_plane_sih.hil | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index a40409203902..e124e9981ac1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -18,22 +18,19 @@ param set UAVCAN_ENABLE 0 param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 param set-default CA_ROTOR0_PX 0.3 -param set-default CA_SV_CS_COUNT 4 -param set-default CA_SV_CS0_TRQ_R 0.5 -param set-default CA_SV_CS0_TYPE 2 +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TYPE 1 param set-default CA_SV_CS1_TRQ_P 1 param set-default CA_SV_CS1_TYPE 3 param set-default CA_SV_CS2_TRQ_Y 1 param set-default CA_SV_CS2_TYPE 4 -param set-default CA_SV_CS3_TYPE 10 - -param set-default HIL_ACT_REV 2 -param set-default HIL_ACT_FUNC1 201 -param set-default HIL_ACT_FUNC2 202 -param set-default HIL_ACT_FUNC3 203 -param set-default HIL_ACT_FUNC4 101 -param set-default HIL_ACT_FUNC5 204 -param set-default HIL_ACT_FUNC6 400 + +param set HIL_ACT_REV 1 +param set HIL_ACT_FUNC1 201 +param set HIL_ACT_FUNC2 202 +param set HIL_ACT_FUNC3 203 +param set HIL_ACT_FUNC4 101 # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set-default SYS_HITL 2 From b06ff99a3ea46b3955adc8f43f169e72d90ac4d7 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 29 Nov 2024 13:35:06 +0100 Subject: [PATCH 096/211] disable SIH on x21-777 and v6u to save flash --- boards/mro/x21-777/default.px4board | 2 +- boards/px4/fmu-v6u/default.px4board | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index 361aca448471..bf94e4b2c7ac 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -67,7 +67,7 @@ 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_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index e733dccc35fc..6addac12f3f4 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -68,7 +68,7 @@ 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_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y From 8880569b317476f1495bd37917eb7bcfbe65af67 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 11 Nov 2024 11:27:30 +0100 Subject: [PATCH 097/211] differential: adjust speed setpoint based on yaw rate setpoint --- .../RoverDifferentialControl.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index e864e3e31f43..a378de3b9428 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -116,6 +116,20 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con float forward_speed_normalized{0.f}; if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { + if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { + + forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, + -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), -1.f, 1.f); + + if (fabsf(forward_speed_normalized) > 1.f - + fabsf(speed_diff_normalized)) { // Adjust forward speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + _rover_differential_setpoint.forward_speed_setpoint = sign(_rover_differential_setpoint.forward_speed_setpoint) * + math::interpolate(1.f - fabsf(speed_diff_normalized), -1.f, 1.f, + -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get()); + } + } + + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false); From 7e705bbf5550f75525d0874317b9fcba5bdf544a Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 18 Nov 2024 16:27:28 +0100 Subject: [PATCH 098/211] differential: add slow down effect in mission mode --- .../init.d-posix/airframes/4009_gz_r1_rover | 1 - .../init.d-posix/airframes/4011_gz_lawnmower | 1 - .../airframes/50001_aion_robotics_r1_rover | 1 - .../RoverDifferentialGuidance.cpp | 14 ++++++++-- .../RoverDifferentialGuidance.hpp | 5 ++-- src/modules/rover_differential/module.yaml | 27 +++++++++++-------- 6 files changed, 30 insertions(+), 19 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index ffbdfad2f9be..3f96dad5f883 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -26,7 +26,6 @@ param set-default RD_MAX_THR_SPD 2.15 param set-default RD_SPEED_P 0.1 param set-default RD_SPEED_I 0.01 param set-default RD_MAX_YAW_RATE 180 -param set-default RD_MISS_SPD_DEF 2 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 param set-default RD_MAX_YAW_ACCEL 1000 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 5f2b635238f7..d4c80a29092f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -29,7 +29,6 @@ param set-default RD_MAX_SPEED 8 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0.1 param set-default RD_MAX_YAW_RATE 30 -param set-default RD_MISS_SPD_DEF 8 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 859d589d6ac5..7cdf2d10a673 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -31,7 +31,6 @@ param set-default RD_MAX_THR_SPD 1.9 param set-default RD_MAX_THR_YAW_R 0.7 param set-default RD_MAX_YAW_ACCEL 600 param set-default RD_MAX_YAW_RATE 250 -param set-default RD_MISS_SPD_DEF 1.5 param set-default RD_SPEED_P 0.1 param set-default RD_SPEED_I 0.01 param set-default RD_TRANS_DRV_TRN 0.785398 diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index 400cd643f3d7..bb45e6b7b99c 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -40,7 +40,7 @@ using namespace matrix; RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _max_forward_speed = _param_rd_miss_spd_def.get(); + _max_forward_speed = _param_rd_max_speed.get(); _rover_differential_guidance_status_pub.advertise(); } @@ -96,6 +96,16 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f _param_rd_max_decel.get(), distance_to_curr_wp, 0.0f); desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); } + + } else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON + && _param_rd_miss_spd_gain.get() > FLT_EPSILON) { + const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate( + M_PI_F - _waypoint_transition_angle, 0.f, + M_PI_F, 0.f, 1.f), 0.f, 1.f); + desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), + _param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction)); + desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, + _max_forward_speed); } } break; @@ -219,6 +229,6 @@ void RoverDifferentialGuidance::updateWaypoints() _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); } else { - _max_forward_speed = _param_rd_miss_spd_def.get(); + _max_forward_speed = _param_rd_max_speed.get(); } } diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index c5d3a3f1734f..cc1b03363537 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -144,9 +144,8 @@ class RoverDifferentialGuidance : public ModuleParams (ParamFloat) _param_rd_max_jerk, (ParamFloat) _param_rd_max_decel, (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_miss_spd_def, (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn - + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain ) }; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index d1de0cbddb9f..6297f3aafd78 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -191,17 +191,6 @@ parameters: decimal: 2 default: 2 - RD_MISS_SPD_DEF: - description: - short: Default forward speed for the rover during auto modes - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving @@ -228,3 +217,19 @@ parameters: increment: 0.01 decimal: 3 default: 0.174533 + + RD_MISS_SPD_GAIN: + description: + short: Tuning parameter for the speed reduction during waypoint transition + long: | + The waypoint transition speed is calculated as: + Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) + The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP + interpolated from [0, 180] -> [0, 1]. + Higher value -> More speed reduction during waypoint transitions. + type: float + min: 0.05 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 From 6cce4430056a3f4e7a876b81b76e93a22bceb463 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 12 Nov 2024 16:10:41 +0100 Subject: [PATCH 099/211] mecanum: deprecate RM_MISS_SPD_DEF --- .../init.d-posix/airframes/4015_gz_r1_rover_mecanum | 1 - msg/RoverMecanumSetpoint.msg | 2 +- .../RoverMecanumGuidance/RoverMecanumGuidance.cpp | 12 ++++-------- .../RoverMecanumGuidance/RoverMecanumGuidance.hpp | 1 - src/modules/rover_mecanum/module.yaml | 11 ----------- 5 files changed, 5 insertions(+), 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index e0968a3db58e..42ac895f4655 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -24,7 +24,6 @@ param set-default RM_MAX_THR_YAW_R 7.5 param set-default RM_YAW_P 5 param set-default RM_YAW_I 0.1 param set-default RM_MAX_YAW_RATE 180 -param set-default RM_MISS_SPD_DEF 3 param set-default RM_MISS_VEL_GAIN 1 param set-default RM_SPEED_I 0.01 param set-default RM_SPEED_P 0.1 diff --git a/msg/RoverMecanumSetpoint.msg b/msg/RoverMecanumSetpoint.msg index 8718c039a762..0cc9415be1bb 100644 --- a/msg/RoverMecanumSetpoint.msg +++ b/msg/RoverMecanumSetpoint.msg @@ -5,7 +5,7 @@ float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward s float32 lateral_speed_setpoint # [m/s] Desired lateral speed float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed float32 yaw_rate_setpoint # [rad/s] Desired yaw rate -float32 yaw_rate_setpoint_normalized # [-1, 1] Desired normalized yaw rate +float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels float32 yaw_setpoint # [rad] Desired yaw (heading) # TOPICS rover_mecanum_setpoint diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index 4ffab5491975..a3d623820ccd 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -41,7 +41,7 @@ using namespace time_literals; RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _max_velocity_magnitude = _param_rm_miss_spd_def.get(); + _max_velocity_magnitude = _param_rm_max_speed.get(); _rover_mecanum_guidance_status_pub.advertise(); } @@ -195,13 +195,9 @@ void RoverMecanumGuidance::updateWaypoints() _waypoint_transition_angle = acosf(cosin); // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > FLT_EPSILON) { - _max_velocity_magnitude = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, - _param_rm_max_speed.get()); - - } else { - _max_velocity_magnitude = _param_rm_miss_spd_def.get(); - } + _max_velocity_magnitude = position_setpoint_triplet.current.cruising_speed > FLT_EPSILON ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, + _param_rm_max_speed.get()) : _param_rm_max_speed.get(); // Waypoint yaw setpoint if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp index 509c2251b430..0970b83646e3 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -133,7 +133,6 @@ class RoverMecanumGuidance : public ModuleParams (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rm_max_jerk, (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_miss_spd_def, (ParamFloat) _param_rm_max_yaw_rate, (ParamFloat) _param_rm_miss_vel_gain ) diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 92f660042044..bc93fc5e0342 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -173,17 +173,6 @@ parameters: decimal: 2 default: 0.5 - RM_MISS_SPD_DEF: - description: - short: Default rover speed during a mission - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - RM_MISS_VEL_GAIN: description: short: Tuning parameter for the velocity reduction during waypoint transition From 54abc59283ebf38b4720b541eabd79d893366448 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 12 Nov 2024 16:29:36 +0100 Subject: [PATCH 100/211] mecanum: deprecate RM_MAN_YAW_SCALE --- .../airframes/4015_gz_r1_rover_mecanum | 17 +++++++------- src/modules/rover_mecanum/RoverMecanum.cpp | 22 ++++++++++++++----- src/modules/rover_mecanum/RoverMecanum.hpp | 9 ++++---- .../RoverMecanumControl.cpp | 4 ++-- .../RoverMecanumGuidance.cpp | 2 +- src/modules/rover_mecanum/module.yaml | 13 ----------- 6 files changed, 33 insertions(+), 34 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 42ac895f4655..711427e2c5a1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -13,7 +13,6 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge # Rover parameters param set-default RM_WHEEL_TRACK 0.3 -param set-default RM_MAN_YAW_SCALE 0.1 param set-default RM_YAW_RATE_I 0 param set-default RM_YAW_RATE_P 0.01 param set-default RM_MAX_ACCEL 3 @@ -41,23 +40,23 @@ param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 102 # right wheel front -param set-default SIM_GZ_WH_MIN1 0 -param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_MIN1 70 +param set-default SIM_GZ_WH_MAX1 130 param set-default SIM_GZ_WH_DIS1 100 param set-default SIM_GZ_WH_FUNC2 101 # left wheel front -param set-default SIM_GZ_WH_MIN2 0 -param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_MIN2 70 +param set-default SIM_GZ_WH_MAX2 130 param set-default SIM_GZ_WH_DIS2 100 param set-default SIM_GZ_WH_FUNC3 104 # right wheel back -param set-default SIM_GZ_WH_MIN3 0 -param set-default SIM_GZ_WH_MAX3 200 +param set-default SIM_GZ_WH_MIN3 70 +param set-default SIM_GZ_WH_MAX3 130 param set-default SIM_GZ_WH_DIS3 100 param set-default SIM_GZ_WH_FUNC4 103 # left wheel back -param set-default SIM_GZ_WH_MIN4 0 -param set-default SIM_GZ_WH_MAX4 200 +param set-default SIM_GZ_WH_MIN4 70 +param set-default SIM_GZ_WH_MAX4 130 param set-default SIM_GZ_WH_DIS4 100 param set-default SIM_GZ_WH_REV 10 diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index 2bb3d23c3f16..6c9538d6dd4d 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -81,7 +81,19 @@ void RoverMecanum::Run() rover_mecanum_setpoint.lateral_speed_setpoint = NAN; rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get(); + + if (_max_yaw_rate > FLT_EPSILON && _param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { + const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.yaw, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float speed_diff = scaled_yaw_rate_input * _param_rm_wheel_track.get() / 2.f; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, + -_param_rm_max_thr_yaw_r.get(), _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); + + } else { + rover_mecanum_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.yaw; + + } + rover_mecanum_setpoint.yaw_setpoint = NAN; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); @@ -100,7 +112,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = NAN; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); } @@ -119,7 +131,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = NAN; if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON @@ -157,7 +169,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = NAN; // Reset cruise control if the manual input changes @@ -226,7 +238,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.lateral_speed_setpoint = NAN; rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = 0.f; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = 0.f; rover_mecanum_setpoint.yaw_setpoint = NAN; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); break; diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index b2d89efbd31a..7837d436dc2d 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -133,9 +133,10 @@ class RoverMecanum : public ModuleBase, public ModuleParams, bool _armed{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_rm_man_yaw_scale, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_pp_lookahd_max + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_yaw_r, + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_pp_lookahd_max ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index 87c9f86079c4..ecb0ea61c2bd 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -103,8 +103,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl -1.f, 1.f); // Feedback } else { // Use normalized setpoint - speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f; + speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized) ? math::constrain( + _rover_mecanum_setpoint.speed_diff_setpoint_normalized, -1.f, 1.f) : 0.f; } // Speed control diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index a3d623820ccd..b72478d746e2 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -110,7 +110,7 @@ void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); } diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index bc93fc5e0342..fa1bd01e4beb 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -4,19 +4,6 @@ parameters: - group: Rover Mecanum definitions: - RM_MAN_YAW_SCALE: - description: - short: Manual yaw rate scale - long: | - In Manual mode the setpoint for the yaw rate received from the control stick - is scaled by this value. - type: float - min: 0.01 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RM_WHEEL_TRACK: description: short: Wheel track From 5dcccd999c4c127ba70b654464f9d0fa5a73d32b Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 12 Nov 2024 17:05:10 +0100 Subject: [PATCH 101/211] mecanum: add slew rates to yaw, yaw rate and speed setpoints --- msg/RoverMecanumStatus.msg | 22 +- src/modules/rover_mecanum/RoverMecanum.hpp | 2 +- .../RoverMecanumControl.cpp | 228 +++++++++++++----- .../RoverMecanumControl.hpp | 65 ++++- src/modules/rover_mecanum/module.yaml | 31 +++ 5 files changed, 265 insertions(+), 83 deletions(-) diff --git a/msg/RoverMecanumStatus.msg b/msg/RoverMecanumStatus.msg index d9bc7b127e55..7547fd5be652 100644 --- a/msg/RoverMecanumStatus.msg +++ b/msg/RoverMecanumStatus.msg @@ -1,13 +1,17 @@ uint64 timestamp # time since system start (microseconds) -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards -float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output of the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 measured_yaw # [rad] Measured yaw -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller -float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller +float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards +float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left +float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller +float32 measured_yaw # [rad] Measured yaw +float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller +float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller +float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller +float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller # TOPICS rover_mecanum_status diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 7837d436dc2d..7c57a1138f0e 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -66,7 +66,7 @@ static constexpr float YAW_RATE_THRESHOLD = static constexpr float SPEED_THRESHOLD = 0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still static constexpr float STICK_DEADZONE = - 0.3f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value + 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value using namespace time_literals; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index ecb0ea61c2bd..eb536e2ea40b 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -61,9 +61,19 @@ void RoverMecanumControl::updateParams() _pid_lateral_throttle.setOutputLimit(1.f); _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F; _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); _pid_yaw.setIntegralLimit(_max_yaw_rate); _pid_yaw.setOutputLimit(_max_yaw_rate); + + // Update slew rates + if (_max_yaw_rate > FLT_EPSILON) { + _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); + } + + if (_max_yaw_accel > FLT_EPSILON) { + _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); + } } void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, @@ -79,98 +89,191 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl // Closed loop yaw control if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { + _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt); + _rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); _pid_yaw.setSetpoint( - matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw)); // error as setpoint to take care of wrapping + matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - + vehicle_yaw)); // error as setpoint to take care of wrapping _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); + _rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; } else { _pid_yaw.resetIntegral(); + _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); } // Yaw rate control float speed_diff_normalized{0.f}; - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - if (_param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward - const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get(); - speed_diff_normalized = math::interpolate(speed_diff, -_param_rm_max_thr_yaw_r.get(), - _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); - } - - _pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint); - speed_diff_normalized = math::constrain(speed_diff_normalized + - _pid_yaw_rate.update(vehicle_yaw_rate, dt), - -1.f, 1.f); // Feedback + if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, + _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, false); + _rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState(); } else { // Use normalized setpoint - speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.speed_diff_setpoint_normalized, -1.f, 1.f) : 0.f; + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized, + vehicle_yaw_rate, + _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, true); } // Speed control - float forward_throttle{0.f}; - float lateral_throttle{0.f}; + float forward_speed_normalized{0.f}; + float lateral_speed_normalized{0.f}; if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint) && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control - // Closed loop forward speed control - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_throttle = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - } - - _pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint); - forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt); - - // Closed loop lateral speed control - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - lateral_throttle = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - } + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint, + vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); + lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint, + vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); + _rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); + _rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState(); + + + } else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) + && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized, + vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); + lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized, + vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - _pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint); - lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt); - - } else { // Use normalized setpoint - forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f; - lateral_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.lateral_speed_setpoint_normalized, -1.f, 1.f) : 0.f; } // Publish rover mecanum status (logging) - rover_mecanum_status_s rover_mecanum_status{}; - rover_mecanum_status.timestamp = _timestamp; - rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; - rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; - rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; - rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; - rover_mecanum_status.measured_yaw = vehicle_yaw; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); - rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); - rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); - _rover_mecanum_status_pub.publish(rover_mecanum_status); + _rover_mecanum_status.timestamp = _timestamp; + _rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; + _rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; + _rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; + _rover_mecanum_status.measured_yaw = vehicle_yaw; + _rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); + _rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); + _rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); + _rover_mecanum_status_pub.publish(_rover_mecanum_status); // Publish to motors actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_throttle, lateral_throttle, + computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); } -matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle, +float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, + const float max_thr_yaw_r, + const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, + PID &pid_yaw_rate, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; + } + + if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); + yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); + + } else { + yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (normalized) { + speed_diff_normalized = yaw_rate_with_accel_limit.getState(); + + } else { + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + _pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); + speed_diff_normalized = math::constrain(speed_diff_normalized + + _pid_yaw_rate.update(vehicle_yaw_rate, dt), + -1.f, 1.f); // Feedback + + + } + + return math::constrain(speed_diff_normalized, -1.f, 1.f); + +} + +float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint, + const float vehicle_speed, const float max_thr_spd, SlewRate &speed_setpoint_with_accel_limit, + PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; + } + + // Apply acceleration and deceleration limit + if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) { + if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); + speed_setpoint_with_accel_limit.update(speed_setpoint, dt); + + } else { + speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); + + } + + } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); + speed_setpoint_with_accel_limit.update(speed_setpoint, dt); + + } else { + speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + if (normalized) { + forward_speed_normalized = speed_setpoint_with_accel_limit.getState(); + + } else { // Closed loop speed control + + if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward + forward_speed_normalized = math::interpolate(speed_setpoint_with_accel_limit.getState(), + -max_thr_spd, max_thr_spd, + -1.f, 1.f); + } + + pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback + + } + + return math::constrain(forward_speed_normalized, -1.f, 1.f); + +} + +matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized, + float lateral_speed_normalized, float speed_diff) { // Prioritize ratio between forward and lateral speed over either magnitude - float combined_speed = fabsf(forward_throttle) + fabsf(lateral_throttle); + float combined_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized); if (combined_speed > 1.f) { - forward_throttle /= combined_speed; - lateral_throttle /= combined_speed; + forward_speed_normalized /= combined_speed; + lateral_speed_normalized /= combined_speed; combined_speed = 1.f; } @@ -179,20 +282,21 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr if (total_speed > 1.f) { const float excess_velocity = fabsf(total_speed - 1.f); - const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity; - const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity; + const float forward_throttle_temp = forward_speed_normalized - sign(forward_speed_normalized) * 0.5f * excess_velocity; + const float lateral_throttle_temp = lateral_speed_normalized - sign(lateral_speed_normalized) * 0.5f * excess_velocity; - if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) { - forward_throttle = forward_throttle_temp; - lateral_throttle = lateral_throttle_temp; + if (sign(forward_throttle_temp) == sign(forward_speed_normalized) + && sign(lateral_speed_normalized) == sign(lateral_throttle_temp)) { + forward_speed_normalized = forward_throttle_temp; + lateral_speed_normalized = lateral_throttle_temp; } else { - forward_throttle = lateral_throttle = 0.f; + forward_speed_normalized = lateral_speed_normalized = 0.f; } } // Calculate motor commands - const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff}; + const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff}; const Matrix input(input_data); const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; const Matrix m(m_data); diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp index 42cec627a077..53ae266383e6 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -43,7 +43,8 @@ #include #include #include - +#include +#include // Standard libraries #include @@ -89,6 +90,39 @@ class RoverMecanumControl : public ModuleParams void updateParams() override; private: + /** + * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. + * @param vehicle_yaw_rate Measured yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. + * @param wheel_track Wheel track [m]. + * @param dt Time since last update [s]. + * @param yaw_rate_with_accel_limit Yaw rate slew rate. + * @param pid_yaw_rate Yaw rate PID + * @param normalized Indicates if the forward speed setpoint is already normalized. + * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, + float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); + /** + * @brief Compute normalized speed setpoint and apply slew rates. + * to the speed setpoint and doing closed loop speed control if enabled. + * @param speed_setpoint Speed setpoint [m/s]. + * @param vehicle_speed Actual speed [m/s]. + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. + * @param speed_setpoint_with_accel_limit Speed slew rate. + * @param pid_throttle Throttle PID + * @param max_accel Maximum acceleration [m/s^2] + * @param max_decel Maximum deceleration [m/s^2] + * @param dt Time since last update [s]. + * @param normalized Indicates if the speed setpoint is already normalized. + * @return Normalized speed setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd, + SlewRate &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, + float dt, bool normalized); + /** * @brief Turn normalized speed setpoints into normalized motor commands. * @@ -105,30 +139,39 @@ class RoverMecanumControl : public ModuleParams // uORB publications uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; + rover_mecanum_status_s _rover_mecanum_status{}; // Variables rover_mecanum_setpoint_s _rover_mecanum_setpoint{}; hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; // Controllers PID _pid_forward_throttle; // PID for the closed loop forward speed control PID _pid_lateral_throttle; // PID for the closed loop lateral speed control PID _pid_yaw; // PID for the closed loop yaw control PID _pid_yaw_rate; // PID for the closed loop yaw rate control + SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; + SlewRate _lateral_speed_setpoint_with_accel_limit{0.f}; + SlewRate _yaw_rate_with_accel_limit{0.f}; + SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_spd, + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_spd, + (ParamFloat) _param_rm_max_accel, + (ParamFloat) _param_rm_max_decel, (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_yaw_rate_p, - (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_yaw, - (ParamFloat) _param_rm_i_gain_yaw, - (ParamInt) _param_r_rev + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_rm_max_yaw_accel, + (ParamFloat) _param_rm_yaw_rate_p, + (ParamFloat) _param_rm_yaw_rate_i, + (ParamFloat) _param_rm_p_gain_speed, + (ParamFloat) _param_rm_i_gain_speed, + (ParamFloat) _param_rm_p_gain_yaw, + (ParamFloat) _param_rm_i_gain_yaw, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index fa1bd01e4beb..bccbe3e31f63 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -29,6 +29,22 @@ parameters: decimal: 2 default: 90 + RM_MAX_YAW_ACCEL: + description: + short: Maximum allowed yaw acceleration for the rover + long: | + This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints + to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. + This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 + RM_MAX_THR_YAW_R: description: short: Yaw rate turning left/right wheels at max speed in opposite directions @@ -160,6 +176,21 @@ parameters: decimal: 2 default: 0.5 + RM_MAX_DECEL: + description: + short: Maximum deceleration + long: | + Maximum decelaration is used to limit the deceleration of the rover. + Set to -1 to disable, causing the rover to decelerate as fast as possible. + Caution: This disables the slow down effect in auto modes. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + RM_MISS_VEL_GAIN: description: short: Tuning parameter for the velocity reduction during waypoint transition From 2eda5659ebe17a8a0ecce85c21b7e9935254d2ed Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 18 Nov 2024 15:11:02 +0100 Subject: [PATCH 102/211] mecanum: fix inverse kinematics --- .../RoverMecanumControl.cpp | 30 +++++-------------- 1 file changed, 7 insertions(+), 23 deletions(-) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index eb536e2ea40b..fe179cd6e9ff 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -268,31 +268,15 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_spe float lateral_speed_normalized, float speed_diff) { - // Prioritize ratio between forward and lateral speed over either magnitude - float combined_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized); + const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(speed_diff); - if (combined_speed > 1.f) { - forward_speed_normalized /= combined_speed; - lateral_speed_normalized /= combined_speed; - combined_speed = 1.f; - } - - // Prioritize yaw rate over forward and lateral speed - const float total_speed = combined_speed + fabsf(speed_diff); - - if (total_speed > 1.f) { - const float excess_velocity = fabsf(total_speed - 1.f); - const float forward_throttle_temp = forward_speed_normalized - sign(forward_speed_normalized) * 0.5f * excess_velocity; - const float lateral_throttle_temp = lateral_speed_normalized - sign(lateral_speed_normalized) * 0.5f * excess_velocity; + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); + const float magnitude = (1.f - fabsf(speed_diff)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); + forward_speed_normalized *= magnitude * normalization; + lateral_speed_normalized *= magnitude * normalization; - if (sign(forward_throttle_temp) == sign(forward_speed_normalized) - && sign(lateral_speed_normalized) == sign(lateral_throttle_temp)) { - forward_speed_normalized = forward_throttle_temp; - lateral_speed_normalized = lateral_throttle_temp; - - } else { - forward_speed_normalized = lateral_speed_normalized = 0.f; - } } // Calculate motor commands From 369ce37d65c364cd42373eecec39427f147d3b02 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 18 Nov 2024 15:21:40 +0100 Subject: [PATCH 103/211] mecanum: adjust speed setpoints to always be feasible --- .../RoverMecanumControl.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index fe179cd6e9ff..c58123678452 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -124,6 +124,31 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint) && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control + + if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { + + forward_speed_normalized = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); + + lateral_speed_normalized = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); + + const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf( + speed_diff_normalized); + + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); + const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); + forward_speed_normalized *= magnitude * normalization; + lateral_speed_normalized *= magnitude * normalization; + _rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(forward_speed_normalized, -1.f, 1.f, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); + _rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(lateral_speed_normalized, -1.f, 1.f, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); + } + } + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint, vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); From a1b68fcac2e2268ac0cdbf8e9dd1aa3909753c4e Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 18 Nov 2024 15:51:36 +0100 Subject: [PATCH 104/211] mecanum: update SITL airframe parameters --- .../airframes/4015_gz_r1_rover_mecanum | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 711427e2c5a1..e252b3cfa0f2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -13,16 +13,18 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge # Rover parameters param set-default RM_WHEEL_TRACK 0.3 -param set-default RM_YAW_RATE_I 0 -param set-default RM_YAW_RATE_P 0.01 +param set-default RM_YAW_RATE_I 0.1 +param set-default RM_YAW_RATE_P 0.1 param set-default RM_MAX_ACCEL 3 +param set-default RM_MAX_DECEL 5 param set-default RM_MAX_JERK 5 -param set-default RM_MAX_SPEED 4 -param set-default RM_MAX_THR_SPD 7 -param set-default RM_MAX_THR_YAW_R 7.5 +param set-default RM_MAX_SPEED 2 +param set-default RM_MAX_THR_SPD 2.2 +param set-default RM_MAX_THR_YAW_R 1.2 param set-default RM_YAW_P 5 param set-default RM_YAW_I 0.1 -param set-default RM_MAX_YAW_RATE 180 +param set-default RM_MAX_YAW_RATE 120 +param set-default RM_MAX_YAW_ACCEL 240 param set-default RM_MISS_VEL_GAIN 1 param set-default RM_SPEED_I 0.01 param set-default RM_SPEED_P 0.1 From ff55313b0bee7d6db88c4e4dbd62fd40c6b6591f Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 26 Nov 2024 16:45:34 +0100 Subject: [PATCH 105/211] mecanum: update current position in main file --- src/modules/rover_mecanum/RoverMecanum.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index 6c9538d6dd4d..b0729acc024d 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -283,6 +283,7 @@ void RoverMecanum::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); // Apply threshold to the velocity measurement to cut off measurement noise when standing still From be2a3afb8307bbfe3cb20cde2cae0d2b266a6e82 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 2 Dec 2024 08:59:38 +0100 Subject: [PATCH 106/211] mecanum: update parameter description --- src/modules/rover_mecanum/module.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index bccbe3e31f63..8d70ec90f8ac 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -182,7 +182,7 @@ parameters: long: | Maximum decelaration is used to limit the deceleration of the rover. Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: This disables the slow down effect in auto modes. + Caution: Disabling the deceleration limit also disables the slow down effect in auto modes. type: float unit: m/s^2 min: -1 From 5ce2bf662b3aaa1822024d196f801382b6ddd32c Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 28 Nov 2024 21:40:46 +1100 Subject: [PATCH 107/211] airframes markdownout.py - br rather than p for generated code --- Tools/px4airframes/markdownout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px4airframes/markdownout.py b/Tools/px4airframes/markdownout.py index bf1a605723f7..866aa1abeff5 100644 --- a/Tools/px4airframes/markdownout.py +++ b/Tools/px4airframes/markdownout.py @@ -137,7 +137,7 @@ def __init__(self, groups, board, image_path): #print(output_name,value, attribstrs[0].strip(),attribstrs[1].strip()) outputs += '' if has_outputs: - outputs_entry = '

Specific Outputs:' + outputs + '

' + outputs_entry = '
Specific Outputs:' + outputs else: outputs_entry = '' From 72e758950b1538723dabed798094969886fce43a Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 19 Nov 2024 10:45:23 +0100 Subject: [PATCH 108/211] Included turning radius calculations for vertical changes and removed the requirement for the same altitude in the 2D turning radius logic. --- src/lib/motion_planning/PositionSmoothing.cpp | 25 +++++++++++++++++-- .../motion_planning/TrajectoryConstraints.hpp | 6 +---- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 28c70c690d60..ea93708bedc9 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,15 +106,36 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - + const auto &start_position = waypoints[0]; const auto &target = waypoints[1]; + const auto &next_target = waypoints[2]; Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), _trajectory[2].getCurrentPosition()); const float distance_start_target = fabs(target(2) - pos_traj(2)); - const float arrival_z_speed = 0.f; + float arrival_z_speed = 0.0f; + + const float distance_target_next = fabsf(target(2) - next_target(2)); + + const bool target_next_different = distance_target_next > 0.001f; + + Vector2f target_xy_z = {target.xy().norm(), target(2)}; + Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; + Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; + + if (target_next_different + ) { + const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( + Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero())); + + const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); + float accel_tmp = _trajectory[2].getMaxAccel(); + float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, + _vertical_acceptance_radius); + arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); + } float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index 6d52e1861e6e..ffe68840cb12 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -74,15 +74,11 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co const bool target_next_different = distance_target_next > 0.001f; const bool waypoint_overlap = distance_target_next < config.xy_accept_rad; - const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad; - const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad; float speed_at_target = 0.0f; if (target_next_different && - !waypoint_overlap && - has_reached_altitude && - altitude_stays_same + !waypoint_overlap ) { const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot( Vector2f((target - next_target).xy()).unit_or_zero())); From 06dde4ede83e0eda786239254aa8a725ab6116e3 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 19 Nov 2024 13:58:43 +0100 Subject: [PATCH 109/211] MPC: PositonSmoothing, change test to reflect that we have to come inwithing the acceptance radius, and not exact position. --- src/lib/motion_planning/PositionSmoothingTest.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp index ee06f8c2b1be..5cf45fd5fec8 100644 --- a/src/lib/motion_planning/PositionSmoothingTest.cpp +++ b/src/lib/motion_planning/PositionSmoothingTest.cpp @@ -151,14 +151,17 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration) TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) { - const int N_ITER = 2000; + const int N_ITER = 20000; const float DELTA_T = 0.02f; const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f}; const Vector3f TARGET{12.f, 17.f, 8.f}; const Vector3f NEXT_TARGET{8.f, 12.f, 80.f}; + const float XY_ACC_RAD = 10.f; + const float Z_ACC_RAD = 0.8f; + - Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET}; + Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET}; Vector3f ff_velocity{1.f, 0.1f, 0.3f}; Vector3f position{0.f, 0.f, 0.f}; @@ -180,12 +183,13 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) ff_velocity = {0.f, 0.f, 0.f}; expectDynamicsLimitsRespected(out); - if (position == TARGET) { + if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) { printf("Converged in %d iterations\n", iteration); break; } } - EXPECT_EQ(TARGET, position); + EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD); + EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD); EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n"; } From 092e5e8f9db434435807bf728dc71055bbe1b54a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 28 Nov 2024 16:19:09 +0100 Subject: [PATCH 110/211] TrajectoryConstraints: clarify waypoint indexing --- src/lib/motion_planning/TrajectoryConstraints.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index ffe68840cb12..cc52a984de30 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -104,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co * * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits */ -template +template float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config) { static_assert(N >= 2, "Need at least 2 points to compute speed"); float max_speed = 0.f; - for (size_t j = 0; j < N - 1; j++) { - size_t i = N - 2 - j; + // go backwards through the waypoints + for (int i = (N - 2); i >= 0; i--) { max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], waypoints[i + 1], waypoints[min(i + 2, N - 1)], From 7dcfeb2f77f3ba520076a4f5a1cda8eaec1fccd6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 28 Nov 2024 17:34:35 +0100 Subject: [PATCH 111/211] PositionSmoothing: refactor _getMaxZSpeed() --- src/lib/motion_planning/PositionSmoothing.cpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index ea93708bedc9..0d9f3acb8d66 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,27 +106,22 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - const auto &start_position = waypoints[0]; - const auto &target = waypoints[1]; - const auto &next_target = waypoints[2]; + const Vector3f &start_position = waypoints[0]; + const Vector3f &target = waypoints[1]; + const Vector3f &next_target = waypoints[2]; + + const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; + const Vector2f target_xy_z = {target.xy().norm(), target(2)}; + const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), _trajectory[2].getCurrentPosition()); - const float distance_start_target = fabs(target(2) - pos_traj(2)); float arrival_z_speed = 0.0f; + const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f; - const float distance_target_next = fabsf(target(2) - next_target(2)); - - const bool target_next_different = distance_target_next > 0.001f; - - Vector2f target_xy_z = {target.xy().norm(), target(2)}; - Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; - Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; - - if (target_next_different - ) { + if (target_next_different) { const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero())); @@ -137,6 +132,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); } + const float distance_start_target = fabs(target(2) - pos_traj(2)); float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), distance_start_target, arrival_z_speed)); From 1a165a4956801f779b6673019adeeebf62c0d35d Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 29 Nov 2024 10:51:13 +0100 Subject: [PATCH 112/211] Added possibility to modify the start position from external sources, as its done in the _getMaxXYSpeed --- src/lib/motion_planning/PositionSmoothing.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 0d9f3acb8d66..85c274ab5d5c 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,7 +106,10 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - const Vector3f &start_position = waypoints[0]; + const Vector3f &start_position = {_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition() + }; const Vector3f &target = waypoints[1]; const Vector3f &next_target = waypoints[2]; @@ -114,10 +117,6 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const const Vector2f target_xy_z = {target.xy().norm(), target(2)}; const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; - Vector3f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition(), - _trajectory[2].getCurrentPosition()); - float arrival_z_speed = 0.0f; const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f; @@ -132,7 +131,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); } - const float distance_start_target = fabs(target(2) - pos_traj(2)); + const float distance_start_target = fabs(target(2) - start_position(2)); float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), distance_start_target, arrival_z_speed)); From 4696338d297b7e64092eae28db82e77c6bc12030 Mon Sep 17 00:00:00 2001 From: Perre Date: Mon, 2 Dec 2024 17:27:23 +0100 Subject: [PATCH 113/211] Add gz model for quadtailsitter (#23943) * Add gazebo airspeed plugin and add a tailsitter model --------- Co-authored-by: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> --- .../airframes/4018_gz_quadtailsitter | 97 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 15 ++- src/modules/simulation/gz_bridge/GZBridge.hpp | 5 +- 5 files changed, 108 insertions(+), 12 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter new file mode 100644 index 000000000000..9856b1bf273e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -0,0 +1,97 @@ +#!/bin/sh +# +# @name Quadrotor + Tailsitter +# +# @type VTOL Quad Tailsitter +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + + +param set-default MAV_TYPE 20 + +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.23 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.23 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.23 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.23 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_SV_CS_COUNT 0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 + +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MIN4 10 + +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default FD_FAIL_R 70 + +param set-default FW_P_TC 0.6 + +param set-default FW_PR_I 0.3 +param set-default FW_PR_P 0.5 +param set-default FW_PSP_OFF 2 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_I 0.1 +param set-default FW_RR_P 0.2 +param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P +param set-default FW_YR_I 0 +param set-default FW_THR_TRIM 0.35 +param set-default FW_THR_MAX 0.8 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 6 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 3 +param set-default FW_T_SINK_MIN 1.6 +param set-default FW_AIRSPD_STALL 10 +param set-default FW_AIRSPD_MIN 14 +param set-default FW_AIRSPD_TRIM 18 +param set-default FW_AIRSPD_MAX 22 + +param set-default MC_AIRMODE 2 +param set-default MAN_ARM_GESTURE 0 # required for yaw airmode +param set-default MC_ROLL_P 3 +param set-default MC_PITCH_P 3 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_PITCHRATE_P 0.3 + +param set-default VT_ARSP_TRANS 15 +param set-default VT_B_TRANS_DUR 5 +param set-default VT_FW_DIFTHR_EN 7 +param set-default VT_FW_DIFTHR_S_Y 1 +param set-default VT_F_TRANS_DUR 1.5 +param set-default VT_TYPE 0 + +param set-default WV_EN 0 + +param set-default EKF2_FUSE_BETA 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 96e1737e5d53..1987464e319b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -89,6 +89,7 @@ px4_add_romfs_files( 4015_gz_r1_rover_mecanum 4016_gz_x500_lidar_down 4017_gz_x500_lidar_front + 4018_gz_quadtailsitter 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 9e47793f2bc1..019f63e2d507 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 9e47793f2bc18aa7cde39b1fc1c4b7bbc67e04ba +Subproject commit 019f63e2d50763862b8f8d40cce77371b3260c52 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 95be696835e2..61aa5d0aa4e1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -226,17 +226,15 @@ int GZBridge::init() PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str()); } -#if 0 // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed - std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + - "/link/airspeed_link/sensor/air_speed/air_speed"; + std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; - if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str()); return PX4_ERROR; } -#endif // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; @@ -449,8 +447,8 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) pthread_mutex_unlock(&_node_mutex); } -#if 0 -void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) + +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) { if (hrt_absolute_time() == 0) { return; @@ -475,7 +473,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) pthread_mutex_unlock(&_node_mutex); } -#endif void GZBridge::imuCallback(const gz::msgs::IMU &imu) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index f31ecf062f68..e4c6800755e5 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -68,6 +68,7 @@ #include #include +#include #include #include #include @@ -106,7 +107,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void clockCallback(const gz::msgs::Clock &clock); - // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); + void airspeedCallback(const gz::msgs::AirSpeed &air_speed); void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); @@ -167,8 +168,8 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; From 6b637f82f8bda3b06d8e65534dd1ae063879fce8 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Dec 2024 18:17:17 +0100 Subject: [PATCH 114/211] lla; fix conversion to ECEF --- src/lib/lat_lon_alt/lat_lon_alt.cpp | 2 +- src/lib/lat_lon_alt/test_lat_lon_alt.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/lib/lat_lon_alt/lat_lon_alt.cpp b/src/lib/lat_lon_alt/lat_lon_alt.cpp index d9ac442e3a8f..40b86a635a51 100644 --- a/src/lib/lat_lon_alt/lat_lon_alt.cpp +++ b/src/lib/lat_lon_alt/lat_lon_alt.cpp @@ -77,7 +77,7 @@ Vector3d LatLonAlt::toEcef() const return Vector3d(r_total * cos_lat * cos_lon, r_total * cos_lat * sin_lon, - ((1.0 - Wgs84::eccentricity2) * r_total) * sin_lat); + ((1.0 - Wgs84::eccentricity2) * r_e + static_cast(_altitude)) * sin_lat); } Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const diff --git a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp index b7a42ebd48d3..f2419951d5b2 100644 --- a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp +++ b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp @@ -105,3 +105,17 @@ TEST(TestLatLonAlt, subLatLonAlt) EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2); EXPECT_EQ(delta_pos(2), delta_pos_true(2)); } + +TEST(TestLatLonAlt, fromAndToECEF) +{ + for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) { + for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) { + for (float alt = -500.f; alt < 8000.f; alt += 500.f) { + LatLonAlt lla(lat, lon, alt); + + LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef()); + EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt; + } + } + } +} From 8626019ae017fcdd203be23140ccf3208ad87427 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Dec 2024 18:18:14 +0100 Subject: [PATCH 115/211] EKF2: reset global position using variance --- .../aid_sources/aux_global_position/aux_global_position.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 6 +++--- src/modules/ekf2/test/test_EKF_flow.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 973a70dbe439..1d425cb85384 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -110,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } else { // Try to initialize using measurement - if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, - sample.epv)) { + if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, + sq(sample.epv))) { ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index aa4597b25d9c..f578a929aae8 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -191,9 +191,9 @@ class Ekf final : public EstimatorInterface void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool checkLatLonValidity(double latitude, double longitude); bool checkAltitudeValidity(float altitude); - bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN); - bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN, - float epv = NAN); + bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float hpos_var = NAN, float vpos_var = NAN); + bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float hpos_var = NAN, + float vpos_var = NAN); // 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; diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index bc33c350e792..7e0070397168 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -383,7 +383,7 @@ TEST_F(EkfFlowTest, deadReckoning) const float altitude_new = 1500.0; const float eph = 50.f; const float epv = 10.f; - _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv); + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph * eph, epv * epv); const Vector3f lpos_after_reset = _ekf->getPosition(); From dfa48f988dc6e59a7327cffb3f5100bac5a8105a Mon Sep 17 00:00:00 2001 From: Perre Date: Tue, 3 Dec 2024 17:14:09 +0100 Subject: [PATCH 116/211] ESC check: Avoid unsigned timestamp underflow in telemtry timeout (#24069) * Avoid unsigned integer underflow * ESC check: add brackets to timeout for readability --------- Co-authored-by: Matthias Grob --- src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 9554c57a4f62..9b35c80992fa 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -77,7 +77,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter) esc_status_s esc_status; - if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) { + if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) { checkEscStatus(context, reporter, esc_status); reporter.setIsPresent(health_component_t::motors_escs); From d416cd2a6cfc34a59cf8f85c25e7c90286049ba7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 2 Dec 2024 16:08:41 +0100 Subject: [PATCH 117/211] Commander: remove `COM_POS_FS_DELAY` A user configurable delay for the internal `vehicle_local_position` seems confusing in my eyes. It's a different timeout for fixed-wing and multirotor which might have made sense earlier but not really anymore since the topic is constantly published by the estimator and not expected to time out on either vehicle type and the parameter description is also misleading because it's outdated. --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 -- .../HealthAndArmingChecks/checks/estimatorCheck.cpp | 4 ++-- .../HealthAndArmingChecks/checks/estimatorCheck.hpp | 1 - src/modules/commander/commander_params.c | 13 ------------- 4 files changed, 2 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 32991e59b98c..db15c3d09d95 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -13,8 +13,6 @@ param set-default MAV_TYPE 1 # # Default parameters for fixed wing UAVs. # -param set-default COM_POS_FS_DELAY 5 - # there is a 2.5 factor applied on the _FS thresholds if for invalidation param set-default COM_POS_FS_EPH 50 param set-default COM_VEL_FS_EVH 3 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 20dfdfb073e7..1cd140fe5bae 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -813,7 +813,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // altitude - failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s)); + failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 1_s); // attitude @@ -864,7 +864,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat const bool was_valid) const { bool valid = was_valid; - const bool data_stale = (now > data_timestamp_us + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0); + const bool data_stale = (now > data_timestamp_us + 1_s) || (data_timestamp_us == 0); const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index dd96769ce07f..b5cc6c21853f 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -118,7 +118,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase (ParamBool) _param_sys_has_gps, (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_vel_fs_evh, - (ParamInt) _param_com_pos_fs_delay, (ParamFloat) _param_com_low_eph, (ParamInt) _param_com_pos_low_act ) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0d3cf5f25f6f..48b0cce5221a 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -512,19 +512,6 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0); */ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1); -/** - * Loss of position failsafe activation delay. - * - * This sets number of seconds that the position checks need to be failed before the failsafe will activate. - * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used. - * - * @unit s - * @group Commander - * @min 1 - * @max 100 - */ -PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1); - /** * Horizontal position error threshold. * From 28487350d3cd7e6730cec2773e99d9a652666d31 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 25 Nov 2024 10:20:47 -0800 Subject: [PATCH 118/211] ci: update actions and images Updates some actions to run build steps in container instead of the whole workflow. --- .github/workflows/checks.yml | 41 +++--- .github/workflows/clang-tidy.yml | 16 ++- .github/workflows/compile_macos.yml | 8 +- .../ekf_functional_change_indicator.yml | 23 +-- .../workflows/ekf_update_change_indicator.yml | 31 ++-- .github/workflows/failsafe_sim.yml | 36 ++--- .github/workflows/mavros_mission_tests.yml | 133 ++---------------- .github/workflows/mavros_offboard_tests.yml | 125 ++-------------- .github/workflows/nuttx_env_config.yml | 29 ++-- .github/workflows/python_checks.yml | 8 +- 10 files changed, 140 insertions(+), 310 deletions(-) diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index a2d7aa5f97a8..1e8d2238a1a4 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -25,28 +25,27 @@ jobs: "NO_NINJA_BUILD=1 px4_fmu-v5_default", "NO_NINJA_BUILD=1 px4_sitl_default", "px4_sitl_allyes", - "airframe_metadata", "module_documentation", - "parameters_metadata", ] - container: - image: px4io/px4-dev-nuttx-focal:2022-08-12 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} + - uses: actions/checkout@v4 + with: + fetch-depth: 0 - - name: check environment - run: | - export - ulimit -a - - name: ${{matrix.check}} - run: make ${{matrix.check}} - - name: upload coverage - if: contains(matrix.check, 'coverage') - uses: codecov/codecov-action@v1 - with: - token: ${{ secrets.CODECOV_TOKEN }} - flags: unittests - file: coverage/lcov.info + - name: Building [${{ matrix.check }}] + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-nuttx-focal:2022-08-12 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make ${{ matrix.check }} + + - name: Uploading Coverage to Codecov.io + if: contains(matrix.check, 'coverage') + uses: codecov/codecov-action@v1 + with: + token: ${{ secrets.CODECOV_TOKEN }} + flags: unittests + file: coverage/lcov.info diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index 2736767e0bb7..89c1ffe1ec37 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -11,13 +11,17 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-clang:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{secrets.ACCESS_TOKEN}} + fetch-depth: 0 - - uses: corrupt952/actions-retry-command@v1.0.7 + - name: Testing (clang-tidy-quiet) + uses: addnab/docker-run-action@v3 with: - command: make clang-tidy-quiet - max_attempts: 3 + image: px4io/px4-dev-clang:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make clang-tidy-quiet diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml index 5c45a7710845..c9b83bdfec2b 100644 --- a/.github/workflows/compile_macos.yml +++ b/.github/workflows/compile_macos.yml @@ -19,13 +19,11 @@ jobs: ] steps: - name: install Python 3.10 - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: "3.10" - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} + - uses: actions/checkout@v4 - name: setup run: ./Tools/setup/macos.sh; ./Tools/setup/macos.sh @@ -37,7 +35,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v4 with: path: ~/.ccache key: macos_${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/ekf_functional_change_indicator.yml b/.github/workflows/ekf_functional_change_indicator.yml index a5fa9db59955..7fa9a834aa0d 100644 --- a/.github/workflows/ekf_functional_change_indicator.yml +++ b/.github/workflows/ekf_functional_change_indicator.yml @@ -1,21 +1,28 @@ name: EKF Change Indicator -on: pull_request +on: + pull_request: + branches: + - '*' jobs: unit_tests: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v2.3.1 + - uses: actions/checkout@v4 with: fetch-depth: 0 - - name: checkout newest version of branch - run: | - git fetch origin pull/${{github.event.pull_request.number}}/head:${{github.head_ref}} - git checkout ${GITHUB_HEAD_REF} + - name: main test - run: make tests TESTFILTER=EKF + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-base-focal:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make tests TESTFILTER=EKF + - name: Check if there is a functional change run: git diff --exit-code working-directory: src/modules/ekf2/test/change_indication diff --git a/.github/workflows/ekf_update_change_indicator.yml b/.github/workflows/ekf_update_change_indicator.yml index 7de35207f096..ecf4f2a5644e 100644 --- a/.github/workflows/ekf_update_change_indicator.yml +++ b/.github/workflows/ekf_update_change_indicator.yml @@ -5,25 +5,40 @@ on: push jobs: unit_tests: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 env: GIT_COMMITTER_EMAIL: bot@px4.io GIT_COMMITTER_NAME: PX4BuildBot steps: - - uses: actions/checkout@v2.3.1 + - uses: actions/checkout@v4 with: fetch-depth: 0 - - name: main test updates change indication files - run: make tests TESTFILTER=EKF + + - name: main test + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-base-focal:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make tests TESTFILTER=EKF + - name: Check if there exists diff and save result in variable - run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_ENV + id: diff-check + run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_OUTPUT working-directory: src/modules/ekf2/test/change_indication + - name: auto-commit any changes to change indication uses: stefanzweifel/git-auto-commit-action@v4 with: - commit_message: '[AUTO COMMIT] update change indication' + file_pattern: 'src/modules/ekf2/test/change_indication/*.csv' commit_user_name: ${GIT_COMMITTER_NAME} commit_user_email: ${GIT_COMMITTER_EMAIL} - - if: ${{env.CHANGE_INDICATED}} - name: if there is a functional change, fail check + commit_message: | + '[AUTO COMMIT] update change indication' + + See .github/workflopws/ekf_update_change_indicator.yml for more details + + - name: if there is a functional change, fail check + if: ${{ steps.diff-check.outputs.CHANGE_INDICATED }} run: exit 1 diff --git a/.github/workflows/failsafe_sim.yml b/.github/workflows/failsafe_sim.yml index 4d2ed21728a0..6210b713dca6 100644 --- a/.github/workflows/failsafe_sim.yml +++ b/.github/workflows/failsafe_sim.yml @@ -24,21 +24,23 @@ jobs: image: px4io/px4-dev-nuttx-focal:2022-08-12 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} + - name: Install Node v20.18.0 + uses: actions/setup-node@v4 + with: + node-version: 20.18.0 - - name: check environment - run: | - export - ulimit -a - - name: install emscripten - run: | - git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk - cd _emscripten_sdk - ./emsdk install latest - ./emsdk activate latest - - name: ${{matrix.check}} - run: | - . ./_emscripten_sdk/emsdk_env.sh - make ${{matrix.check}} + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Install empscripten + run: | + git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk + cd _emscripten_sdk + ./emsdk install latest + ./emsdk activate latest + + - name: Testing [${{ matrix.check }}] + run: | + . ./_emscripten_sdk/emsdk_env.sh + make ${{ matrix.check }} diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index f1b6737334cb..b2807f988f6b 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -11,131 +11,26 @@ on: jobs: build: runs-on: ubuntu-latest - env: - ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true strategy: fail-fast: false matrix: config: - - {vehicle: "iris", mission: "MC_mission_box", build_type: "RelWithDebInfo"} - - {vehicle: "rover", mission: "rover_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "plane", mission: "FW_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "plane_catapult",mission: "FW_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "standard_vtol", mission: "VTOL_mission_1", build_type: "Coverage"} - #- {vehicle: "standard_vtol", mission: "VTOL_mission_1", build_type: "AddressSanitizer"} - #- {vehicle: "tailsitter", mission: "VTOL_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "tiltrotor", mission: "VTOL_mission_1", build_type: "RelWithDebInfo"} + - {vehicle: "iris", mission: "MC_mission_box"} + - {vehicle: "rover", mission: "rover_mission_1"} - container: - image: px4io/px4-dev-ros-melodic:2021-09-08 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{ secrets.ACCESS_TOKEN }} + fetch-depth: 0 - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 + - name: Build SITL and Run Tests + uses: addnab/docker-run-action@v3 with: - path: ~/.ccache - key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: check environment - run: | - export - ulimit -a - - name: Build PX4 and sitl_gazebo-classic - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - ccache -z - make px4_sitl_default - make px4_sitl_default sitl_gazebo-classic - ccache -s - - - name: Core dump settings - run: | - ulimit -c unlimited - echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern - - - name: Run SITL tests - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - export - ./test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}} - timeout-minutes: 45 - - - name: Look at core files - if: failure() - run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - - name: Upload px4 coredump - if: failure() - uses: actions/upload-artifact@v3 - with: - name: coredump - path: px4.core - - - name: ecl EKF analysis - if: always() - run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg || true - - - name: Upload logs to flight review - if: always() - run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg - - - name: Upload px4 binary - if: failure() - uses: actions/upload-artifact@v3 - with: - name: binary - path: build/px4_sitl_default/bin/px4 - - - name: Store PX4 log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: px4_log - path: ~/.ros/log/*/*.ulg - - - name: Store ROS log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: ros_log - path: ~/.ros/**/rostest-*.log - - # Report test coverage - - name: Upload coverage - if: contains(matrix.config.build_type, 'Coverage') - run: | - git config --global credential.helper "" # disable the keychain credential helper - git config --global --add credential.helper store # enable the local store credential helper - echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential - git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential - mkdir -p coverage - lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - - name: Upload coverage information to Codecov - if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 - with: - token: ${{ secrets.CODECOV_TOKEN }} - flags: mavros_mission - file: coverage/lcov.info + image: px4io/px4-dev-ros-melodic:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make px4_sitl_default + make px4_sitl_default sitl_gazebo-classic + ./test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}} diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index c45c45ac0bc5..a968af24ef25 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -17,120 +17,21 @@ jobs: fail-fast: false matrix: config: - - {test_file: "mavros_posix_tests_offboard_posctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} - #- {test_file: "mavros_posix_tests_offboard_attctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} - #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} + - {test_file: "mavros_posix_tests_offboard_posctl.test", vehicle: "iris"} - container: - image: px4io/px4-dev-ros-melodic:2021-09-08 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{ secrets.ACCESS_TOKEN }} + fetch-depth: 0 - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 + - name: Build PX4 and Run Tests + uses: addnab/docker-run-action@v3 with: - path: ~/.ccache - key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: check environment - run: | - export - ulimit -a - - name: Build PX4 and sitl_gazebo-classic - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - ccache -z - make px4_sitl_default - make px4_sitl_default sitl_gazebo-classic - ccache -s - - - name: Core dump settings - run: | - ulimit -c unlimited - echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern - - - name: Run SITL tests - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - export - ./test/rostest_px4_run.sh ${{matrix.config.test_file}} vehicle:=${{matrix.config.vehicle}} - timeout-minutes: 45 - - - name: Look at core files - if: failure() - run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - - name: Upload px4 coredump - if: failure() - uses: actions/upload-artifact@v3 - with: - name: coredump - path: px4.core - - - name: ecl EKF analysis - if: always() - run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg || true - - - name: Upload logs to flight review - if: always() - run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg - - - name: Upload px4 binary - if: failure() - uses: actions/upload-artifact@v3 - with: - name: binary - path: build/px4_sitl_default/bin/px4 - - - name: Store PX4 log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: px4_log - path: ~/.ros/log/*/*.ulg - - - name: Store ROS log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: ros_log - path: ~/.ros/**/rostest-*.log - - # Report test coverage - - name: Upload coverage - if: contains(matrix.config.build_type, 'Coverage') - run: | - git config --global credential.helper "" # disable the keychain credential helper - git config --global --add credential.helper store # enable the local store credential helper - echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential - git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential - mkdir -p coverage - lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - - name: Upload coverage information to Codecov - if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 - with: - token: ${{ secrets.CODECOV_TOKEN }} - flags: mavros_offboard - file: coverage/lcov.info + image: px4io/px4-dev-ros-melodic:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make px4_sitl_default + make px4_sitl_default sitl_gazebo-classic + ./test/rostest_px4_run.sh ${{matrix.config.test_file}} vehicle:=${{matrix.config.vehicle}} diff --git a/.github/workflows/nuttx_env_config.yml b/.github/workflows/nuttx_env_config.yml index d151444ea9c1..4a655ffc7a96 100644 --- a/.github/workflows/nuttx_env_config.yml +++ b/.github/workflows/nuttx_env_config.yml @@ -11,22 +11,27 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2022-08-12 strategy: matrix: config: [ - px4_fmu-v5, + px4_fmu-v5_default, ] + steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{secrets.ACCESS_TOKEN}} + fetch-depth: 0 - - name: make ${{matrix.config}} - env: - PX4_EXTRA_NUTTX_CONFIG: "CONFIG_NSH_LOGIN_PASSWORD=\"test\";CONFIG_NSH_CONSOLE_LOGIN=y" - run: | - echo "PX4_EXTRA_NUTTX_CONFIG: $PX4_EXTRA_NUTTX_CONFIG" - make ${{matrix.config}} nuttx_context - # Check that the config option is set - grep CONFIG_NSH_LOGIN_PASSWORD build/${{matrix.config}}_default/NuttX/nuttx/.config + - name: Build PX4 and Run Test [${{ matrix.config }}] + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-nuttx-focal:2022-08-12 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + export PX4_EXTRA_NUTTX_CONFIG="CONFIG_NSH_LOGIN_PASSWORD=\"test\";CONFIG_NSH_CONSOLE_LOGIN=y" + echo "PX4_EXTRA_NUTTX_CONFIG: $PX4_EXTRA_NUTTX_CONFIG" + make ${{ matrix.config }} nuttx_context + echo "Check that the config option is set" + grep CONFIG_NSH_LOGIN_PASSWORD build/${{ matrix.config }}/NuttX/nuttx/.config diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index 9503fc99ebe2..3c273ba12497 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -12,14 +12,18 @@ jobs: build: runs-on: ubuntu-24.04 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{ secrets.ACCESS_TOKEN }} + fetch-depth: 0 + - name: Install Python3 run: sudo apt-get install python3 python3-setuptools python3-pip -y + - name: Install tools run: python3 -m pip install mypy types-requests flake8 --break-system-packages + - name: Check MAVSDK test scripts with mypy run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py + - name: Check MAVSDK test scripts with flake8 run: $HOME/.local/bin/flake8 test/mavsdk_tests/*.py From 1778692ca29531ee12c4635fd913caa3288d2bfa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Dec 2024 08:30:20 +1300 Subject: [PATCH 119/211] gps: heading fixes for NMEA/Unicore This updates the GPS submodule which includes NMEA/Unicore fixes: - Add correct return value for sat infos - Only publish on position updates - Request required topics at 5 Hz for Unicore --- src/drivers/gps/devices | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index fb151bcaa269..e048340d0f6a 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit fb151bcaa2690b56d4b16aa8de9fe2448f637c3b +Subproject commit e048340d0f6a395a3189292c33d08174bb309143 From ae61b4bfe0cd152b055de12110159f4cbb90f59e Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 4 Dec 2024 21:24:56 -0900 Subject: [PATCH 120/211] params: flash: erase corrupt sector (#24065) --- src/lib/parameters/flashparams/flashfs32.c | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/src/lib/parameters/flashparams/flashfs32.c b/src/lib/parameters/flashparams/flashfs32.c index 5680beb5a9f7..848623b26195 100644 --- a/src/lib/parameters/flashparams/flashfs32.c +++ b/src/lib/parameters/flashparams/flashfs32.c @@ -1126,19 +1126,8 @@ int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16 /* No paramaters */ if (pf == NULL) { - size_t total_size = size + sizeof(flash_entry_header_t); - size_t alignment = 31;//32-byte flash line - 1 - size_t size_adjust = ((total_size + alignment) & ~alignment) - total_size; - total_size += size_adjust; - - /* Do we have free space ?*/ - - if (find_free(total_size) == NULL) { - - /* No parameters and no free space => need erase */ - - rv = parameter_flashfs_erase(); - } + // Parameters can't be found, assume sector is corrupt or empty + rv = parameter_flashfs_erase(); } return rv; From 2da944a834e0866072cfae72138b56df0c80703d Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Tue, 19 Nov 2024 10:36:21 +0200 Subject: [PATCH 121/211] Logger: combined key & cipher file Use one single .ulge file to store both wrapped symmetric key and encrypted ulog data instead of creating separate .ulgk/ulgc files --- src/modules/logger/log_writer_file.cpp | 31 ++++++-------------------- src/modules/logger/logger.cpp | 2 +- 2 files changed, 8 insertions(+), 25 deletions(-) diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index 2001baf2f61a..14a2ad01368f 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -152,28 +152,7 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) rsa_crypto.close(); // Write the encrypted key to the disk - - // Allocate a buffer for filename - size_t fnlen = strlen(filename); - char *tmp_buf = (char *)malloc(fnlen + 1); - - if (!tmp_buf) { - PX4_ERR("out of memory"); - free(key); - return false; - } - - // Copy the original logfile name, and append 'k' to the filename - - memcpy(tmp_buf, filename, fnlen + 1); - tmp_buf[fnlen - 1] = 'k'; - tmp_buf[fnlen] = 0; - - int key_fd = ::open((const char *)tmp_buf, O_CREAT | O_WRONLY, PX4_O_MODE_666); - - // The file name is no longer needed, free it - free(tmp_buf); - tmp_buf = nullptr; + int key_fd = ::open((const char *)filename, O_CREAT | O_WRONLY | O_DIRECT | O_SYNC, PX4_O_MODE_666); if (key_fd < 0) { PX4_ERR("Can't open key file, errno: %d", errno); @@ -181,9 +160,9 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) return false; } - // write the header to the key exchange file + // write the header to the combined key exchange & cipherdata file struct ulog_key_header_s keyfile_header = { - .magic = {'U', 'L', 'o', 'g', 'K', 'e', 'y'}, + .magic = {'U', 'L', 'o', 'g', 'E', 'n', 'c'}, .hdr_ver = 1, .timestamp = hrt_absolute_time(), .exchange_algorithm = CRYPTO_RSA_OAEP, @@ -658,7 +637,11 @@ size_t LogWriterFile::LogFileBuffer::get_read_ptr(void **ptr, bool *is_part) bool LogWriterFile::LogFileBuffer::start_log(const char *filename) { +#if defined(PX4_CRYPTO) + _fd = ::open(filename, O_APPEND | O_WRONLY, PX4_O_MODE_666); +#else _fd = ::open(filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); +#endif _had_write_error.store(false); if (_fd < 0) { diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ff5221d8b61b..99ec75898c2c 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -1307,7 +1307,7 @@ int Logger::get_log_file_name(LogType type, char *file_name, size_t file_name_si #if defined(PX4_CRYPTO) if (_param_sdlog_crypto_algorithm.get() != 0) { - crypto_suffix = "c"; + crypto_suffix = "e"; } #endif From 814b243931796bd9dd907605ec374b45ecbd382d Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Tue, 19 Nov 2024 10:42:26 +0200 Subject: [PATCH 122/211] Tools/decrypt_ulog.py: support for .ulge log files --- Tools/decrypt_ulog.py | 67 +++++++++++++++++++++++++++++++------------ 1 file changed, 48 insertions(+), 19 deletions(-) diff --git a/Tools/decrypt_ulog.py b/Tools/decrypt_ulog.py index f6d888c56b62..3015686b9749 100755 --- a/Tools/decrypt_ulog.py +++ b/Tools/decrypt_ulog.py @@ -1,62 +1,91 @@ #!/usr/bin/env python3 +import sys + +try: + from Crypto.Cipher import ChaCha20 +except ImportError as e: + print("Failed to import crypto: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pycryptodome") + print("") + sys.exit(1) + from Crypto.PublicKey import RSA from Crypto.Cipher import PKCS1_OAEP -from Crypto.Cipher import ChaCha20 from Crypto.Hash import SHA256 -import binascii +from pathlib import Path import argparse -#from pathlib import Path -import sys + if __name__ == "__main__": parser = argparse.ArgumentParser(description="""CLI tool to decrypt an ulog file\n""") - parser.add_argument("ulog_file", help=".ulog file", nargs='?', default=None) - parser.add_argument("ulog_key", help=".ulogk, encrypted key", nargs='?', default=None) + parser.add_argument("ulog_file", help=".ulge/.ulgc, encrypted log file", nargs='?', default=None) + parser.add_argument("ulog_key", help=".ulgk, legacy encrypted key (give empty string '' to ignore for .ulge)", nargs='?', default=None) parser.add_argument("rsa_key", help=".pem format key for decrypting the ulog key", nargs='?', default=None) args = parser.parse_args() - # Only generate a key pair, don't sign - if not args.ulog_file or not args.ulog_key or not args.rsa_key: - print('Need all arguments, the encrypted ulog file, the key and the key decryption key') - sys.exit(1); + # Check all arguments are given + if not args.rsa_key: + print('Need all arguments, the encrypted ulog file, key file (or empty string if not needed) and the key decryption key (.pem)') + sys.exit(1) # Read the private RSA key to decrypt the cahcha key with open(args.rsa_key, 'rb') as f: r = RSA.importKey(f.read(), passphrase='') - # Read the encrypted xchacha key and the nonce - with open(args.ulog_key, 'rb') as f: + if args.ulog_key == "": + key_data_filename = args.ulog_file + magic = "ULogEnc" + else: + key_data_filename = args.ulog_key + magic = "ULogKey" + + with open(key_data_filename, 'rb') as f: + # Read the encrypted xchacha key and the nonce ulog_key_header = f.read(22) # Parse the header try: # magic - if not ulog_key_header.startswith(bytearray("ULogKey".encode())): + if not ulog_key_header.startswith(bytearray(magic.encode())): + print("Incorrect header magic") raise Exception() # version if ulog_key_header[7] != 1: + print("Unsupported header version") raise Exception() # expected key exchange algorithm (RSA_OAEP) if ulog_key_header[16] != 4: + print("Unsupported key algorithm") raise Exception() - key_size = ulog_key_header[19] << 8 | ulog_key_header[18]; - nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20]; + key_size = ulog_key_header[19] << 8 | ulog_key_header[18] + nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20] ulog_key_cipher = f.read(key_size) nonce = f.read(nonce_size) except: - print("Keyfile format error") - sys.exit(1); + print("Keydata format error") + sys.exit(1) + + if magic == "ULogEnc": + data_offset = 22 + key_size + nonce_size + else: + data_offset = 0 # Decrypt the xchacha key cipher_rsa = PKCS1_OAEP.new(r,SHA256) ulog_key = cipher_rsa.decrypt(ulog_key_cipher) #print(binascii.hexlify(ulog_key)) - # Read and decrypt the .ulgc + # Read and decrypt the ulog data cipher = ChaCha20.new(key=ulog_key, nonce=nonce) + + outfilename = Path(args.ulog_file).stem + ".ulog" with open(args.ulog_file, 'rb') as f: - with open(args.ulog_file.rstrip(args.ulog_file[-1]), 'wb') as out: + if data_offset > 0: + f.seek(data_offset) + with open(outfilename, 'wb') as out: out.write(cipher.decrypt(f.read())) From 197aed8bddca8df6a6d7036009c66216ccaa04df Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Fri, 6 Dec 2024 14:52:54 +0100 Subject: [PATCH 123/211] Add mavlink stream for plane camera (#24081) --- .../init.d-posix/airframes/1031_gazebo-classic_plane_cam.post | 1 + ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt | 1 + 2 files changed, 2 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post new file mode 100644 index 000000000000..647362a710b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post @@ -0,0 +1 @@ +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 1987464e319b..ef208ab31460 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -49,6 +49,7 @@ px4_add_romfs_files( 1022_gazebo-classic_uuv_bluerov2_heavy 1030_gazebo-classic_plane 1031_gazebo-classic_plane_cam + 1031_gazebo-classic_plane_cam.post 1032_gazebo-classic_plane_catapult 1033_jsbsim_rascal 1034_flightgear_rascal-electric From 430f0888b307434feb72dcbb6bab64d51c5f50bb Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Fri, 6 Dec 2024 11:29:57 +0000 Subject: [PATCH 124/211] update all px4board kconfig --- boards/mro/x21-777/default.px4board | 1 - boards/px4/fmu-v6u/default.px4board | 1 - 2 files changed, 2 deletions(-) diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index bf94e4b2c7ac..cec5dd074a85 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -67,7 +67,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index 6addac12f3f4..29d6ae8afe97 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -68,7 +68,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y From 09cd42911d993544f50b4d1747f6beedbb9eb61b Mon Sep 17 00:00:00 2001 From: Riccardo Mengoli Date: Tue, 10 Dec 2024 09:20:53 +0100 Subject: [PATCH 125/211] Mission: Replay gimbal cached items before reaching mission waypoint (#24085) When flying patterns, photos are sometimes taken while the gimbal is pitching up or down. To address this, we orient the gimbal before reaching the mission waypoint, allowing more time to complete the action. Additionally, we verify if the vehicle is climbing to avoid orienting the gimbal while on the ground. --- src/modules/navigator/mission_base.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 37f104dea2b7..0d653a1882e1 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -299,15 +299,14 @@ MissionBase::on_active() replayCachedCameraModeItems(); } + // Replay cached gimbal commands immediately upon mission resume, but only after the vehicle has reached the final target altitude + if (haveCachedGimbalItems() && _work_item_type != WorkItemType::WORK_ITEM_TYPE_CLIMB) { + replayCachedGimbalItems(); + } // Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption. // Each replay function also clears the cached items afterwards if (_mission.current_seq > _mission_activation_index) { - // replay gimbal commands - if (haveCachedGimbalItems()) { - replayCachedGimbalItems(); - } - // replay trigger commands if (cameraWasTriggering()) { replayCachedTriggerItems(); From f9ecc0fcd15ea8aeb23175a43bd9a593fce05c58 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Tue, 10 Dec 2024 10:15:49 +0100 Subject: [PATCH 126/211] Added various features to flash analysis (#24072) * Added various features to flash analysis (also run with FLASH overflow, summary in comment output, newer bloaty version, only add comment if change is large enough, ...) * Added feedback from review * Use wildcards * Removed backward-compat logic and use correct base --- .github/workflows/flash_analysis.yml | 55 +++++++++++++------ boards/px4/fmu-v5x/flash-analysis.px4board | 1 + .../scripts/flash-analysis-script.ld | 6 ++ boards/px4/fmu-v6x/flash-analysis.px4board | 1 + .../scripts/flash-analysis-script.ld | 6 ++ 5 files changed, 53 insertions(+), 16 deletions(-) create mode 100644 boards/px4/fmu-v5x/flash-analysis.px4board create mode 100644 boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld create mode 100644 boards/px4/fmu-v6x/flash-analysis.px4board create mode 100644 boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml index 2b5209511844..8ab97fdda898 100644 --- a/.github/workflows/flash_analysis.yml +++ b/.github/workflows/flash_analysis.yml @@ -8,6 +8,10 @@ on: branches: - '*' +env: + MIN_FLASH_POS_DIFF_FOR_COMMENT: 50 + MIN_FLASH_NEG_DIFF_FOR_COMMENT: -50 + jobs: analyze_flash: name: Analyzing ${{ matrix.target }} @@ -18,8 +22,10 @@ jobs: matrix: target: [px4_fmu-v5x, px4_fmu-v6x] outputs: - px4_fmu-v5x: ${{ steps.gen-output.outputs.output_px4_fmu-v5x }} - px4_fmu-v6x: ${{ steps.gen-output.outputs.output_px4_fmu-v6x }} + px4_fmu-v5x-bloaty-output: ${{ steps.gen-output.outputs.px4_fmu-v5x-bloaty-output }} + px4_fmu-v5x-bloaty-summary-map: ${{ steps.gen-output.outputs.px4_fmu-v5x-bloaty-summary-map }} + px4_fmu-v6x-bloaty-output: ${{ steps.gen-output.outputs.px4_fmu-v6x-bloaty-output }} + px4_fmu-v6x-bloaty-summary-map: ${{ steps.gen-output.outputs.px4_fmu-v6x-bloaty-summary-map }} steps: - uses: actions/checkout@v4 with: @@ -30,19 +36,20 @@ jobs: run: git config --system --add safe.directory '*' - name: Build Target - run: make ${{ matrix.target }} + run: make ${{ matrix.target }}_flash-analysis - name: Store the ELF with the change - run: cp ./build/${{ matrix.target }}_default/${{ matrix.target }}_default.elf ./with-change.elf + run: cp ./build/**/*.elf ./with-change.elf - name: Clean previous build run: | make clean make distclean - - name: If it's a PR checkout the base commit + - name: If it's a PR checkout the base branch if: ${{ github.event.pull_request }} - run: git checkout ${{ github.event.pull_request.base.sha }} + # As checkout creates a merge commit (merging the base branch into the PR branch), the base branch is the base for a diff of the PR changes. + run: git checkout ${{ github.event.pull_request.base.ref }} - name: If it's a push checkout the previous commit if: github.event_name == 'push' @@ -52,30 +59,39 @@ jobs: run: make submodulesupdate - name: Build - run: make ${{ matrix.target }} + run: make ${{ matrix.target }}_flash-analysis - name: Store the ELF before the change - run: cp ./build/${{ matrix.target }}_default/${{ matrix.target }}_default.elf ./before-change.elf + run: cp ./build/**/*.elf ./before-change.elf - name: bloaty-action - uses: carlosperate/bloaty-action@v1.1.0 + uses: PX4/bloaty-action@v1.0.0 id: bloaty-step with: - bloaty-args: -d sections,compileunits -n 0 ./with-change.elf -- ./before-change.elf + bloaty-file-args: ./with-change.elf -- ./before-change.elf + bloaty-additional-args: -d sections,compileunits -s vm -n 20 output-to-summary: true - name: Generate output id: gen-output run: | EOF=$(dd if=/dev/urandom bs=15 count=1 status=none | base64) - echo "output_${{ matrix.target }}<<$EOF" >> $GITHUB_OUTPUT + echo "${{ matrix.target }}-bloaty-output<<$EOF" >> $GITHUB_OUTPUT echo "${{ steps.bloaty-step.outputs.bloaty-output-encoded }}" >> $GITHUB_OUTPUT echo "$EOF" >> $GITHUB_OUTPUT + echo "${{ matrix.target }}-bloaty-summary-map<<$EOF" >> $GITHUB_OUTPUT + echo '${{ steps.bloaty-step.outputs.bloaty-summary-map }}' >> $GITHUB_OUTPUT + echo "$EOF" >> $GITHUB_OUTPUT post_pr_comment: name: Publish Results runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: [analyze_flash] + env: + V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }} + V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }} + V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }} + V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }} if: ${{ github.event.pull_request }} steps: - name: Find Comment @@ -92,25 +108,32 @@ jobs: echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT - name: Create or update comment + # This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env. + if: | + steps.fc.outputs.comment-id != '' || + env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) || + env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) uses: peter-evans/create-or-update-comment@v4 with: comment-id: ${{ steps.fc.outputs.comment-id }} issue-number: ${{ github.event.pull_request.number }} body: | - ## FLASH Analysis + ## 🔎 FLASH Analysis
- px4_fmu-v5x + px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)] ``` - ${{ needs.analyze_flash.outputs.px4_fmu-v5x }} + ${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }} ```
- px4_fmu-v6x + px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)] ``` - ${{ needs.analyze_flash.outputs.px4_fmu-v6x }} + ${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }} ```
diff --git a/boards/px4/fmu-v5x/flash-analysis.px4board b/boards/px4/fmu-v5x/flash-analysis.px4board new file mode 100644 index 000000000000..30717ad93ca7 --- /dev/null +++ b/boards/px4/fmu-v5x/flash-analysis.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LINKER_PREFIX="flash-analysis" diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld new file mode 100644 index 000000000000..29c75f02eee8 --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld @@ -0,0 +1,6 @@ +INCLUDE "script.ld" + +MEMORY +{ + FLASH_AXIM (rx) : ORIGIN = 0x08008000, LENGTH = 10080K +} diff --git a/boards/px4/fmu-v6x/flash-analysis.px4board b/boards/px4/fmu-v6x/flash-analysis.px4board new file mode 100644 index 000000000000..30717ad93ca7 --- /dev/null +++ b/boards/px4/fmu-v6x/flash-analysis.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LINKER_PREFIX="flash-analysis" diff --git a/boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld b/boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld new file mode 100644 index 000000000000..5df2f5180a59 --- /dev/null +++ b/boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld @@ -0,0 +1,6 @@ +INCLUDE "script.ld" + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 10080K +} From bdbf7fd1dda1b5fc493effe0e850e1ba723cbfa5 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Tue, 10 Dec 2024 11:05:55 +0100 Subject: [PATCH 127/211] Also consider targets that include a '-' (#24087) --- Tools/ci/generate_board_targets_json.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ci/generate_board_targets_json.py b/Tools/ci/generate_board_targets_json.py index c5c86f782fb4..218862cecdb0 100755 --- a/Tools/ci/generate_board_targets_json.py +++ b/Tools/ci/generate_board_targets_json.py @@ -73,7 +73,7 @@ def process_target(px4board_file, target_name): px4board_file.endswith("bootloader.px4board"): kconf.load_config(px4board_file, replace=True) else: # Merge config with default.px4board - default_kconfig = re.sub(r'[a-zA-Z\d_]+\.px4board', 'default.px4board', px4board_file) + default_kconfig = re.sub(r'[a-zA-Z\d_-]+\.px4board', 'default.px4board', px4board_file) kconf.load_config(default_kconfig, replace=True) kconf.load_config(px4board_file, replace=False) From 0f1a4eb72c2a1afbb8e907be3e59ec8396864e27 Mon Sep 17 00:00:00 2001 From: Vincent Poon Date: Wed, 11 Dec 2024 04:49:06 +0800 Subject: [PATCH 128/211] update H-Flow Boot config and add stm32_configgpio (#24086) --- boards/holybro/h-flow/src/board_config.h | 7 +++++-- boards/holybro/h-flow/src/init.c | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/boards/holybro/h-flow/src/board_config.h b/boards/holybro/h-flow/src/board_config.h index d2932ccc817b..2a8a0691e422 100644 --- a/boards/holybro/h-flow/src/board_config.h +++ b/boards/holybro/h-flow/src/board_config.h @@ -46,9 +46,8 @@ /* CAN Silent mode control */ #define GPIO_CAN1_SILENT_S0 /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) -// TODO: figure out /* Boot config */ -//#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI) +#define GPIO_BOOT_CONFIG /* PC14 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN14|GPIO_EXTI) /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ #define GPIO_nLED_RED /* PB0 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) @@ -68,6 +67,10 @@ // TODO figure out #define GPIO_GETNODEINFO_JUMPER 0 //(GPIO_BOOT_CONFIG & ~GPIO_EXTI) +// CAN termination set by param, available from RC02 +#define GPIO_CAN1_TERMINATION /* PA12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + #define FLASH_BASED_PARAMS /* High-resolution timer */ diff --git a/boards/holybro/h-flow/src/init.c b/boards/holybro/h-flow/src/init.c index d358e3aa6c04..644dfb63d7c5 100644 --- a/boards/holybro/h-flow/src/init.c +++ b/boards/holybro/h-flow/src/init.c @@ -96,6 +96,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); // Configure SPI all interfaces GPIO & enable power. stm32_spiinitialize(); From 091974e6c54795d343e4aef12d38c06f568aaf25 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Dec 2024 10:00:36 +1300 Subject: [PATCH 129/211] listener: only clear screen with multiple messages (#24019) * listener: only clear screen with multiple messages * listener: fully clear before printing again Otherwise, we end up seeing artifacts from the previous print. --- src/systemcmds/topic_listener/listener_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index 83ca22ddd710..28244869f90f 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -113,9 +113,6 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, fds[1].fd = sub; fds[1].events = POLLIN; - // Clear screen - dprintf(1, "\033[2J\n"); - while (msgs_received < num_msgs) { if (poll(&fds[0], 2, int(MESSAGE_TIMEOUT_S * 1000)) > 0) { @@ -142,8 +139,13 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, if (fds[1].revents & POLLIN) { msgs_received++; - // Move cursor to home position - dprintf(1, "\033[H"); + if (num_msgs > 1) { + // Clear screen + dprintf(1, "\033[2J\n"); + // Move cursor to home position + dprintf(1, "\033[H"); + } + PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received); int ret = listener_print_topic(id, sub); From cbde9729e85bf6ddec15bb318eced79e21264ca7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 19:44:31 +0100 Subject: [PATCH 130/211] Airframes: remove unnecessary double newlines --- ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil | 1 - ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox | 1 - ROMFS/px4fmu_common/init.d/airframes/16001_helicopter | 1 - ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 | 2 -- ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 | 1 - ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox | 1 - ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 | 1 - ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu | 5 ----- ROMFS/px4fmu_common/init.d/airframes/4071_ifo | 1 - ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 | 2 -- .../init.d/airframes/50001_aion_robotics_r1_rover | 1 - .../init.d/airframes/59000_generic_ground_vehicle | 1 - .../px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy | 1 - 13 files changed, 19 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index e124e9981ac1..2bcfc14a2e03 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.fw_defaults - param set UAVCAN_ENABLE 0 param set-default CA_AIRFRAME 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox b/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox index 1dba92b89d9d..d72eaa213211 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox @@ -21,7 +21,6 @@ . ${R}etc/init.d/rc.mc_defaults - param set-default MAV_TYPE 14 param set-default CA_ROTOR_COUNT 8 diff --git a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter index de21a8821668..695604e0b189 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter +++ b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter @@ -11,7 +11,6 @@ . ${R}etc/init.d/rc.heli_defaults - # Disable PID gains for initial setup. These should be enabled after setting the FF gain. # P is expected to be lower than FF. param set-default MC_ROLLRATE_P 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 index 0ed3b201c8a5..7f85a770e19e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 +++ b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 @@ -22,7 +22,6 @@ . ${R}etc/init.d/rc.fw_defaults - param set-default BAT1_CAPACITY 2500 param set-default BAT1_N_CELLS 3 @@ -41,7 +40,6 @@ param set-default FW_P_LIM_MAX 25 param set-default FW_P_LIM_MIN -5 param set-default FW_P_RMAX_NEG 20 - param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 index 0fced30c8efa..1907b33a3248 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 +++ b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 @@ -20,7 +20,6 @@ . ${R}etc/init.d/rc.fw_defaults - param set-default BAT1_CAPACITY 3300 param set-default BAT1_N_CELLS 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox index f894d47d74fd..6f75f8619867 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox @@ -19,7 +19,6 @@ param set-default NAV_ACC_RAD 2 param set-default RTL_DESCEND_ALT 10 param set-default RTL_RETURN_ALT 30 - param set-default CA_ROTOR_COUNT 12 # Bottom motors param set-default CA_ROTOR0_PX 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 index d460138cc815..129eacbffdeb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 @@ -21,7 +21,6 @@ param set-default MC_PITCHRATE_P 0.08 param set-default MC_PITCHRATE_D 0.001 param set-default MC_YAW_P 4 - param set-default MC_ROLLRATE_MAX 1600 param set-default MC_PITCHRATE_MAX 1600 param set-default MC_YAWRATE_MAX 1000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index e5e5e083437f..fe40b95aa034 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -38,7 +38,6 @@ param set-default COM_FLTMODE5 -1 param set-default COM_FLTMODE6 1 param set-default COM_RC_LOSS_T 3 - # ekf2 param set-default EKF2_BARO_NOISE 2 @@ -79,19 +78,16 @@ param set-default EKF2_RNG_POS_Z 0.033 param set-default EKF2_TERR_NOISE 1 - # Maximum allowed angle velocity in the landed state param set-default LNDMC_ROT_MAX 40 # Maximum vertical velocity allowed in the landed state param set-default LNDMC_Z_VEL_MAX 0.7 - # filtering param set-default IMU_DGYRO_CUTOFF 50 param set-default IMU_GYRO_CUTOFF 65 - # Pitch angle & rate setting param set-default MC_PITCHRATE_P 0.075 param set-default MC_PITCHRATE_I 0.1 @@ -148,7 +144,6 @@ param set-default RC_MAP_RETURN_SW 7 param set-default RC1_TRIM 1000 - # optical flow param set-default SENS_FLOW_MAXR 7.4 param set-default SENS_FLOW_MINHGT 0.15 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index 8cfc14ae1ef7..141ca861c422 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -30,7 +30,6 @@ param set-default IMU_DGYRO_CUTOFF 90 param set-default IMU_GYRO_CUTOFF 100 # System - param set-default SENS_BOARD_ROT 10 # EKF2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index 4833d17af23c..85b045e84a2a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -18,7 +18,6 @@ # . ${R}etc/init.d/rc.mc_defaults - param set-default SYS_HAS_MAG 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_GPS_CTRL 0 @@ -85,6 +84,5 @@ param set-default PWM_MAIN_MAX3 255 param set-default SENS_FLOW_MINRNG 0.05 - syslink start mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 7cdf2d10a673..57a3644dae00 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -40,7 +40,6 @@ param set-default RD_YAW_I 0.1 param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0.01 - # Pure pursuit parameters param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle index a773c1158d44..078ea7859d8b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle @@ -15,7 +15,6 @@ . ${R}etc/init.d/rc.rover_defaults - param set-default BAT1_N_CELLS 2 param set-default EKF2_ANGERR_INIT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy index 06a2d3fb2f91..1449a6d3adf3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy @@ -22,7 +22,6 @@ . ${R}etc/init.d/rc.uuv_defaults - param set-default MAV_1_CONFIG 102 param set-default BAT1_A_PER_V 37.8798 From 5010b01e2ec326b4efb5128b6087aa13d7cf9fd9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 19:45:12 +0100 Subject: [PATCH 131/211] PCF8583: refactor 1e6f for 1000000.f --- src/drivers/rpm/pcf8583/PCF8583.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index fcc9828dad86..085015247f4b 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/src/drivers/rpm/pcf8583/PCF8583.cpp @@ -195,8 +195,8 @@ void PCF8583::RunImpl() } // Calculate RPM and accuracy estimation - float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f; - float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f; + float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f; + float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1e6f) * 60.f; // publish data to uorb rpm_s msg{}; From d5c9ea048e33b241aba4d86e496e585779ff9478 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 19:45:42 +0100 Subject: [PATCH 132/211] LidarLitePWM: refactor static cast and comment --- src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index 1ef65a5ca396..8e94a4171034 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp @@ -109,7 +109,7 @@ LidarLitePWM::measure() return PX4_ERROR; } - const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ + const float current_distance = static_cast(_pwm.pulse_width) * 1e-3f; // 1us = 1mm distance for LIDAR-Lite /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ if (current_distance <= 0.0f) { From fc6c678274ccebe11b7d503d5e15b86257c10f1d Mon Sep 17 00:00:00 2001 From: Pernilla Date: Tue, 3 Dec 2024 17:26:45 +0100 Subject: [PATCH 133/211] Remove gz-garden as it reached EOL --- Tools/setup/ubuntu.sh | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index e2f215dc2d66..434b2a81cd09 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -179,17 +179,6 @@ if [[ $INSTALL_SIM == "true" ]]; then gazebo_classic_version=11 gazebo_packages="gazebo$gazebo_classic_version libgazebo$gazebo_classic_version-dev" fi - elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then - echo "Gazebo (Garden) will be installed" - echo "Earlier versions will be removed" - # Add Gazebo binary repository - sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null - - sudo apt-get update -y --quiet - - # Install Gazebo - gazebo_packages="gz-garden" else # Expects Ubuntu 22.04 > by default echo "Gazebo (Harmonic) will be installed" From cc92979b06e15bf97494fb574dbd7d3b878bf634 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 12 Dec 2024 14:04:53 +0100 Subject: [PATCH 134/211] ekf-agp: fix timeout --- .../EKF/aid_sources/aux_global_position/aux_global_position.cpp | 2 +- .../EKF/aid_sources/aux_global_position/aux_global_position.hpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 1d425cb85384..6aec6936b210 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -73,7 +73,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed return; } - estimator_aid_source2d_s aid_src{}; + estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position; const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl); const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index e5bc78026a68..423462d6eef1 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -92,6 +92,7 @@ class AuxGlobalPosition : public ModuleParams uint8_t lat_lon_reset_counter{}; }; + estimator_aid_source2d_s _aid_src_aux_global_position{}; RingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate uint64_t _time_last_buffer_push{0}; From 0b4b794de7e26cdee9e34a1bb5fa80a8fd9da254 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 20 Nov 2024 14:06:48 +0100 Subject: [PATCH 135/211] COMMON_DISTANCE_SENSOR: remove leddar_one from the list Signed-off-by: Silvan Fuhrer --- src/drivers/distance_sensor/Kconfig | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index f45abe877c50..3ec551bf5cd3 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -4,7 +4,6 @@ menu "Distance sensors" default n select DRIVERS_DISTANCE_SENSOR_CM8JL65 select DRIVERS_DISTANCE_SENSOR_GY_US42 - select DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL select DRIVERS_DISTANCE_SENSOR_LL40LS From f4b5d8e3d812ee6de7d662ef41274cdd0738b7d7 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 20 Nov 2024 14:07:00 +0100 Subject: [PATCH 136/211] COMMON_DISTANCE_SENSOR: remove gy_us42 from the list Signed-off-by: Silvan Fuhrer --- src/drivers/distance_sensor/Kconfig | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index 3ec551bf5cd3..e51d04fb2b20 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -3,7 +3,6 @@ menu "Distance sensors" bool "Common distance sensor's" default n select DRIVERS_DISTANCE_SENSOR_CM8JL65 - select DRIVERS_DISTANCE_SENSOR_GY_US42 select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL select DRIVERS_DISTANCE_SENSOR_LL40LS From 896c08bd68f959da980373f641f6ea861f463173 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 20 Nov 2024 14:10:51 +0100 Subject: [PATCH 137/211] COMMON_DISTANCE_SENSOR: remove srf02 from the list Signed-off-by: Silvan Fuhrer --- src/drivers/distance_sensor/Kconfig | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index e51d04fb2b20..25adedcd894c 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -6,7 +6,6 @@ menu "Distance sensors" select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL select DRIVERS_DISTANCE_SENSOR_LL40LS - select DRIVERS_DISTANCE_SENSOR_SRF02 select DRIVERS_DISTANCE_SENSOR_TERARANGER select DRIVERS_DISTANCE_SENSOR_TF02PRO select DRIVERS_DISTANCE_SENSOR_TFMINI From eb829676b0e8ad527ca11917c9dc38ced4bb1ef9 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 12 Dec 2024 17:13:04 -0900 Subject: [PATCH 138/211] sim: gz: remove Garden from cmake --- src/modules/simulation/gz_bridge/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index a094298fab90..e3e6b3cc9088 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -32,8 +32,8 @@ ############################################################################ # Find the gz_Transport library -# Look for GZ Ionic, Harmonic, and Garden library options in that order -find_package(gz-transport NAMES gz-transport14 gz-transport13 gz-transport12) +# Look for GZ Ionic or Harmonic +find_package(gz-transport NAMES gz-transport14 gz-transport13) if(gz-transport_FOUND) From fad9ae855d1450ccb7876c58c88a839c54994756 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 28 Nov 2024 14:20:31 +0100 Subject: [PATCH 139/211] EKF2: reset aid src when resetting state to measurement Filtered innovations and test ratios can be large before the reset and would trigger pre-flight warnings --- .../aux_global_position/aux_global_position.cpp | 1 + .../aid_sources/barometer/baro_height_control.cpp | 2 ++ .../EKF/aid_sources/gnss/gnss_height_control.cpp | 2 ++ .../range_finder/range_height_control.cpp | 6 ++++++ src/modules/ekf2/EKF/ekf.h | 15 +-------------- src/modules/ekf2/EKF/ekf_helper.cpp | 15 +++++++++++++++ src/modules/ekf2/test/test_EKF_height_fusion.cpp | 6 ++---- 7 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 6aec6936b210..3307dd56b924 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -112,6 +112,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed // Try to initialize using measurement if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv))) { + ekf.resetAidSourceStatusZeroInnovation(aid_src); ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index c645afd9d5c7..119c6fa1b38b 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -133,6 +133,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_baro = true; resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); + resetAidSourceStatusZeroInnovation(aid_src); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -165,6 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_baro = true; initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); + resetAidSourceStatusZeroInnovation(aid_src); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c936442b1405..c81af9c581fa 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -107,6 +107,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) _information_events.flags.reset_hgt_to_gps = true; resetAltitudeTo(measurement, measurement_var); bias_est.setBias(-_gpos.altitude() + measurement); + resetAidSourceStatusZeroInnovation(aid_src); aid_src.time_last_fuse = _time_delayed_us; @@ -131,6 +132,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); + resetAidSourceStatusZeroInnovation(aid_src); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index fec1fd9d8e85..6ab803656076 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -139,6 +139,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } else if (do_range_aid) { @@ -150,6 +151,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_rng = true; resetAltitudeTo(aid_src.observation, aid_src.observation_variance); _state.terrain = 0.f; + resetAidSourceStatusZeroInnovation(aid_src); _control_status.flags.rng_hgt = true; stopRngTerrFusion(); @@ -163,6 +165,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } } @@ -181,6 +184,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_rng = true; resetAltitudeTo(aid_src.observation - _state.terrain); + resetAidSourceStatusZeroInnovation(aid_src); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -198,6 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } @@ -218,6 +223,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { if (aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } _control_status.flags.rng_terrain = true; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f578a929aae8..b831d096d153 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1016,20 +1016,7 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_EXTERNAL_VISION // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) - void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const - { - status.time_last_fuse = _time_delayed_us; - - status.innovation = 0.f; - status.innovation_filtered = 0.f; - status.innovation_variance = status.observation_variance; - - status.test_ratio = 0.f; - status.test_ratio_filtered = 0.f; - - status.innovation_rejected = false; - status.fused = true; - } + void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const; // helper used for populating and filtering estimator aid source struct for logging void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9638abeb694c..a6872b40d257 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1131,6 +1131,21 @@ bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R, return true; } +void Ekf::resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const +{ + status.time_last_fuse = _time_delayed_us; + + status.innovation = 0.f; + status.innovation_filtered = 0.f; + status.innovation_variance = status.observation_variance; + + status.test_ratio = 0.f; + status.test_ratio_filtered = 0.f; + + status.innovation_rejected = false; + status.fused = true; +} + void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, const float &observation, const float &observation_variance, const float &innovation, const float &innovation_variance, diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 17c95cad453f..595d518abb7c 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -345,12 +345,10 @@ TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) // Also check the reset counters to make sure the reset logic triggered reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); + // The velocity does not reset as baro only provides height measurement EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); - - // The height resets twice in a row as the baro innovation is not corrected after a height - // reset and triggers a new reset at the next iteration - EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); } TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) From f9140fcd503b9d01c9588ec8dda6d50359a08177 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 11 Dec 2024 14:51:54 +0100 Subject: [PATCH 140/211] ekf2: set baro bias when GNSS is alt ref Do this even when GNSS altitude fusion is disabled. --- .../aid_sources/gnss/gnss_height_control.cpp | 22 +++++++++++++--- src/modules/ekf2/module.yaml | 2 ++ .../ekf2/test/test_EKF_height_fusion.cpp | 26 +++++++++++++++++++ 3 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c81af9c581fa..9426fa266433 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -85,14 +85,21 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) } // determine if we should use height aiding + const bool common_conditions_passing = measurement_valid + && _local_origin_lat_lon.isInitialized() + && _gps_checks_passed; + const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VPOS)) - && measurement_valid - && _local_origin_lat_lon.isInitialized() - && _gps_checks_passed; + && common_conditions_passing; const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); + const bool altitude_initialisation_conditions_passing = common_conditions_passing + && !PX4_ISFINITE(_local_origin_alt) + && _params.height_sensor_ref == static_cast(HeightSensor::GNSS) + && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); + if (_control_status.flags.gps_hgt) { if (continuing_conditions_passing) { @@ -142,6 +149,15 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) aid_src.time_last_fuse = _time_delayed_us; bias_est.setFusionActive(); _control_status.flags.gps_hgt = true; + + } if (altitude_initialisation_conditions_passing) { + + // Do not start GNSS altitude aiding, but use measurement + // to initialize altitude and bias of other height sensors + _information_events.flags.reset_hgt_to_gps = true; + + initialiseAltitudeTo(measurement, measurement_var); + bias_est.reset(); } } diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index d8295fd60c25..cd815ed56f10 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -92,6 +92,8 @@ parameters: by this parameter. The range sensor and vision options should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level. + If GPS is set as reference but altitude fusion is disabled in EKF2_GPS_CTRL, + the GPS altitude is still used to initiaize the bias of the other height sensors. type: enum values: 0: Barometric pressure diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 595d518abb7c..3b9cd2363b7b 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -208,6 +208,32 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f); } +TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion) +{ + // GIVEN: GNSS alt reference but not selected as an aiding source + _ekf_wrapper.setGpsHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.disableGpsHeightFusion(); + _ekf_wrapper.enableRangeHeightFusion(); + _sensor_simulator.runSeconds(1); + + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion()); + + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + + // THEN: the altitude estimate is initialised using GNSS altitude + EXPECT_NEAR(_ekf->getLatLonAlt().altitude(), _sensor_simulator._gps.getData().alt, 1.f); + // We cannot check the value of the bias estimate as the status is only updatad when the bias estimator is + // active. Since the estimator had a baro fallback, the baro bias estimate is not actively updated. + // EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() - _sensor_simulator._gps.getData().alt, 0.2f); +} + TEST_F(EkfHeightFusionTest, baroRefFailOver) { // GIVEN: baro reference with GPS and range height fusion From 4fe7d713d3579b50668616ce176bd78e454473d4 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Thu, 24 Oct 2024 11:21:15 +0200 Subject: [PATCH 141/211] msg: introduce message versioning --- msg/CMakeLists.txt | 73 ++++++++++--------- msg/{ => versioned}/ActuatorMotors.msg | 3 + msg/{ => versioned}/ActuatorServos.msg | 3 + msg/{ => versioned}/ArmingCheckReply.msg | 2 + msg/{ => versioned}/ArmingCheckRequest.msg | 2 + msg/{ => versioned}/BatteryStatus.msg | 2 + msg/{ => versioned}/ConfigOverrides.msg | 3 + msg/{ => versioned}/GotoSetpoint.msg | 2 + msg/{ => versioned}/HomePosition.msg | 2 + msg/{ => versioned}/ManualControlSetpoint.msg | 2 + msg/{ => versioned}/ModeCompleted.msg | 3 + .../RegisterExtComponentReply.msg | 2 + .../RegisterExtComponentRequest.msg | 3 + msg/{ => versioned}/TrajectorySetpoint.msg | 2 + .../UnregisterExtComponent.msg | 2 + .../VehicleAngularVelocity.msg | 1 + msg/{ => versioned}/VehicleAttitude.msg | 2 + .../VehicleAttitudeSetpoint.msg | 2 + msg/{ => versioned}/VehicleCommand.msg | 2 + msg/{ => versioned}/VehicleCommandAck.msg | 2 + msg/{ => versioned}/VehicleControlMode.msg | 2 + msg/{ => versioned}/VehicleGlobalPosition.msg | 2 + msg/{ => versioned}/VehicleLandDetected.msg | 2 + msg/{ => versioned}/VehicleLocalPosition.msg | 2 + msg/{ => versioned}/VehicleOdometry.msg | 3 + msg/{ => versioned}/VehicleRatesSetpoint.msg | 2 + msg/{ => versioned}/VehicleStatus.msg | 2 + msg/{ => versioned}/VtolVehicleStatus.msg | 3 + 28 files changed, 100 insertions(+), 33 deletions(-) rename msg/{ => versioned}/ActuatorMotors.msg (95%) rename msg/{ => versioned}/ActuatorServos.msg (92%) rename msg/{ => versioned}/ArmingCheckReply.msg (96%) rename msg/{ => versioned}/ArmingCheckRequest.msg (84%) rename msg/{ => versioned}/BatteryStatus.msg (99%) rename msg/{ => versioned}/ConfigOverrides.msg (95%) rename msg/{ => versioned}/GotoSetpoint.msg (97%) rename msg/{ => versioned}/HomePosition.msg (96%) rename msg/{ => versioned}/ManualControlSetpoint.msg (98%) rename msg/{ => versioned}/ModeCompleted.msg (94%) rename msg/{ => versioned}/RegisterExtComponentReply.msg (93%) rename msg/{ => versioned}/RegisterExtComponentRequest.msg (97%) rename msg/{ => versioned}/TrajectorySetpoint.msg (95%) rename msg/{ => versioned}/UnregisterExtComponent.msg (92%) rename msg/{ => versioned}/VehicleAngularVelocity.msg (94%) rename msg/{ => versioned}/VehicleAttitude.msg (96%) rename msg/{ => versioned}/VehicleAttitudeSetpoint.msg (96%) rename msg/{ => versioned}/VehicleCommand.msg (99%) rename msg/{ => versioned}/VehicleCommandAck.msg (98%) rename msg/{ => versioned}/VehicleControlMode.msg (97%) rename msg/{ => versioned}/VehicleGlobalPosition.msg (98%) rename msg/{ => versioned}/VehicleLandDetected.msg (96%) rename msg/{ => versioned}/VehicleLocalPosition.msg (99%) rename msg/{ => versioned}/VehicleOdometry.msg (97%) rename msg/{ => versioned}/VehicleRatesSetpoint.msg (95%) rename msg/{ => versioned}/VehicleStatus.msg (99%) rename msg/{ => versioned}/VtolVehicleStatus.msg (94%) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c87e75fb695e..8d4e27931b88 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -40,19 +40,14 @@ set(msg_files ActionRequest.msg ActuatorArmed.msg ActuatorControlsStatus.msg - ActuatorMotors.msg ActuatorOutputs.msg - ActuatorServos.msg ActuatorServosTrim.msg ActuatorTest.msg AdcReport.msg Airspeed.msg AirspeedValidated.msg AirspeedWind.msg - ArmingCheckReply.msg - ArmingCheckRequest.msg AutotuneAttitudeControlStatus.msg - BatteryStatus.msg Buffer128.msg ButtonEvent.msg CameraCapture.msg @@ -61,7 +56,7 @@ set(msg_files CanInterfaceStatus.msg CellularStatus.msg CollisionConstraints.msg - ConfigOverrides.msg + CollisionReport.msg ControlAllocatorStatus.msg Cpuload.msg DatamanRequest.msg @@ -109,7 +104,6 @@ set(msg_files GimbalManagerSetAttitude.msg GimbalManagerSetManualControl.msg GimbalManagerStatus.msg - GotoSetpoint.msg GpioConfig.msg GpioIn.msg GpioOut.msg @@ -119,7 +113,6 @@ set(msg_files Gripper.msg HealthReport.msg HeaterStatus.msg - HomePosition.msg HoverThrustEstimate.msg InputRc.msg InternalCombustionEngineStatus.msg @@ -135,7 +128,6 @@ set(msg_files LogMessage.msg MagnetometerBiasEstimate.msg MagWorkerData.msg - ManualControlSetpoint.msg ManualControlSwitches.msg MavlinkLog.msg MavlinkTunnel.msg @@ -144,7 +136,6 @@ set(msg_files Mission.msg MissionResult.msg MountOrientation.msg - ModeCompleted.msg NavigatorMissionItem.msg NavigatorStatus.msg NormalizedUnsignedSetpoint.msg @@ -181,8 +172,6 @@ set(msg_files RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg - RegisterExtComponentReply.msg - RegisterExtComponentRequest.msg RoverAckermannGuidanceStatus.msg RoverAckermannSetpoint.msg RoverAckermannStatus.msg @@ -223,7 +212,6 @@ set(msg_files TiltrotorExtraControls.msg TimesyncStatus.msg TrajectoryBezier.msg - TrajectorySetpoint.msg TrajectoryWaypoint.msg TransponderReport.msg TuneControl.msg @@ -231,39 +219,52 @@ set(msg_files UavcanParameterValue.msg UlogStream.msg UlogStreamAck.msg - UnregisterExtComponent.msg VehicleAcceleration.msg VehicleAirData.msg VehicleAngularAccelerationSetpoint.msg - VehicleAngularVelocity.msg - VehicleAttitude.msg - VehicleAttitudeSetpoint.msg - VehicleCommand.msg - VehicleCommandAck.msg VehicleConstraints.msg - VehicleControlMode.msg - VehicleGlobalPosition.msg VehicleImu.msg VehicleImuStatus.msg - VehicleLandDetected.msg - VehicleLocalPosition.msg VehicleLocalPositionSetpoint.msg VehicleMagnetometer.msg - VehicleOdometry.msg VehicleOpticalFlow.msg VehicleOpticalFlowVel.msg - VehicleRatesSetpoint.msg VehicleRoi.msg - VehicleStatus.msg VehicleThrustSetpoint.msg VehicleTorqueSetpoint.msg VehicleTrajectoryBezier.msg VehicleTrajectoryWaypoint.msg VelocityLimits.msg - VtolVehicleStatus.msg WheelEncoders.msg Wind.msg YawEstimatorStatus.msg + versioned/ActuatorMotors.msg + versioned/ActuatorServos.msg + versioned/ArmingCheckReply.msg + versioned/ArmingCheckRequest.msg + versioned/BatteryStatus.msg + versioned/ConfigOverrides.msg + versioned/GotoSetpoint.msg + versioned/HomePosition.msg + versioned/ManualControlSetpoint.msg + versioned/ModeCompleted.msg + versioned/RegisterExtComponentReply.msg + versioned/RegisterExtComponentRequest.msg + versioned/TrajectorySetpoint.msg + versioned/UnregisterExtComponent.msg + versioned/VehicleAngularVelocity.msg + versioned/VehicleAttitude.msg + versioned/VehicleAttitudeSetpoint.msg + versioned/VehicleCommandAck.msg + versioned/VehicleCommand.msg + versioned/VehicleControlMode.msg + versioned/VehicleGlobalPosition.msg + versioned/VehicleLandDetected.msg + versioned/VehicleLocalPosition.msg + versioned/VehicleOdometry.msg + versioned/VehicleRatesSetpoint.msg + versioned/VehicleStatus.msg + versioned/VtolVehicleStatus.msg ) list(SORT msg_files) @@ -315,7 +316,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${msg_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS @@ -336,7 +337,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --json -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${msg_source_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS @@ -374,7 +375,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${ucdr_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/ucdr DEPENDS @@ -396,7 +397,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --sources -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${msg_source_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS @@ -447,7 +448,13 @@ if(CONFIG_LIB_CDRSTREAM) # 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) + get_filename_component(msg_directory ${msg_file} DIRECTORY) + get_filename_component(msg_directory ${msg_directory} NAME) + if(msg_directory STREQUAL "versioned") + configure_file(${PX4_SOURCE_DIR}/msg/${msg_directory}/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) + else() + configure_file(${PX4_SOURCE_DIR}/msg/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) + endif() 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) @@ -492,7 +499,7 @@ if(CONFIG_LIB_CDRSTREAM) 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} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${idl_uorb_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream DEPENDS diff --git a/msg/ActuatorMotors.msg b/msg/versioned/ActuatorMotors.msg similarity index 95% rename from msg/ActuatorMotors.msg rename to msg/versioned/ActuatorMotors.msg index e74566f1f75e..da63a388bb04 100644 --- a/msg/ActuatorMotors.msg +++ b/msg/versioned/ActuatorMotors.msg @@ -1,4 +1,7 @@ # Motor control message + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp the data this control response is based on was sampled diff --git a/msg/ActuatorServos.msg b/msg/versioned/ActuatorServos.msg similarity index 92% rename from msg/ActuatorServos.msg rename to msg/versioned/ActuatorServos.msg index 2c7900e8116a..e740f39c8c7d 100644 --- a/msg/ActuatorServos.msg +++ b/msg/versioned/ActuatorServos.msg @@ -1,4 +1,7 @@ # Servo control message + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp the data this control response is based on was sampled diff --git a/msg/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg similarity index 96% rename from msg/ArmingCheckReply.msg rename to msg/versioned/ArmingCheckReply.msg index 589ad1b1cd7d..1a1aaa518379 100644 --- a/msg/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint8 request_id diff --git a/msg/ArmingCheckRequest.msg b/msg/versioned/ArmingCheckRequest.msg similarity index 84% rename from msg/ArmingCheckRequest.msg rename to msg/versioned/ArmingCheckRequest.msg index 69e7e85f351b..17c6e4bacd98 100644 --- a/msg/ArmingCheckRequest.msg +++ b/msg/versioned/ArmingCheckRequest.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # broadcast message to request all registered arming checks to be reported diff --git a/msg/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg similarity index 99% rename from msg/BatteryStatus.msg rename to msg/versioned/BatteryStatus.msg index e8651616152f..9838786f3fce 100644 --- a/msg/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool connected # Whether or not a battery is connected, based on a voltage threshold float32 voltage_v # Battery voltage in volts, 0 if unknown diff --git a/msg/ConfigOverrides.msg b/msg/versioned/ConfigOverrides.msg similarity index 95% rename from msg/ConfigOverrides.msg rename to msg/versioned/ConfigOverrides.msg index 09b87253a8f3..b274adfbecd7 100644 --- a/msg/ConfigOverrides.msg +++ b/msg/versioned/ConfigOverrides.msg @@ -1,4 +1,7 @@ # Configurable overrides by (external) modes or mode executors + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured) diff --git a/msg/GotoSetpoint.msg b/msg/versioned/GotoSetpoint.msg similarity index 97% rename from msg/GotoSetpoint.msg rename to msg/versioned/GotoSetpoint.msg index 5fe3ab8a7916..4871a3cb7d56 100644 --- a/msg/GotoSetpoint.msg +++ b/msg/versioned/GotoSetpoint.msg @@ -5,6 +5,8 @@ # Unset optional setpoints are not controlled # Unset optional constraints default to vehicle specifications +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # setpoints diff --git a/msg/HomePosition.msg b/msg/versioned/HomePosition.msg similarity index 96% rename from msg/HomePosition.msg rename to msg/versioned/HomePosition.msg index e6a517285fe4..a25bf241636e 100644 --- a/msg/HomePosition.msg +++ b/msg/versioned/HomePosition.msg @@ -1,5 +1,7 @@ # GPS home position in WGS84 coordinates. +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) float64 lat # Latitude in degrees diff --git a/msg/ManualControlSetpoint.msg b/msg/versioned/ManualControlSetpoint.msg similarity index 98% rename from msg/ManualControlSetpoint.msg rename to msg/versioned/ManualControlSetpoint.msg index 95fa62228344..ddf902f00f7d 100644 --- a/msg/ManualControlSetpoint.msg +++ b/msg/versioned/ManualControlSetpoint.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/msg/ModeCompleted.msg b/msg/versioned/ModeCompleted.msg similarity index 94% rename from msg/ModeCompleted.msg rename to msg/versioned/ModeCompleted.msg index 0a20b0294e53..1022df36d364 100644 --- a/msg/ModeCompleted.msg +++ b/msg/versioned/ModeCompleted.msg @@ -2,6 +2,9 @@ # The possible values of nav_state are defined in the VehicleStatus msg. # Note that this is not always published (e.g. when a user switches modes or on # failsafe activation) + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) diff --git a/msg/RegisterExtComponentReply.msg b/msg/versioned/RegisterExtComponentReply.msg similarity index 93% rename from msg/RegisterExtComponentReply.msg rename to msg/versioned/RegisterExtComponentReply.msg index 7cd7eef07b2b..4495bcdb90b5 100644 --- a/msg/RegisterExtComponentReply.msg +++ b/msg/versioned/RegisterExtComponentReply.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 request_id # ID from the request diff --git a/msg/RegisterExtComponentRequest.msg b/msg/versioned/RegisterExtComponentRequest.msg similarity index 97% rename from msg/RegisterExtComponentRequest.msg rename to msg/versioned/RegisterExtComponentRequest.msg index 46ab0cb0a15d..b0798705ca30 100644 --- a/msg/RegisterExtComponentRequest.msg +++ b/msg/versioned/RegisterExtComponentRequest.msg @@ -1,4 +1,7 @@ # Request to register an external component + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 request_id # ID, set this to a random value diff --git a/msg/TrajectorySetpoint.msg b/msg/versioned/TrajectorySetpoint.msg similarity index 95% rename from msg/TrajectorySetpoint.msg rename to msg/versioned/TrajectorySetpoint.msg index 4a88c86769a6..150be404b94f 100644 --- a/msg/TrajectorySetpoint.msg +++ b/msg/versioned/TrajectorySetpoint.msg @@ -3,6 +3,8 @@ # Needs to be kinematically consistent and feasible for smooth flight. # setting a value to NaN means the state should not be controlled +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # NED local world frame diff --git a/msg/UnregisterExtComponent.msg b/msg/versioned/UnregisterExtComponent.msg similarity index 92% rename from msg/UnregisterExtComponent.msg rename to msg/versioned/UnregisterExtComponent.msg index 2ad78d4b6836..8f85aaebfe4c 100644 --- a/msg/UnregisterExtComponent.msg +++ b/msg/versioned/UnregisterExtComponent.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) char[25] name # either the mode name, or component name diff --git a/msg/VehicleAngularVelocity.msg b/msg/versioned/VehicleAngularVelocity.msg similarity index 94% rename from msg/VehicleAngularVelocity.msg rename to msg/versioned/VehicleAngularVelocity.msg index db3767c0aa80..6c91f3c0703e 100644 --- a/msg/VehicleAngularVelocity.msg +++ b/msg/versioned/VehicleAngularVelocity.msg @@ -1,3 +1,4 @@ +uint32 MESSAGE_VERSION = 0 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) diff --git a/msg/VehicleAttitude.msg b/msg/versioned/VehicleAttitude.msg similarity index 96% rename from msg/VehicleAttitude.msg rename to msg/versioned/VehicleAttitude.msg index 99e6f25c2e42..fde3a85546c7 100644 --- a/msg/VehicleAttitude.msg +++ b/msg/versioned/VehicleAttitude.msg @@ -1,6 +1,8 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use # The quaternion uses the Hamilton convention, and the order is q(w, x, y, z) +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/versioned/VehicleAttitudeSetpoint.msg similarity index 96% rename from msg/VehicleAttitudeSetpoint.msg rename to msg/versioned/VehicleAttitudeSetpoint.msg index 74a753023df5..28aa780699dc 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/versioned/VehicleAttitudeSetpoint.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) float32 yaw_sp_move_rate # rad/s (commanded by user) diff --git a/msg/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg similarity index 99% rename from msg/VehicleCommand.msg rename to msg/versioned/VehicleCommand.msg index 2970410a886e..96162e9f89f9 100644 --- a/msg/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -1,6 +1,8 @@ # Vehicle Command uORB message. Used for commanding a mission / action / etc. # Follows the MAVLink COMMAND_INT / COMMAND_LONG definition +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint16 VEHICLE_CMD_CUSTOM_0 = 0 # test command diff --git a/msg/VehicleCommandAck.msg b/msg/versioned/VehicleCommandAck.msg similarity index 98% rename from msg/VehicleCommandAck.msg rename to msg/versioned/VehicleCommandAck.msg index 6f54fa46315b..c11a5f8c5dfd 100644 --- a/msg/VehicleCommandAck.msg +++ b/msg/versioned/VehicleCommandAck.msg @@ -2,6 +2,8 @@ # Used for acknowledging the vehicle command being received. # Follows the MAVLink COMMAND_ACK message definition +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # Result cases. This follows the MAVLink MAV_RESULT enum definition diff --git a/msg/VehicleControlMode.msg b/msg/versioned/VehicleControlMode.msg similarity index 97% rename from msg/VehicleControlMode.msg rename to msg/versioned/VehicleControlMode.msg index 9b33f9b8cad3..0a1fdedfbf14 100644 --- a/msg/VehicleControlMode.msg +++ b/msg/versioned/VehicleControlMode.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool flag_armed # synonym for actuator_armed.armed diff --git a/msg/VehicleGlobalPosition.msg b/msg/versioned/VehicleGlobalPosition.msg similarity index 98% rename from msg/VehicleGlobalPosition.msg rename to msg/versioned/VehicleGlobalPosition.msg index 30aaf5652a96..387576d8c792 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/versioned/VehicleGlobalPosition.msg @@ -5,6 +5,8 @@ # e.g. control inputs of the vehicle in a Kalman-filter implementation. # +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/msg/VehicleLandDetected.msg b/msg/versioned/VehicleLandDetected.msg similarity index 96% rename from msg/VehicleLandDetected.msg rename to msg/versioned/VehicleLandDetected.msg index fc0ca4a6d81c..1cbf54571360 100644 --- a/msg/VehicleLandDetected.msg +++ b/msg/versioned/VehicleLandDetected.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool freefall # true if vehicle is currently in free-fall diff --git a/msg/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg similarity index 99% rename from msg/VehicleLocalPosition.msg rename to msg/versioned/VehicleLocalPosition.msg index 0e74ac0f4bfc..aec6311ab383 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -1,6 +1,8 @@ # Fused local position in NED. # The coordinate system origin is the vehicle position at the time when the EKF2-module was started. +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/msg/VehicleOdometry.msg b/msg/versioned/VehicleOdometry.msg similarity index 97% rename from msg/VehicleOdometry.msg rename to msg/versioned/VehicleOdometry.msg index fbdd1920e1b4..f4d600d2b81e 100644 --- a/msg/VehicleOdometry.msg +++ b/msg/versioned/VehicleOdometry.msg @@ -1,4 +1,7 @@ # Vehicle odometry data. Fits ROS REP 147 for aerial vehicles + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample diff --git a/msg/VehicleRatesSetpoint.msg b/msg/versioned/VehicleRatesSetpoint.msg similarity index 95% rename from msg/VehicleRatesSetpoint.msg rename to msg/versioned/VehicleRatesSetpoint.msg index 35a06c35aa5f..a4c169e4123c 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/versioned/VehicleRatesSetpoint.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # body angular rates in FRD frame diff --git a/msg/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg similarity index 99% rename from msg/VehicleStatus.msg rename to msg/versioned/VehicleStatus.msg index 6756da812977..a347dfce3d9d 100644 --- a/msg/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,5 +1,7 @@ # Encodes the system state of the vehicle published by commander +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 armed_time # Arming timestamp (microseconds) diff --git a/msg/VtolVehicleStatus.msg b/msg/versioned/VtolVehicleStatus.msg similarity index 94% rename from msg/VtolVehicleStatus.msg rename to msg/versioned/VtolVehicleStatus.msg index 61a8246796f1..51beb30fa307 100644 --- a/msg/VtolVehicleStatus.msg +++ b/msg/versioned/VtolVehicleStatus.msg @@ -1,4 +1,7 @@ # VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE + +uint32 MESSAGE_VERSION = 0 + uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 From 2d667238f702ddca1fe4acb75589fa38c62f80fc Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Thu, 31 Oct 2024 13:15:42 +0100 Subject: [PATCH 142/211] tools: update scripts after msg/ restructure --- Tools/msg/generate_msg_docs.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/Tools/msg/generate_msg_docs.py b/Tools/msg/generate_msg_docs.py index a44a317b5279..e097e79672e0 100755 --- a/Tools/msg/generate_msg_docs.py +++ b/Tools/msg/generate_msg_docs.py @@ -11,9 +11,22 @@ def get_msgs_list(msgdir): """ - Makes list of msg files in the given directory + Makes a list of relative paths of .msg files in the given directory + and its subdirectories. + + Parameters: + msgdir (str): The directory to search for .msg files. + + Returns: + list: A list of relative paths to .msg files. """ - return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] + msgs = [] + for root, _, files in os.walk(msgdir): + for fn in files: + if fn.endswith(".msg"): + relative_path = os.path.relpath(os.path.join(root, fn), msgdir) + msgs.append(relative_path) + return msgs if __name__ == "__main__": @@ -32,7 +45,7 @@ def get_msgs_list(msgdir): filelist_in_markdown='' for msg_file in msg_files: - msg_name = os.path.splitext(msg_file)[0] + msg_name = os.path.splitext(os.path.basename(msg_file))[0] output_file = os.path.join(output_dir, msg_name+'.md') msg_filename = os.path.join(msg_path, msg_file) print("{:} -> {:}".format(msg_filename, output_file)) From 58e5b75d06178e73ee39d60faefe2a9051923619 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Thu, 31 Oct 2024 13:29:46 +0100 Subject: [PATCH 143/211] jenkins: update after msg/ restructure --- Jenkinsfile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Jenkinsfile b/Jenkinsfile index d7f166b5fe4d..9e9f5787c885 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -231,9 +231,13 @@ pipeline { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_msgs.git") // 'main' branch sh('rm -f px4_msgs/msg/*.msg') + sh('rm -f px4_msgs/msg/versioned/*.msg') sh('rm -f px4_msgs/srv/*.srv') + sh('rm -f px4_msgs/srv/versioned/*.srv') sh('cp msg/*.msg px4_msgs/msg/') + sh('mkdir -p px4_msgs/msg/versioned && cp msg/versioned/*.msg px4_msgs/msg/versioned/') sh('cp srv/*.srv px4_msgs/srv/') + sh('mkdir -p px4_msgs/srv/versioned && cp srv/versioned/*.srv px4_msgs/srv/versioned/') sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') sh('cd px4_msgs; git push origin main || true') sh('rm -rf px4_msgs') From 4d62522942fcf0848feb1b6c4c8a406be67e531d Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 20 Nov 2024 08:24:58 +0100 Subject: [PATCH 144/211] uxrce_dds_client: remove unused msgdir arg from script --- msg/CMakeLists.txt | 8 +------- src/modules/uxrce_dds_client/CMakeLists.txt | 1 - src/modules/uxrce_dds_client/generate_dds_topics.py | 3 --- 3 files changed, 1 insertion(+), 11 deletions(-) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 8d4e27931b88..10f34abe8133 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -448,13 +448,7 @@ if(CONFIG_LIB_CDRSTREAM) # Copy .msg files foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) - get_filename_component(msg_directory ${msg_file} DIRECTORY) - get_filename_component(msg_directory ${msg_directory} NAME) - if(msg_directory STREQUAL "versioned") - configure_file(${PX4_SOURCE_DIR}/msg/${msg_directory}/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) - else() - configure_file(${PX4_SOURCE_DIR}/msg/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) - endif() + configure_file(${msg_file} ${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) diff --git a/src/modules/uxrce_dds_client/CMakeLists.txt b/src/modules/uxrce_dds_client/CMakeLists.txt index acbc56c5a6b8..e5fc48154327 100644 --- a/src/modules/uxrce_dds_client/CMakeLists.txt +++ b/src/modules/uxrce_dds_client/CMakeLists.txt @@ -117,7 +117,6 @@ else() add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py - --topic-msg-dir ${PX4_SOURCE_DIR}/msg --client-outdir ${CMAKE_CURRENT_BINARY_DIR} --dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml --template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 4052958df87b..78e3bf3b9380 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -43,9 +43,6 @@ import yaml parser = argparse.ArgumentParser() -parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, - help="Topics message, by default using relative path 'msg/'", default="msg") - parser.add_argument("-y", "--dds-topics-file", dest='yaml_file', type=str, help="Setup topics file path, by default using 'dds_topics.yaml'") From 2e166379c76153694bf857028a7105a66085b68d Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 20 Nov 2024 16:11:57 +0100 Subject: [PATCH 145/211] docs: split msgs as un/versioned --- Tools/msg/generate_msg_docs.py | 30 ++++++++++++++++++++++++------ msg/CMakeLists.txt | 1 - 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/Tools/msg/generate_msg_docs.py b/Tools/msg/generate_msg_docs.py index e097e79672e0..193c56431fa1 100755 --- a/Tools/msg/generate_msg_docs.py +++ b/Tools/msg/generate_msg_docs.py @@ -42,7 +42,8 @@ def get_msgs_list(msgdir): msg_files = get_msgs_list(msg_path) msg_files.sort() - filelist_in_markdown='' + versioned_msgs_list = '' + unversioned_msgs_list = '' for msg_file in msg_files: msg_name = os.path.splitext(os.path.basename(msg_file))[0] @@ -94,10 +95,17 @@ def get_msgs_list(msgdir): with open(output_file, 'w') as content_file: content_file.write(markdown_output) - index_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name) - if summary_description: - index_markdown_file_link+=" — %s" % summary_description - filelist_in_markdown+=index_markdown_file_link+"\n" + # Categorize as versioned or unversioned + if "versioned" in msg_file: + versioned_msgs_list += '- [%s](%s.md)' % (msg_name, msg_name) + if summary_description: + versioned_msgs_list += " — %s" % summary_description + versioned_msgs_list += "\n" + else: + unversioned_msgs_list += '- [%s](%s.md)' % (msg_name, msg_name) + if summary_description: + unversioned_msgs_list += " — %s" % summary_description + unversioned_msgs_list += "\n" # Write out the index.md file index_text="""# uORB Message Reference @@ -107,10 +115,20 @@ def get_msgs_list(msgdir): ::: This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)). + +[Versioned messages](../middleware/uorb.md#message-versioning) track changes to their definitions, with each modification resulting in a version increment. +These messages are most likely shared through the PX4-ROS 2 Bridge. + Graphs showing how these are used [can be found here](../middleware/uorb_graph.md). +## Versioned Messages + +%s + +## Unversioned Messages + %s - """ % (filelist_in_markdown) + """ % (versioned_msgs_list, unversioned_msgs_list) index_file = os.path.join(output_dir, 'index.md') with open(index_file, 'w') as content_file: content_file.write(index_text) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 10f34abe8133..7abae7729370 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -56,7 +56,6 @@ set(msg_files CanInterfaceStatus.msg CellularStatus.msg CollisionConstraints.msg - CollisionReport.msg ControlAllocatorStatus.msg Cpuload.msg DatamanRequest.msg From 2a08d4bdb8c3e91abc523b5b939544f517830ee8 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 28 Nov 2024 16:07:06 +0100 Subject: [PATCH 146/211] add SensorAgpSim to generate Aux Global Position data --- .../init.d-posix/px4-rc.simulator | 8 + ROMFS/px4fmu_common/init.d/rcS | 1 + boards/px4/sitl/default.px4board | 1 + .../simulation/sensor_agp_sim/CMakeLists.txt | 43 ++++ src/modules/simulation/sensor_agp_sim/Kconfig | 5 + .../sensor_agp_sim/SensorAgpSim.cpp | 184 ++++++++++++++++++ .../sensor_agp_sim/SensorAgpSim.hpp | 82 ++++++++ .../simulation/sensor_agp_sim/parameters.c | 43 ++++ 8 files changed, 367 insertions(+) create mode 100644 src/modules/simulation/sensor_agp_sim/CMakeLists.txt create mode 100644 src/modules/simulation/sensor_agp_sim/Kconfig create mode 100644 src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp create mode 100644 src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp create mode 100644 src/modules/simulation/sensor_agp_sim/parameters.c diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 76be887e84bb..90e86aa6880e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -33,6 +33,10 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" then sensor_mag_sim start fi + if param compare -s SENS_EN_AGPSIM 1 + then + sensor_agp_sim start + fi else echo "ERROR [init] simulator_sih failed to start" @@ -153,6 +157,10 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then then sensor_airspeed_sim start fi + if param compare -s SENS_EN_AGPSIM 1 + then + sensor_agp_sim start + fi elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 28804361d1ed..3352cddad659 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -386,6 +386,7 @@ else sensor_baro_sim start sensor_mag_sim start sensor_gps_sim start + sensor_agp_sim start fi else diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index f157cf5ae8d7..1584f93ef647 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -52,6 +52,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/src/modules/simulation/sensor_agp_sim/CMakeLists.txt b/src/modules/simulation/sensor_agp_sim/CMakeLists.txt new file mode 100644 index 000000000000..e1390068eb2c --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__simulation__sensor_agp_sim + MAIN sensor_agp_sim + COMPILE_FLAGS + SRCS + SensorAgpSim.cpp + SensorAgpSim.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/simulation/sensor_agp_sim/Kconfig b/src/modules/simulation/sensor_agp_sim/Kconfig new file mode 100644 index 000000000000..43c6885246ec --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SIMULATION_SENSOR_AGP_SIM + bool "sensor_agp_sim" + default n + ---help--- + Enable support for sensor_agp_sim diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp new file mode 100644 index 000000000000..4b9327978f16 --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SensorAgpSim.hpp" + +#include +#include +#include + +using namespace matrix; + +SensorAgpSim::SensorAgpSim() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +SensorAgpSim::~SensorAgpSim() +{ + perf_free(_loop_perf); +} + +bool SensorAgpSim::init() +{ + ScheduleOnInterval(500_ms); // 2 Hz + return true; +} + +float SensorAgpSim::generate_wgn() +{ + // generate white Gaussian noise sample with std=1 + // from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / (float)RAND_MAX; + float U2 = (float)rand() / (float)RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); + } + + phase = !phase; + return X; +} + +void SensorAgpSim::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + if (_vehicle_global_position_sub.updated()) { + + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); + + double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); + + vehicle_global_position_s sample{}; + + sample.timestamp_sample = gpos.timestamp_sample; + sample.lat = latitude; + sample.lon = longitude; + sample.alt = altitude; + sample.lat_lon_valid = true; + sample.alt_ellipsoid = altitude; + sample.alt_valid = true; + sample.eph = 0.9f; + sample.epv = 1.78f; + + sample.timestamp = hrt_absolute_time(); + _aux_global_position_pub.publish(sample); + } + + perf_end(_loop_perf); +} + +int SensorAgpSim::task_spawn(int argc, char *argv[]) +{ + SensorAgpSim *instance = new SensorAgpSim(); + + 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 SensorAgpSim::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SensorAgpSim::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module to simulate auxiliary global position measurements with optional failure modes for SIH simulation. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sensor_agp_sim", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sensor_agp_sim_main(int argc, char *argv[]) +{ + return SensorAgpSim::main(argc, argv); +} diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp new file mode 100644 index 000000000000..6ef7d3aa9ac2 --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SensorAgpSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SensorAgpSim(); + ~SensorAgpSim() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + // generate white Gaussian noise sample with std=1 + static float generate_wgn(); + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; + + uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamInt) _sim_gps_used + ) +}; diff --git a/src/modules/simulation/sensor_agp_sim/parameters.c b/src/modules/simulation/sensor_agp_sim/parameters.c new file mode 100644 index 000000000000..6ee86e740b89 --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/parameters.c @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ +/** + * Simulate Aux Global Position (AGP) + * + * @reboot_required true + * @min 0 + * @max 1 + * @group Sensors + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0); From 3165a77e26023d083b2b2540ce60ca6978f2154e Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 6 Dec 2024 15:04:19 +0100 Subject: [PATCH 147/211] SIH-AGP: add sensor failure simulation --- .../sensor_agp_sim/SensorAgpSim.cpp | 28 +++++++++++++++---- .../sensor_agp_sim/SensorAgpSim.hpp | 13 ++++++++- .../simulation/sensor_agp_sim/parameters.c | 14 ++++++++++ 3 files changed, 49 insertions(+), 6 deletions(-) diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp index 4b9327978f16..4de5d46c2d54 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -107,9 +107,27 @@ void SensorAgpSim::Run() vehicle_global_position_s gpos{}; _vehicle_global_position_sub.copy(&gpos); - double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); + const uint64_t now = gpos.timestamp; + const float dt = (now - _time_last_update) * 1e-6f; + _time_last_update = now; + + if (!(_param_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { + _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); + } + + if (_param_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { + _position_bias += Vector3f(1.5f, -5.f, 0.f) * dt; + _measured_lla += _position_bias; + + } else { + _position_bias.zero(); + } + + const double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 / + CONSTANTS_RADIUS_OF_EARTH); + const double longitude = _measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 / + CONSTANTS_RADIUS_OF_EARTH); + const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f)); vehicle_global_position_s sample{}; @@ -120,8 +138,8 @@ void SensorAgpSim::Run() sample.lat_lon_valid = true; sample.alt_ellipsoid = altitude; sample.alt_valid = true; - sample.eph = 0.9f; - sample.epv = 1.78f; + sample.eph = 20.f; + sample.epv = 5.f; sample.timestamp = hrt_absolute_time(); _aux_global_position_pub.publish(sample); diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp index 6ef7d3aa9ac2..9700780adba3 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -64,11 +65,21 @@ class SensorAgpSim : public ModuleBase, public ModuleParams, publi bool init(); private: + enum class FailureMode : int32_t { + Stuck = (1 << 0), + Drift = (1 << 1) + }; + void Run() override; // generate white Gaussian noise sample with std=1 static float generate_wgn(); + LatLonAlt _measured_lla{}; + matrix::Vector3f _position_bias{}; + + uint64_t _time_last_update{}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; @@ -77,6 +88,6 @@ class SensorAgpSim : public ModuleBase, public ModuleParams, publi perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; DEFINE_PARAMETERS( - (ParamInt) _sim_gps_used + (ParamInt) _param_sim_agp_fail ) }; diff --git a/src/modules/simulation/sensor_agp_sim/parameters.c b/src/modules/simulation/sensor_agp_sim/parameters.c index 6ee86e740b89..6690255f0912 100644 --- a/src/modules/simulation/sensor_agp_sim/parameters.c +++ b/src/modules/simulation/sensor_agp_sim/parameters.c @@ -41,3 +41,17 @@ * @value 1 Enabled */ PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0); + +/** + * AGP failure mode + * + * Stuck: freeze the measurement to the current location + * Drift: add a linearly growing bias to the sensor data + * + * @group Simulator + * @min 0 + * @max 3 + * @bit 0 Stuck + * @bit 1 Drift + */ +PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0); From 3941b19968ea01a9de1ff16a0f23578de07257d7 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 6 Dec 2024 15:03:55 +0100 Subject: [PATCH 148/211] ekf2_replay: log groundtruth messages --- src/modules/replay/ReplayEkf2.cpp | 12 ++++++++++++ src/modules/replay/ReplayEkf2.hpp | 3 +++ 2 files changed, 15 insertions(+) diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 1add36489d82..89f4874ce545 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -108,6 +108,15 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(aux_global_position)) { _aux_global_position_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_local_position_groundtruth)) { + _vehicle_local_position_groundtruth_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_attitude_groundtruth)) { + _vehicle_attitude_groundtruth_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) { + _vehicle_global_position_groundtruth_msg_id = msg_id; } // the main loop should only handle publication of the following topics, the sensor topics are @@ -135,6 +144,9 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id); handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id); handle_sensor_publication(0, _aux_global_position_msg_id); + handle_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id); + handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id); + handle_sensor_publication(0, _vehicle_attitude_groundtruth_msg_id); // sensor_combined: publish last because ekf2 is polling on this if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) { diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index bb7f8d135939..13cb3d8be5d5 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -89,6 +89,9 @@ class ReplayEkf2 : public Replay uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid; uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid; uint16_t _aux_global_position_msg_id = msg_id_invalid; + uint16_t _vehicle_local_position_groundtruth_msg_id = msg_id_invalid; + uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid; + uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid; }; } //namespace px4 From d7904b5f2cb0f52e325ebe147fcdd6942acef82e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 13 Dec 2024 16:32:47 +0100 Subject: [PATCH 149/211] mixer: FunctionMotors: leave NAN control values at NAN with non-zero THR_MDL_FAC Signed-off-by: Silvan Fuhrer --- src/lib/mixer_module/functions/FunctionMotors.hpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index fa42c8b63d22..65837cfa75eb 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -87,16 +87,19 @@ class FunctionMotors : public FunctionProviderBase const float tmp2 = b * b / (4.f * a * a); for (int i = 0; i < num_values; ++i) { + float control = values[i]; - if (control > 0.f) { - values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); + if (PX4_ISFINITE(control)) { + if (control > 0.f) { + values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); - } else if (control < -0.f) { - values[i] = tmp1 - sqrtf(tmp2 - (control / a)); + } else if (control < -0.f) { + values[i] = tmp1 - sqrtf(tmp2 - (control / a)); - } else { - values[i] = 0.f; + } else { + values[i] = 0.f; + } } } } From 4db55cd25fe8ec67e2796b5116a87b1630bab763 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 13 Dec 2024 16:34:13 +0100 Subject: [PATCH 150/211] mixer: FunctionMotors: compare against FLT_EPSILON instead of 0.f Signed-off-by: Silvan Fuhrer --- src/lib/mixer_module/functions/FunctionMotors.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index 65837cfa75eb..efba28188457 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -76,7 +76,7 @@ class FunctionMotors : public FunctionProviderBase static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) { - if (thrust_factor > 0.f && thrust_factor <= 1.f) { + if (thrust_factor > FLT_EPSILON && thrust_factor <= 1.f) { // thrust factor // rel_thrust = factor * x^2 + (1-factor) * x, const float a = thrust_factor; @@ -88,17 +88,15 @@ class FunctionMotors : public FunctionProviderBase for (int i = 0; i < num_values; ++i) { - float control = values[i]; + const float control = values[i]; if (PX4_ISFINITE(control)) { - if (control > 0.f) { + if (control > FLT_EPSILON) { values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); - } else if (control < -0.f) { + } else if (control < -FLT_EPSILON) { values[i] = tmp1 - sqrtf(tmp2 - (control / a)); - } else { - values[i] = 0.f; } } } From a751e900f51176efd9bfe58e8e4c762bd9d71fb8 Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Sat, 14 Dec 2024 00:39:04 +0000 Subject: [PATCH 151/211] Update submodule mavlink to latest Sat Dec 14 00:39:04 UTC 2024 - mavlink in PX4/Firmware (661ae0c0e3ac493ceddd13120e8ccb5bac47d887): https://github.com/mavlink/mavlink/commit/0e420102dbdd7e9f59617dd00c05a2470f22eef2 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/5e3a42b8f3f53038f2779f9f69bd64767b913bb8 - Changes: https://github.com/mavlink/mavlink/compare/0e420102dbdd7e9f59617dd00c05a2470f22eef2...5e3a42b8f3f53038f2779f9f69bd64767b913bb8 5e3a42b8 2024-12-12 Hamish Willee - development: Remove parameter transaction proposal (#2169) 25f4e9f0 2024-12-11 Hamish Willee - STANDARD_MODE_SAFE_RECOVERY + RTL merge (#2191) d5a8cb43 2024-12-11 Hamish Willee - Docs generate fix (#2194) 35f70c4a 2024-12-05 Hamish Willee - Create index.md, dialects.md and make notes/warning work with vitepress (#2193) 57c02856 2024-12-05 Hamish Willee - CAMERA_THERMAL_RANGE - no longer WIP (#2189) 1ee2ebe1 2024-12-04 Hamish Willee - fetch_dialect_ardupilotmega.yml: Not fail if nothing to commit (#2181) 1aa8c2d0 2024-12-04 Hamish Willee - ardupilotmega - remove_mav_cmd_external_estimate (#2187) 49fa509a 2024-11-30 github-actions[bot] - ardupilotmega dialects from ArduPilot/mavlink: Fri Nov 29 23:49:36 UTC 2024 (#2185) 75ebfc8f 2024-11-30 Hamish Willee - Update fetch_dialect_ardupilotmega.yml - fix copy error (#2184) 8e97709d 2024-11-30 Hamish Willee - fetch_dialect_ardupilotmega.yml - add loweheiser (#2183) 82b81aa8 2024-11-27 github-actions[bot] - ardupilotmega.xml from ArduPilot/mavlink: Wed Nov 27 04:13:16 UTC 2024 (#2178) 5c421a33 2024-11-27 Hamish Willee - Speed uncertainty units /s (#2180) 33f8a327 2024-11-24 David Sastre - Some modifications to multi-GCS protocol (#2179) 9938f940 2024-11-21 Hamish Willee - fetch_dialect_ardupilotmega - create (#2177) a55d0ec5 2024-11-21 Hamish Willee - MAV_CMD_GROUP_START and _GROUP_END delete wip (#2176) 1cf3c721 2024-11-21 Hamish Willee - dev: move FUEL_STATUS to common (#2170) 7ecb8e33 2024-11-21 Hamish Willee - WIFI_NETWORK_SECURITY - wip removal (#2164) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 0e420102dbdd..5e3a42b8f3f5 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 0e420102dbdd7e9f59617dd00c05a2470f22eef2 +Subproject commit 5e3a42b8f3f53038f2779f9f69bd64767b913bb8 From 1718b37fe4ba381d08fd8c1f552f8d685e2e356d Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 8 Nov 2024 09:11:57 +0100 Subject: [PATCH 152/211] SENS: RNG: SF45 changed data processing and publication design, moved to a publishing per sector design. other minor improvements --- .../lightware_sf45_serial.cpp | 94 +++++++++++++------ .../lightware_sf45_serial.hpp | 75 +++++++++------ .../lightware_sf45_serial_main.cpp | 16 +--- .../lightware_sf45_serial/module.yaml | 12 +-- src/modules/logger/logged_topics.cpp | 2 + 5 files changed, 120 insertions(+), 79 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index 22ae7c820703..c5e0c750b2d7 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -47,9 +47,8 @@ #define SF45_MAX_PAYLOAD 256 #define SF45_SCALE_FACTOR 0.01f -SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : +SF45LaserSerial::SF45LaserSerial(const char *port) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0, rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { @@ -69,15 +68,18 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : } _num_retries = 2; - _px4_rangefinder.set_device_id(device_id.devid); - _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); // populate obstacle map members _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + _obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; _obstacle_map_msg.increment = 5; - _obstacle_map_msg.angle_offset = 2.5; - _obstacle_map_msg.min_distance = UINT16_MAX; + _obstacle_map_msg.min_distance = 20; _obstacle_map_msg.max_distance = 5000; + _obstacle_map_msg.angle_offset = 0; + + for (uint32_t i = 0 ; i < BIN_COUNT; i++) { + _obstacle_map_msg.distances[i] = UINT16_MAX; + } } @@ -91,16 +93,11 @@ SF45LaserSerial::~SF45LaserSerial() int SF45LaserSerial::init() { - param_get(param_find("SF45_UPDATE_CFG"), &_update_rate); param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); - /* SF45/B (50M) */ - _px4_rangefinder.set_min_distance(0.2f); - _px4_rangefinder.set_max_distance(50.0f); _interval = 10000; - start(); return PX4_OK; @@ -161,7 +158,6 @@ int SF45LaserSerial::collect() float distance_m = -1.0f; /* read from the sensor (uart buffer) */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); @@ -214,7 +210,6 @@ int SF45LaserSerial::collect() // Stream data from sensor } else { - ret = ::read(_fd, &readbuf[0], 10); if (ret < 0) { @@ -262,7 +257,7 @@ int SF45LaserSerial::collect() } PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO")); - _px4_rangefinder.update(timestamp_sample, distance_m); + perf_end(_sample_perf); @@ -687,8 +682,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) { switch (rx_field.msg_id) { case SF_DISTANCE_DATA_CM: { - - uint16_t obstacle_dist_cm = 0; const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); int16_t scaled_yaw = 0; @@ -700,7 +693,7 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle - if (_orient_cfg == 1) { + if (_orient_cfg == ROTATION_DOWNWARD_FACING) { raw_yaw = raw_yaw * -1; } @@ -708,10 +701,10 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; switch (_yaw_cfg) { - case 0: + case ROTATION_FORWARD_FACING: break; - case 1: + case ROTATION_BACKWARD_FACING: if (scaled_yaw > 180) { scaled_yaw = scaled_yaw - 180; @@ -721,11 +714,11 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) break; - case 2: + case ROTATION_RIGHT_FACING: scaled_yaw = scaled_yaw + 90; // rotation facing right break; - case 3: + case ROTATION_LEFT_FACING: scaled_yaw = scaled_yaw - 90; // rotation facing left break; @@ -733,26 +726,65 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) break; } - // Convert to meters for rangefinder update + // Convert to meters for the debug message *distance_m = raw_distance * SF45_SCALE_FACTOR; - obstacle_dist_cm = (uint16_t)raw_distance; + _current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist; uint8_t current_bin = sf45_convert_angle(scaled_yaw); - // If we have moved to a new bin - if (current_bin != _previous_bin) { + PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin, + (double)*distance_m); + + if (_current_bin_dist > _obstacle_map_msg.max_distance) { + _current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition + } + + // if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement. + // in this case we assume the measurement to be valid for all bins between the previous and the current bin. win + uint8_t start; + uint8_t end; + + if (abs(current_bin - _previous_bin) > BIN_COUNT / + 4) { // wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins + // TODO: differentiate direction of wrap-around, currently it overwrites a previous measurement. + start = math::max(_previous_bin, current_bin); + end = math::min(_previous_bin, current_bin); + + } else if (_previous_bin < current_bin) { // Scanning clockwise + start = _previous_bin + 1; + end = current_bin; + + } else { // scanning counter-clockwise + start = current_bin; + end = _previous_bin - 1; + } + + if (start <= end) { + for (uint8_t i = start; i <= end; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;} + + } else { // wrap-around case + for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;} + + for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;} + } - // update the current bin to the distance sensor reading - // readings in cm - _obstacle_map_msg.distances[current_bin] = obstacle_dist_cm; _obstacle_map_msg.timestamp = hrt_absolute_time(); + _obstacle_distance_pub.publish(_obstacle_map_msg); - } + // reset the values for the next measurement + if (start <= end) { + for (uint8_t i = start; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;} - _previous_bin = current_bin; + } else { // wrap-around case + for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;} - _obstacle_distance_pub.publish(_obstacle_map_msg); + for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;} + } + + _current_bin_dist = UINT16_MAX; + _previous_bin = current_bin; + } break; } diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index 89e736719262..c2d0961f01da 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -63,10 +63,20 @@ enum SF_SERIAL_STATE { }; + +enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE + ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 + ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180 + ROTATION_LEFT_FACING = 6, // MAV_SENSOR_ROTATION_YAW_270 + ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90 + ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270 +}; +using namespace time_literals; class SF45LaserSerial : public px4::ScheduledWorkItem { public: - SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + SF45LaserSerial(const char *port); ~SF45LaserSerial() override; int init(); @@ -77,11 +87,11 @@ class SF45LaserSerial : public px4::ScheduledWorkItem void sf45_process_replies(float *data); uint8_t sf45_convert_angle(const int16_t yaw); float sf45_wrap_360(float f); -protected: - obstacle_distance_s _obstacle_map_msg{}; - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ private: + obstacle_distance_s _obstacle_map_msg{}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ + static constexpr int BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); void start(); void stop(); @@ -89,43 +99,46 @@ class SF45LaserSerial : public px4::ScheduledWorkItem int measure(); int collect(); bool _crc_valid{false}; - PX4Rangefinder _px4_rangefinder; + + void _publish_obstacle_msg(hrt_abstime now); + uint64_t _data_timestamps[BIN_COUNT]; + char _port[20] {}; - int _interval{10000}; + int _interval{10000}; bool _collect_phase{false}; int _fd{-1}; - int _linebuf[256] {}; - unsigned _linebuf_index{0}; - hrt_abstime _last_read{0}; + int _linebuf[256] {}; + unsigned _linebuf_index{0}; + hrt_abstime _last_read{0}; // SF45/B uses a binary protocol to include header,flags // message ID, payload, and checksum - bool _is_sf45{false}; - bool _init_complete{false}; - bool _sensor_ready{false}; - uint8_t _sensor_state{0}; - int _baud_rate{0}; - int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int _stream_data{0}; - int32_t _update_rate{1}; - int _data_output{0}; - const uint8_t _start_of_frame{0xAA}; - uint16_t _data_bytes_recv{0}; - uint8_t _parsed_state{0}; - bool _sop_valid{false}; - uint16_t _calc_crc{0}; - uint8_t _num_retries{0}; - int32_t _yaw_cfg{0}; - int32_t _orient_cfg{0}; - int32_t _collision_constraint{0}; - uint16_t _previous_bin{0}; + bool _is_sf45{false}; + bool _init_complete{false}; + bool _sensor_ready{false}; + uint8_t _sensor_state{0}; + int _baud_rate{0}; + int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + int _stream_data{0}; + int32_t _update_rate{1}; + int _data_output{0}; + const uint8_t _start_of_frame{0xAA}; + uint16_t _data_bytes_recv{0}; + uint8_t _parsed_state{0}; + bool _sop_valid{false}; + uint16_t _calc_crc{0}; + uint8_t _num_retries{0}; + int32_t _yaw_cfg{0}; + int32_t _orient_cfg{0}; + uint8_t _previous_bin{0}; + uint16_t _current_bin_dist{UINT16_MAX}; // end of SF45/B data members - unsigned _consecutive_fail_count; + unsigned _consecutive_fail_count; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; }; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp index f05417e053bd..3446ad1a359f 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp @@ -41,7 +41,7 @@ namespace lightware_sf45 SF45LaserSerial *g_dev{nullptr}; -static int start(const char *port, uint8_t rotation) +static int start(const char *port) { if (g_dev != nullptr) { PX4_WARN("already started"); @@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation) } /* create the driver */ - g_dev = new SF45LaserSerial(port, rotation); + g_dev = new SF45LaserSerial(port); if (g_dev == nullptr) { return -1; @@ -102,7 +102,7 @@ static int usage() Serial bus driver for the Lightware SF45/b Laser rangefinder. -Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html +Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html ### Examples @@ -116,7 +116,6 @@ Stop driver PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false); PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); return PX4_OK; } @@ -125,18 +124,13 @@ Stop driver extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) { - uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING; const char *device_path = nullptr; int ch; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - break; - case 'd': device_path = myoptarg; break; @@ -153,7 +147,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) } if (!strcmp(argv[myoptind], "start")) { - return lightware_sf45::start(device_path, rotation); + return lightware_sf45::start(device_path); } else if (!strcmp(argv[myoptind], "stop")) { return lightware_sf45::stop(); diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml index 6356411fbe43..4dbdf25c9083 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -1,6 +1,6 @@ module_name: Lightware SF45 Rangefinder (serial) serial_config: - - command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV} + - command: lightware_sf45_serial start -d ${SERIAL_DEV} port_config_param: name: SENS_EN_SF45_CFG group: Sensors @@ -41,11 +41,11 @@ parameters: The SF45 mounted facing upward or downward on the frame type: enum values: - 0: Rotation upward - 1: Rotation downward + 24: Rotation upward + 25: Rotation downward reboot_required: true num_instances: 1 - default: 0 + default: 24 SF45_YAW_CFG: description: @@ -55,9 +55,9 @@ parameters: type: enum values: 0: Rotation forward - 1: Rotation backward 2: Rotation right - 3: Rotation left + 4: Rotation backward + 6: Rotation left reboot_required: true num_instances: 1 default: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..d1e67a5cd7c9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -323,7 +323,9 @@ void LoggedTopics::add_sensor_comparison_topics() void LoggedTopics::add_vision_and_avoidance_topics() { add_topic("collision_constraints"); + add_topic_multi("distance_sensor"); add_topic("obstacle_distance_fused"); + add_topic("obstacle_distance"); add_topic("vehicle_mocap_odometry", 30); add_topic("vehicle_trajectory_waypoint", 200); add_topic("vehicle_trajectory_waypoint_desired", 200); From 88d771e3e5cfd707447ee47d5a3d575895b98a8b Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Tue, 19 Nov 2024 17:09:25 +0100 Subject: [PATCH 153/211] SENS: RNG: SF45: Fixed sf45 parser, added general checks to avoid potential out-of-bound access --- .../lightware_sf45_serial.cpp | 89 ++++++++++--------- .../lightware_sf45_serial.hpp | 25 +++--- 2 files changed, 59 insertions(+), 55 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index c5e0c750b2d7..d5a4625a665c 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "lightware_sf45_serial.hpp" -#include "sf45_commands.h" #include #include @@ -44,7 +43,6 @@ #include /* Configuration Constants */ -#define SF45_MAX_PAYLOAD 256 #define SF45_SCALE_FACTOR 0.01f SF45LaserSerial::SF45LaserSerial(const char *port) : @@ -105,7 +103,6 @@ int SF45LaserSerial::init() int SF45LaserSerial::measure() { - int rate = (int)_update_rate; _data_output = 0x101; // raw distance + yaw readings _stream_data = 5; // enable constant streaming @@ -150,20 +147,13 @@ int SF45LaserSerial::collect() { perf_begin(_sample_perf); - /* clear buffer if last read was too long ago */ int ret; - /* the buffer for read chars is buflen minus null termination */ - uint8_t readbuf[SF45_MAX_PAYLOAD]; - float distance_m = -1.0f; - /* read from the sensor (uart buffer) */ - - - if (_sensor_state == STATE_SEND_PRODUCT_NAME) { - ret = ::read(_fd, &readbuf[0], 22); + const int payload_length = 22; + ret = ::read(_fd, &_linebuf[0], payload_length); if (ret < 0) { PX4_ERR("ERROR (ack from sending product name cmd): %d", ret); @@ -172,12 +162,13 @@ int SF45LaserSerial::collect() return ret; } - sf45_request_handle(ret, readbuf); + sf45_request_handle(_linebuf); ScheduleDelayed(_interval * 3); } else if (_sensor_state == STATE_SEND_UPDATE_RATE) { - ret = ::read(_fd, &readbuf[0], 7); + const int payload_length = 7; + ret = ::read(_fd, &_linebuf[0], payload_length); if (ret < 0) { PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret); @@ -186,14 +177,15 @@ int SF45LaserSerial::collect() return ret; } - if (readbuf[3] == SF_UPDATE_RATE) { - sf45_request_handle(ret, readbuf); + if (ret == payload_length && _linebuf[3] == SF_UPDATE_RATE) { + sf45_request_handle(_linebuf); ScheduleDelayed(_interval * 3); } } else if (_sensor_state == STATE_SEND_DISTANCE_DATA) { - ret = ::read(_fd, &readbuf[0], 8); + const int payload_length = 8; + ret = ::read(_fd, &_linebuf[0], payload_length); if (ret < 0) { PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret); @@ -202,46 +194,58 @@ int SF45LaserSerial::collect() return ret; } - if (readbuf[3] == SF_DISTANCE_OUTPUT) { - sf45_request_handle(ret, readbuf); + if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) { + sf45_request_handle(_linebuf); ScheduleDelayed(_interval * 3); } - // Stream data from sensor } else { - ret = ::read(_fd, &readbuf[0], 10); + // Stream data from sensor + const int payload_length = 10; + + size_t max_read = sizeof(_linebuf) - _linebuf_size; + ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); + _linebuf_size += ret; if (ret < 0) { PX4_ERR("ERROR (ack from streaming distance data): %d", ret); + _linebuf_size = 0; perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2); + // Not enough data to parse a complete packet. Gather more data in the next cycle. + if (_linebuf_size < payload_length) { + return -EAGAIN; + } - // Process the incoming distance data - if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { + int index = _linebuf_size - payload_length; - for (uint8_t i = 0; i < ret; ++i) { - sf45_request_handle(ret, readbuf); - } + while (index >= 0) { + if (_linebuf[index] == 0xAA) { + uint8_t flags_payload = (_linebuf[index + 1] >> 6) | (_linebuf[index + 2] << 2); - if (_init_complete) { - sf45_process_replies(&distance_m); - } // end if + // Process the incoming distance data + if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { + sf45_request_handle(&_linebuf[index]); - } else { + if (_init_complete && _crc_valid) { + sf45_process_replies(&distance_m); + _linebuf_size = 0; + break; + } + } + } - ret = ::read(_fd, &readbuf[0], 10); + index--; + } - if (ret < 0) { - PX4_ERR("ERROR (unknown sensor data): %d", ret); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } + // The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again. + if (_linebuf_size >= sizeof(_linebuf)) { + _linebuf_size = 0; + perf_count(_comms_errors); } } @@ -256,8 +260,7 @@ int SF45LaserSerial::collect() return -EAGAIN; } - PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO")); - + PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); perf_end(_sample_perf); @@ -269,6 +272,9 @@ void SF45LaserSerial::start() /* reset the report ring and state machine */ _collect_phase = false; + /* reset the UART receive buffer size */ + _linebuf_size = 0; + /* schedule a cycle to start things */ ScheduleNow(); } @@ -394,9 +400,8 @@ void SF45LaserSerial::print_info() perf_print_counter(_comms_errors); } -void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf) +void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf) { - // SF45 protocol // Start byte is 0xAA and is the start of packet // Payload length sanity check (0-1023) bytes diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index c2d0961f01da..4f75b3285201 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -63,7 +63,6 @@ enum SF_SERIAL_STATE { }; - enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 @@ -79,14 +78,14 @@ class SF45LaserSerial : public px4::ScheduledWorkItem SF45LaserSerial(const char *port); ~SF45LaserSerial() override; - int init(); + int init(); void print_info(); - void sf45_request_handle(int val, uint8_t *value); - void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); - uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); - void sf45_process_replies(float *data); - uint8_t sf45_convert_angle(const int16_t yaw); - float sf45_wrap_360(float f); + void sf45_request_handle(uint8_t *value); + void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); + uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); + void sf45_process_replies(float *data); + uint8_t sf45_convert_angle(const int16_t yaw); + float sf45_wrap_360(float f); private: obstacle_distance_s _obstacle_map_msg{}; @@ -98,19 +97,19 @@ class SF45LaserSerial : public px4::ScheduledWorkItem void Run() override; int measure(); int collect(); - bool _crc_valid{false}; + bool _crc_valid{false}; void _publish_obstacle_msg(hrt_abstime now); uint64_t _data_timestamps[BIN_COUNT]; char _port[20] {}; - int _interval{10000}; + int _interval{10000}; bool _collect_phase{false}; int _fd{-1}; - int _linebuf[256] {}; - unsigned _linebuf_index{0}; - hrt_abstime _last_read{0}; + uint8_t _linebuf[SF45_MAX_PAYLOAD] {}; + unsigned _linebuf_size{0}; + hrt_abstime _last_read{0}; // SF45/B uses a binary protocol to include header,flags // message ID, payload, and checksum From f34b22907c7d9e4e17c7a0ad66057ac5f1417110 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Tue, 3 Dec 2024 10:37:15 +0100 Subject: [PATCH 154/211] SENS: RNG: SF45:Fix startup problems, increase frequency, robust parser, use nonblocking reads --- .../lightware_sf45_serial.cpp | 526 ++++++++---------- .../lightware_sf45_serial.hpp | 32 +- .../lightware_sf45_serial/module.yaml | 2 +- 3 files changed, 232 insertions(+), 328 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index d5a4625a665c..b3ac5db49654 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -37,10 +37,11 @@ #include #include #include +#include #include -#include -#include + +using namespace time_literals; /* Configuration Constants */ #define SF45_SCALE_FACTOR 0.01f @@ -65,8 +66,6 @@ SF45LaserSerial::SF45LaserSerial(const char *port) : device_id.devid_s.bus = bus_num; } - _num_retries = 2; - // populate obstacle map members _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; @@ -78,7 +77,6 @@ SF45LaserSerial::SF45LaserSerial(const char *port) : for (uint32_t i = 0 ; i < BIN_COUNT; i++) { _obstacle_map_msg.distances[i] = UINT16_MAX; } - } SF45LaserSerial::~SF45LaserSerial() @@ -95,43 +93,36 @@ int SF45LaserSerial::init() param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); - _interval = 10000; start(); - return PX4_OK; } int SF45LaserSerial::measure() { - int rate = (int)_update_rate; - _data_output = 0x101; // raw distance + yaw readings + int32_t rate = (int32_t)_update_rate; + _data_output = 0x101; // raw distance (first return) + yaw readings _stream_data = 5; // enable constant streaming - // send some packets so the sensor starts scanning + // send packets to the sensor depending on the state switch (_sensor_state) { - // sensor should now respond case STATE_UNINIT: - while (_num_retries--) { - sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); - _sensor_state = STATE_UNINIT; - } - - _sensor_state = STATE_SEND_PRODUCT_NAME; + // Used to probe if the sensor is alive + sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); break; - case STATE_SEND_PRODUCT_NAME: + case STATE_ACK_PRODUCT_NAME: // Update rate default to 50 readings/s sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t)); - _sensor_state = STATE_SEND_UPDATE_RATE; break; - case STATE_SEND_UPDATE_RATE: + case STATE_ACK_UPDATE_RATE: + // Configure the data that the sensor shall output sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output)); - _sensor_state = STATE_SEND_DISTANCE_DATA; break; - case STATE_SEND_DISTANCE_DATA: + case STATE_ACK_DISTANCE_OUTPUT: + // Configure the sensor to automatically output data at the configured update rate sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data)); _sensor_state = STATE_SEND_STREAM; break; @@ -145,136 +136,90 @@ int SF45LaserSerial::measure() int SF45LaserSerial::collect() { - perf_begin(_sample_perf); - - int ret; float distance_m = -1.0f; - if (_sensor_state == STATE_SEND_PRODUCT_NAME) { + if (_sensor_state == STATE_UNINIT) { + perf_begin(_sample_perf); const int payload_length = 22; - ret = ::read(_fd, &_linebuf[0], payload_length); - if (ret < 0) { - PX4_ERR("ERROR (ack from sending product name cmd): %d", ret); - perf_count(_comms_errors); + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME); + + if (_crc_valid) { + _sensor_state = STATE_ACK_PRODUCT_NAME; perf_end(_sample_perf); - return ret; + return PX4_OK; } - sf45_request_handle(_linebuf); - ScheduleDelayed(_interval * 3); + return -EAGAIN; - } else if (_sensor_state == STATE_SEND_UPDATE_RATE) { + } else if (_sensor_state == STATE_ACK_PRODUCT_NAME) { + perf_begin(_sample_perf); const int payload_length = 7; - ret = ::read(_fd, &_linebuf[0], payload_length); - if (ret < 0) { - PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - if (ret == payload_length && _linebuf[3] == SF_UPDATE_RATE) { - sf45_request_handle(_linebuf); - ScheduleDelayed(_interval * 3); - } + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE); - } else if (_sensor_state == STATE_SEND_DISTANCE_DATA) { - - const int payload_length = 8; - ret = ::read(_fd, &_linebuf[0], payload_length); - - if (ret < 0) { - PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret); - perf_count(_comms_errors); + if (_crc_valid) { + _sensor_state = STATE_ACK_UPDATE_RATE; perf_end(_sample_perf); - return ret; + return PX4_OK; } - if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) { - sf45_request_handle(_linebuf); - ScheduleDelayed(_interval * 3); - } + return -EAGAIN; + } else if (_sensor_state == STATE_ACK_UPDATE_RATE) { - } else { - // Stream data from sensor + perf_begin(_sample_perf); const int payload_length = 10; - size_t max_read = sizeof(_linebuf) - _linebuf_size; - ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); - _linebuf_size += ret; + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT); - if (ret < 0) { - PX4_ERR("ERROR (ack from streaming distance data): %d", ret); - _linebuf_size = 0; - perf_count(_comms_errors); + if (_crc_valid) { + _sensor_state = STATE_ACK_DISTANCE_OUTPUT; perf_end(_sample_perf); - return ret; - } - - // Not enough data to parse a complete packet. Gather more data in the next cycle. - if (_linebuf_size < payload_length) { - return -EAGAIN; + return PX4_OK; } - int index = _linebuf_size - payload_length; - - while (index >= 0) { - if (_linebuf[index] == 0xAA) { - uint8_t flags_payload = (_linebuf[index + 1] >> 6) | (_linebuf[index + 2] << 2); + return -EAGAIN; - // Process the incoming distance data - if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { - sf45_request_handle(&_linebuf[index]); + } else { - if (_init_complete && _crc_valid) { - sf45_process_replies(&distance_m); - _linebuf_size = 0; - break; - } - } - } + // Stream data from sensor + perf_begin(_sample_perf); + const int payload_length = 10; - index--; - } + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM); - // The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again. - if (_linebuf_size >= sizeof(_linebuf)) { - _linebuf_size = 0; - perf_count(_comms_errors); + if (_crc_valid) { + sf45_process_replies(&distance_m); + PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); + perf_end(_sample_perf); + return PX4_OK; } - } - if (_consecutive_fail_count > 35 && !_sensor_ready) { - PX4_ERR("Restarting the state machine"); - return PX4_ERROR; - } - - _last_read = hrt_absolute_time(); - - if (!_crc_valid) { return -EAGAIN; } - - PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); - - perf_end(_sample_perf); - - return PX4_OK; } void SF45LaserSerial::start() { - /* reset the report ring and state machine */ + /* reset the sensor state */ + _sensor_state = STATE_UNINIT; + + /* reset the report ring */ _collect_phase = false; /* reset the UART receive buffer size */ _linebuf_size = 0; + /* reset the fail counter */ + _last_received_time = hrt_absolute_time(); + /* schedule a cycle to start things */ ScheduleNow(); } @@ -289,8 +234,7 @@ void SF45LaserSerial::Run() /* fds initialized? */ if (_fd < 0) { /* open fd: non-blocking read mode*/ - - _fd = ::open(_port, O_RDWR | O_NOCTTY); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); if (_fd < 0) { PX4_ERR("serial open failed (%i)", errno); @@ -304,34 +248,11 @@ void SF45LaserSerial::Run() /* fill the struct for the new configuration */ tcgetattr(_fd, &uart_config); - uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8; - - uart_config.c_cflag |= (CLOCAL | CREAD); - - // no parity, 1 stop bit, flow control disabled - uart_config.c_cflag &= ~(PARENB | PARODD); - - uart_config.c_cflag |= 0; - - uart_config.c_cflag &= ~CSTOPB; - - uart_config.c_cflag &= ~CRTSCTS; - - uart_config.c_iflag &= ~IGNBRK; - - uart_config.c_iflag &= ~ICRNL; + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - - // echo and echo NL off, canonical mode off (raw mode) - // extended input processing off, signal chars off - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - uart_config.c_oflag = 0; - - uart_config.c_cc[VMIN] = 0; - - uart_config.c_cc[VTIME] = 1; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); unsigned speed = B921600; @@ -347,51 +268,37 @@ void SF45LaserSerial::Run() if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { PX4_ERR("baud %d ATTR", termios_state); } - } if (_collect_phase) { - - /* perform collection */ - int collect_ret = collect(); - - - if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ - ScheduleDelayed(1042 * 8); - + if (hrt_absolute_time() - _last_received_time >= 1_s) { + start(); return; } - if (OK != collect_ret) { - // Too many packet errors in init, restart the consecutive fail count - _consecutive_fail_count = 0; - - /* restart the measurement state machine */ - start(); - return; + /* perform collection */ + if (collect() != PX4_OK && errno != EAGAIN) { + PX4_DEBUG("collect error"); + } - } else { - /* apparently success */ - _consecutive_fail_count = 0; + if (_sensor_state != STATE_SEND_STREAM) { + /* next phase is measurement */ + _collect_phase = false; } - /* next phase is measurement */ - _collect_phase = false; - } + } else { + /* measurement phase */ - /* measurement phase */ + if (measure() != PX4_OK) { + PX4_DEBUG("measure error"); + } - if (OK != measure()) { - PX4_DEBUG("measure error"); + /* next phase is collection */ + _collect_phase = true; } - /* next phase is collection */ - _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(_interval); - } void SF45LaserSerial::print_info() @@ -400,7 +307,7 @@ void SF45LaserSerial::print_info() perf_print_counter(_comms_errors); } -void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf) +void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id) { // SF45 protocol // Start byte is 0xAA and is the start of packet @@ -410,172 +317,178 @@ void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf) // ID byte precedes the data in the payload // CRC comprised of 16-bit checksum (not included in checksum calc.) - uint16_t recv_crc = 0; - bool restart_flag = false; + int ret; + size_t max_read = sizeof(_linebuf) - _linebuf_size; + ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); - while (restart_flag != true) { + if (ret < 0 && errno != EAGAIN) { + PX4_ERR("ERROR (ack from streaming distance data): %d", ret); + _linebuf_size = 0; + perf_count(_comms_errors); + perf_end(_sample_perf); + return; + } - switch (_parsed_state) { - case 0: { - if (input_buf[0] == 0xAA) { - // start of frame is valid, continue - _sop_valid = true; - _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); - _parsed_state = 1; - break; + if (ret > 0) { + _last_received_time = hrt_absolute_time(); + _linebuf_size += ret; + } - } else { - _sop_valid = false; - _crc_valid = false; - _parsed_state = 0; - restart_flag = true; - _calc_crc = 0; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("Start of packet not valid: %d", _sensor_state); - _consecutive_fail_count++; - break; - } // end else - } // end case 0 + // Not enough data to parse a complete packet. Gather more data in the next cycle. + if (_linebuf_size < payload_length) { + return; + } - case 1: { - rx_field.flags_lo = input_buf[1]; - _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); - _parsed_state = 2; - break; - } + int index = 0; - case 2: { - rx_field.flags_hi = input_buf[2]; - rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); - _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); - - // Check payload length against known max value - if (rx_field.data_len > 17) { - _parsed_state = 0; - restart_flag = true; - _calc_crc = 0; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("Payload length error: %d", _sensor_state); - _consecutive_fail_count++; - break; + while (index <= _linebuf_size - payload_length && _crc_valid == false) { + bool restart_flag = false; - } else { - _parsed_state = 3; - break; - } - } - - case 3: { + while (restart_flag != true) { + switch (_parsed_state) { + case 0: { + if (_linebuf[index] == 0xAA) { + // start of frame is valid, continue + _sop_valid = true; + _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); + _parsed_state = 1; + break; - rx_field.msg_id = input_buf[3]; + } else { + _sop_valid = false; + _crc_valid = false; + _parsed_state = 0; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Start of packet not valid: %d", _sensor_state); + break; + } // end else + } // end case 0 - if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT - || rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) { + case 1: { + rx_field.flags_lo = _linebuf[index + 1]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); + _parsed_state = 2; + break; + } - if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) { - _sensor_ready = true; + case 2: { + rx_field.flags_hi = _linebuf[index + 2]; + rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); + + // Check payload length against known max value + if (rx_field.data_len > 17) { + _parsed_state = 0; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Payload length error: %d", _sensor_state); + break; } else { - _sensor_ready = false; + _parsed_state = 3; + break; } - - _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); - - _parsed_state = 4; - break; } - // Ignore message ID's that aren't defined - else { - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - perf_count(_comms_errors); - perf_end(_sample_perf); - _consecutive_fail_count++; - PX4_DEBUG("Unknown message ID: %d", _sensor_state); - break; + case 3: { + rx_field.msg_id = _linebuf[index + 3]; + if (rx_field.msg_id == msg_id) { + _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); + _parsed_state = 4; + break; + } + + // Ignore message ID's that aren't searched + else { + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + PX4_DEBUG("Non needed message ID: %d", _sensor_state); + break; + } } - } - // Data - case 4: { - // Process commands with & w/out data bytes - if (rx_field.data_len > 1) { - for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { + // Data + case 4: { + // Process commands with & w/out data bytes + if (rx_field.data_len > 1) { + for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { - rx_field.data[_data_bytes_recv] = input_buf[i]; - _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); - _data_bytes_recv = _data_bytes_recv + 1; + rx_field.data[_data_bytes_recv] = _linebuf[index + i]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); + _data_bytes_recv = _data_bytes_recv + 1; - } // end for - } //end if + } // end for + } //end if - else { + else { + + _parsed_state = 5; + _data_bytes_recv = 0; + _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); + } _parsed_state = 5; _data_bytes_recv = 0; - _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); - + break; } - _parsed_state = 5; - _data_bytes_recv = 0; - break; - } - - // CRC low byte - case 5: { - rx_field.crc[0] = input_buf[3 + rx_field.data_len]; - _parsed_state = 6; - break; - } - - // CRC high byte - case 6: { - rx_field.crc[1] = input_buf[4 + rx_field.data_len]; - recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; - - // Check the received crc bytes from the sf45 against our own CRC calcuation - // If it matches, we can check if sensor ready - // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete - if (recv_crc == _calc_crc) { - _crc_valid = true; + // CRC low byte + case 5: { + rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len]; + _parsed_state = 6; + break; + } - // Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM - if (_sensor_ready) { - _init_complete = true; + // CRC high byte + case 6: { + rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len]; + uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; + + // Check the received crc bytes from the sf45 against our own CRC calcuation + // If it matches, we can check if sensor ready + // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete + if (recv_crc == _calc_crc) { + _crc_valid = true; + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + break; } else { - _init_complete = false; + + _crc_valid = false; + _parsed_state = 0; + _calc_crc = 0; + restart_flag = true; + perf_count(_comms_errors); + PX4_DEBUG("CRC mismatch: %d", _sensor_state); + break; } + } + } // end switch + } //end while - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - break; + index++; + } - } else { + // If we parsed successfully, remove the parsed part from the buffer if it is still large enough + if (_crc_valid && index + payload_length < _linebuf_size) { + unsigned next_after_index = index + payload_length; + memmove(&_linebuf[0], &_linebuf[next_after_index], _linebuf_size - next_after_index); + _linebuf_size -= next_after_index; + } - _crc_valid = false; - _init_complete = false; - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("CRC mismatch: %d", _sensor_state); - break; - } - } - } // end switch - } //end while + // The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again. + if ((unsigned)_linebuf_size >= sizeof(_linebuf)) { + _linebuf_size = 0; + perf_count(_comms_errors); + } } -void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len) +void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len) { uint16_t crc_val = 0; uint8_t packet_buff[SF45_MAX_PAYLOAD]; @@ -612,7 +525,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d if (msg_id == SF_DISTANCE_OUTPUT) { uint8_t data_convert = data[0] & 0x00FF; // write data bytes to the output buffer - packet_buff[data_inc] = data_convert; + packet_buff[data_inc] = data_convert; // Add data bytes to crc add function crc_val = sf45_format_crc(crc_val, data_convert); data_inc = data_inc + 1; @@ -650,12 +563,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d // Add data bytes to crc add function crc_val = sf45_format_crc(crc_val, data[0]); data_inc = data_inc + 1; - } else { // Product Name - PX4_INFO("INFO: Product name"); + PX4_DEBUG("DEBUG: Product name"); } uint8_t crc_lo = crc_val & 0xFF; @@ -694,7 +606,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float if (raw_yaw > 32000) { raw_yaw = raw_yaw - 65535; - } // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle @@ -802,7 +713,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { - uint8_t mapped_sector = 0; float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset); mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment); diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index 4f75b3285201..de7af5468485 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -56,9 +56,9 @@ enum SF_SERIAL_STATE { STATE_UNINIT = 0, - STATE_SEND_PRODUCT_NAME = 1, - STATE_SEND_UPDATE_RATE = 2, - STATE_SEND_DISTANCE_DATA = 3, + STATE_ACK_PRODUCT_NAME = 1, + STATE_ACK_UPDATE_RATE = 2, + STATE_ACK_DISTANCE_OUTPUT = 3, STATE_SEND_STREAM = 4, }; @@ -80,8 +80,8 @@ class SF45LaserSerial : public px4::ScheduledWorkItem int init(); void print_info(); - void sf45_request_handle(uint8_t *value); - void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); + void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id); + void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); void sf45_process_replies(float *data); uint8_t sf45_convert_angle(const int16_t yaw); @@ -104,30 +104,26 @@ class SF45LaserSerial : public px4::ScheduledWorkItem char _port[20] {}; - int _interval{10000}; + int _interval{2000}; bool _collect_phase{false}; int _fd{-1}; uint8_t _linebuf[SF45_MAX_PAYLOAD] {}; - unsigned _linebuf_size{0}; - hrt_abstime _last_read{0}; + int _linebuf_size{0}; // SF45/B uses a binary protocol to include header,flags // message ID, payload, and checksum bool _is_sf45{false}; - bool _init_complete{false}; - bool _sensor_ready{false}; - uint8_t _sensor_state{0}; + SF_SERIAL_STATE _sensor_state{STATE_UNINIT}; int _baud_rate{0}; - int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int _stream_data{0}; - int32_t _update_rate{1}; - int _data_output{0}; + int32_t _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + int32_t _stream_data{0}; + int32_t _update_rate{0}; + int32_t _data_output{0}; const uint8_t _start_of_frame{0xAA}; uint16_t _data_bytes_recv{0}; uint8_t _parsed_state{0}; bool _sop_valid{false}; uint16_t _calc_crc{0}; - uint8_t _num_retries{0}; int32_t _yaw_cfg{0}; int32_t _orient_cfg{0}; uint8_t _previous_bin{0}; @@ -135,9 +131,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem // end of SF45/B data members - unsigned _consecutive_fail_count; - + hrt_abstime _last_received_time{0}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - }; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml index 4dbdf25c9083..36eddddfa0d1 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -32,7 +32,7 @@ parameters: 12: 5000hz reboot_required: true num_instances: 1 - default: 1 + default: 5 SF45_ORIENT_CFG: description: From 051ced0fee306d61971055f7355181bdf7ea2d1b Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 4 Dec 2024 15:08:25 +0100 Subject: [PATCH 155/211] SENS: RNG: SF45: Added timeout to sensor measurements, to compensate the lower loop time of CollisionPrevention --- .../lightware_sf45_serial.cpp | 174 ++++++++++-------- .../lightware_sf45_serial.hpp | 19 +- .../lightware_sf45_serial_main.cpp | 1 - 3 files changed, 111 insertions(+), 83 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index b3ac5db49654..a8ed20383d11 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -44,7 +44,7 @@ using namespace time_literals; /* Configuration Constants */ -#define SF45_SCALE_FACTOR 0.01f + SF45LaserSerial::SF45LaserSerial(const char *port) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), @@ -67,15 +67,15 @@ SF45LaserSerial::SF45LaserSerial(const char *port) : } // populate obstacle map members - _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - _obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; - _obstacle_map_msg.increment = 5; - _obstacle_map_msg.min_distance = 20; - _obstacle_map_msg.max_distance = 5000; - _obstacle_map_msg.angle_offset = 0; + _obstacle_distance.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + _obstacle_distance.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; + _obstacle_distance.increment = 5; + _obstacle_distance.min_distance = 20; + _obstacle_distance.max_distance = 5000; + _obstacle_distance.angle_offset = 0; for (uint32_t i = 0 ; i < BIN_COUNT; i++) { - _obstacle_map_msg.distances[i] = UINT16_MAX; + _obstacle_distance.distances[i] = UINT16_MAX; } } @@ -346,63 +346,63 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons while (restart_flag != true) { switch (_parsed_state) { - case 0: { + case SF45_PARSED_STATE::START: { if (_linebuf[index] == 0xAA) { // start of frame is valid, continue _sop_valid = true; _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); - _parsed_state = 1; + _parsed_state = SF45_PARSED_STATE::FLG_LOW; break; } else { _sop_valid = false; _crc_valid = false; - _parsed_state = 0; + _parsed_state = SF45_PARSED_STATE::START; restart_flag = true; _calc_crc = 0; PX4_DEBUG("Start of packet not valid: %d", _sensor_state); break; - } // end else - } // end case 0 + } + } - case 1: { + case SF45_PARSED_STATE::FLG_LOW: { rx_field.flags_lo = _linebuf[index + 1]; _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); - _parsed_state = 2; + _parsed_state = SF45_PARSED_STATE::FLG_HIGH; break; } - case 2: { + case SF45_PARSED_STATE::FLG_HIGH: { rx_field.flags_hi = _linebuf[index + 2]; rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); // Check payload length against known max value if (rx_field.data_len > 17) { - _parsed_state = 0; + _parsed_state = SF45_PARSED_STATE::START; restart_flag = true; _calc_crc = 0; PX4_DEBUG("Payload length error: %d", _sensor_state); break; } else { - _parsed_state = 3; + _parsed_state = SF45_PARSED_STATE::ID; break; } } - case 3: { + case SF45_PARSED_STATE::ID: { rx_field.msg_id = _linebuf[index + 3]; if (rx_field.msg_id == msg_id) { _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); - _parsed_state = 4; + _parsed_state = SF45_PARSED_STATE::DATA; break; } // Ignore message ID's that aren't searched else { - _parsed_state = 0; + _parsed_state = SF45_PARSED_STATE::START; _calc_crc = 0; restart_flag = true; PX4_DEBUG("Non needed message ID: %d", _sensor_state); @@ -410,8 +410,7 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons } } - // Data - case 4: { + case SF45_PARSED_STATE::DATA: { // Process commands with & w/out data bytes if (rx_field.data_len > 1) { for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { @@ -420,30 +419,28 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); _data_bytes_recv = _data_bytes_recv + 1; - } // end for - } //end if + } + } else { - _parsed_state = 5; + _parsed_state = SF45_PARSED_STATE::CRC_LOW; _data_bytes_recv = 0; _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); } - _parsed_state = 5; + _parsed_state = SF45_PARSED_STATE::CRC_LOW; _data_bytes_recv = 0; break; } - // CRC low byte - case 5: { + case SF45_PARSED_STATE::CRC_LOW: { rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len]; - _parsed_state = 6; + _parsed_state = SF45_PARSED_STATE::CRC_HIGH; break; } - // CRC high byte - case 6: { + case SF45_PARSED_STATE::CRC_HIGH: { rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len]; uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; @@ -452,7 +449,7 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete if (recv_crc == _calc_crc) { _crc_valid = true; - _parsed_state = 0; + _parsed_state = SF45_PARSED_STATE::START; _calc_crc = 0; restart_flag = true; break; @@ -460,7 +457,7 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons } else { _crc_valid = false; - _parsed_state = 0; + _parsed_state = SF45_PARSED_STATE::START; _calc_crc = 0; restart_flag = true; perf_count(_comms_errors); @@ -468,8 +465,8 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons break; } } - } // end switch - } //end while + } + } index++; } @@ -652,52 +649,16 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin, (double)*distance_m); - if (_current_bin_dist > _obstacle_map_msg.max_distance) { - _current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition + if (_current_bin_dist > _obstacle_distance.max_distance) { + _current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition } - // if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement. - // in this case we assume the measurement to be valid for all bins between the previous and the current bin. win - uint8_t start; - uint8_t end; - - if (abs(current_bin - _previous_bin) > BIN_COUNT / - 4) { // wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins - // TODO: differentiate direction of wrap-around, currently it overwrites a previous measurement. - start = math::max(_previous_bin, current_bin); - end = math::min(_previous_bin, current_bin); + hrt_abstime now = hrt_absolute_time(); + _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); - } else if (_previous_bin < current_bin) { // Scanning clockwise - start = _previous_bin + 1; - end = current_bin; - - } else { // scanning counter-clockwise - start = current_bin; - end = _previous_bin - 1; - } - - if (start <= end) { - for (uint8_t i = start; i <= end; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;} - - } else { // wrap-around case - for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;} - - for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;} - } - - _obstacle_map_msg.timestamp = hrt_absolute_time(); - _obstacle_distance_pub.publish(_obstacle_map_msg); + _publish_obstacle_msg(now); // reset the values for the next measurement - if (start <= end) { - for (uint8_t i = start; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;} - - } else { // wrap-around case - for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;} - - for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;} - } - _current_bin_dist = UINT16_MAX; _previous_bin = current_bin; } @@ -710,12 +671,67 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) break; } } +void SF45LaserSerial::_publish_obstacle_msg(hrt_abstime now) +{ + // This whole logic is in place because CollisionPrevention, just reads in the last published message on the topic, and not every message, + // This can result in messages being misssed if the sensor is publishing at a faster rate than the CollisionPrevention module is reading. + for (uint8_t i = 0; i < BIN_COUNT; i++) { + if (now - _data_timestamps[i] > SF45_MEAS_TIMEOUT) { + // If the data is older than SF45_MEAS_TIMEOUT, we discard the value (UINT16_MAX means no valid data) + _obstacle_distance.distances[i] = UINT16_MAX; + } + } + + _obstacle_distance.timestamp = now; + _obstacle_distance_pub.publish(_obstacle_distance); +} + +void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, + hrt_abstime now) +{ + // if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement. + // in this case we assume the measurement to be valid for all bins between the previous and the current bin. + uint8_t start; + uint8_t end; + + if (abs(current_bin - previous_bin) > BIN_COUNT / 4) { + // wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins + // THis is simplyfied as we are not considering the scaning direction + start = math::max(previous_bin, current_bin); + end = math::min(previous_bin, current_bin); + + } else if (previous_bin < current_bin) { // Scanning clockwise + start = previous_bin + 1; + end = current_bin; + + } else { // scanning counter-clockwise + start = current_bin; + end = previous_bin - 1; + } + + if (start <= end) { + for (uint8_t i = start; i <= end; i++) { + _obstacle_distance.distances[i] = measurement; + _data_timestamps[i] = now; + } + + } else { // wrap-around case + for (uint8_t i = start; i < BIN_COUNT; i++) { + _obstacle_distance.distances[i] = measurement; + _data_timestamps[i] = now; + } + for (uint8_t i = 0; i <= end; i++) { + _obstacle_distance.distances[i] = measurement; + _data_timestamps[i] = now; + } + } +} uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { uint8_t mapped_sector = 0; - float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset); - mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment); + float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset); + mapped_sector = round(adjusted_yaw / _obstacle_distance.increment); return mapped_sector; } diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index de7af5468485..c5cae5e049df 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -50,7 +50,6 @@ #include #include -#include #include "sf45_commands.h" @@ -62,6 +61,15 @@ enum SF_SERIAL_STATE { STATE_SEND_STREAM = 4, }; +enum SF45_PARSED_STATE { + START = 0, + FLG_LOW, + FLG_HIGH, + ID, + DATA, + CRC_LOW, + CRC_HIGH +}; enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE @@ -71,6 +79,7 @@ enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTA ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90 ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270 }; + using namespace time_literals; class SF45LaserSerial : public px4::ScheduledWorkItem { @@ -88,9 +97,12 @@ class SF45LaserSerial : public px4::ScheduledWorkItem float sf45_wrap_360(float f); private: - obstacle_distance_s _obstacle_map_msg{}; + obstacle_distance_s _obstacle_distance{}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ - static constexpr int BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); + static constexpr uint8_t BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof( + obstacle_distance_s::distances[0]); + static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms}; + static constexpr float SF45_SCALE_FACTOR = 0.01f; void start(); void stop(); @@ -99,6 +111,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem int collect(); bool _crc_valid{false}; + void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now); void _publish_obstacle_msg(hrt_abstime now); uint64_t _data_timestamps[BIN_COUNT]; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp index 3446ad1a359f..4f6c0a814b61 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp @@ -102,7 +102,6 @@ static int usage() Serial bus driver for the Lightware SF45/b Laser rangefinder. -Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html ### Examples From 93c25efb62ebb0ed44b0f259fa1508aab081b2a1 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 16 Dec 2024 16:16:25 +0300 Subject: [PATCH 156/211] Fixed-wing Position Control: Split up control methods for VTOL backtransition (#23893) * split methods to control backtransition depening on availablity of position Signed-off-by: RomanBapst * fixed sign error and replace hardcoded number with constant Signed-off-by: RomanBapst * make changes such that controller holds initial heading during transition Signed-off-by: RomanBapst * use reference instead of copy Signed-off-by: RomanBapst * added comment Signed-off-by: RomanBapst * flash reduction Signed-off-by: RomanBapst --------- Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 85 ++++++++++++------- .../FixedwingPositionControl.hpp | 15 +++- .../fw_path_navigation_params.c | 29 +++---- 3 files changed, 75 insertions(+), 54 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index bdf5a868325f..a15f1e8f7154 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -714,7 +714,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. if (doing_backtransition) { - _control_mode_current = FW_POSCTRL_MODE_TRANSITON; + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW; } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -766,7 +766,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) if (doing_backtransition) { // we handle loss of position control during backtransition as a special case - _control_mode_current = FW_POSCTRL_MODE_TRANSITON; + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; } else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) && !_vehicle_status.in_transition_mode) { @@ -2356,18 +2356,49 @@ FixedwingPositionControl::control_manual_position(const float control_interval, attitude_setpoint.copyTo(_att_sp.q_d); } -void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_curr) +void FixedwingPositionControl::control_backtransition_heading_hold() { - const bool is_low_height = checkLowHeightConditions(); + if (!PX4_ISFINITE(_backtrans_heading)) { + _backtrans_heading = _local_pos.heading; + } - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + float true_airspeed = _airspeed_eas * _eas2tas; + + if (!_airspeed_valid) { + true_airspeed = _performance_model.getCalibratedTrimAirspeed() * _eas2tas; + } + + // we can achieve heading control by setting airspeed and groundspeed vector equal + const Vector2f airspeed_vector = Vector2f(cosf(_local_pos.heading), sinf(_local_pos.heading)) * true_airspeed; + const Vector2f &ground_speed = airspeed_vector; + + _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + + Vector2f virtual_target_point = Vector2f(cosf(_backtrans_heading), sinf(_backtrans_heading)) * HDG_HOLD_DIST_NEXT; + navigateLine(Vector2f(0.f, 0.f), virtual_target_point, Vector2f(0.f, 0.f), ground_speed, Vector2f(0.f, 0.f)); + + const float roll_body = getCorrectedNpfgRollSetpoint(); + + const float yaw_body = _backtrans_heading; + + // these values are overriden by transition logic + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + const float pitch_body = 0.0f; + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + +} + +void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr) +{ Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); // Set the position where the backtransition started the first ime we pass through here. @@ -2376,30 +2407,14 @@ void FixedwingPositionControl::control_backtransition(const float control_interv _lpos_where_backtrans_started = curr_pos_local; } - float roll_body{0.0f}; - - if (_control_mode.flag_control_position_enabled) { - navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - roll_body = getCorrectedNpfgRollSetpoint(); - } - - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + const float roll_body = getCorrectedNpfgRollSetpoint(); - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); + const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - const float pitch_body = get_tecs_pitch(); + // these values are overriden by transition logic + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + const float pitch_body = 0.0f; const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); attitude_setpoint.copyTo(_att_sp.q_d); @@ -2607,6 +2622,7 @@ FixedwingPositionControl::Run() if (!_vehicle_status.in_transition_mode) { // reset position of backtransition start if not in transition _lpos_where_backtrans_started = Vector2f(NAN, NAN); + _backtrans_heading = NAN; } } @@ -2703,8 +2719,13 @@ FixedwingPositionControl::Run() break; } - case FW_POSCTRL_MODE_TRANSITON: { - control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current); + case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW: { + control_backtransition_line_follow(ground_speed, _pos_sp_triplet.current); + break; + } + + case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD: { + control_backtransition_heading_hold(); break; } } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5600e71fcdc1..e56263c5e31e 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -260,7 +260,8 @@ class FixedwingPositionControl final : public ModuleBase Altitude priority + * Speed <--> Altitude weight * * Adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Set to 2 for gliders. + * applies to speed vs height errors. + * 0 -> control height only + * 2 -> control speed only (gliders) * * @min 0.0 * @max 2.0 From fd87cd682db5360de4408aefa1b93bad1eaa1d3d Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Tue, 17 Dec 2024 17:57:18 +0100 Subject: [PATCH 157/211] Dynamically update ff gain for fw rate control (#24120) --- .../fw_rate_control/FixedwingRateControl.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 1f2cff00d6cd..9841ea8ca6f8 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -84,10 +84,6 @@ FixedwingRateControl::parameters_update() _rate_control.setIntegratorLimit( Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); - _rate_control.setFeedForwardGain( - // set FF gains to 0 as we add the FF control outside of the rate controller - Vector3f(0.f, 0.f, 0.f)); - if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) { param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en); } @@ -354,14 +350,15 @@ void FixedwingRateControl::Run() body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); } + const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); + const Vector3f scaled_gain_ff = gain_ff / _airspeed_scaling; + _rate_control.setFeedForwardGain(scaled_gain_ff); + // Run attitude RATE controllers which need the desired attitudes from above, add trim. const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed); - const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); - const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; - - Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward; + Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling; // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) { From 8a18e5b00c64db51444931550f0b71383f803883 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Dec 2024 14:24:07 -0500 Subject: [PATCH 158/211] commander: accelerometer calibration respect rotation - accel cal use Accelerometer calibration class to fully respect rotation (both internal and external sensors) --- .../commander/accelerometer_calibration.cpp | 85 ++++++------------- 1 file changed, 24 insertions(+), 61 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 63d04957af87..ba8bc59b3e8f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -157,22 +157,18 @@ struct accel_worker_data_s { orb_advert_t *mavlink_log_pub{nullptr}; unsigned done_count{0}; float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {}; + + calibration::Accelerometer calibration[MAX_ACCEL_SENS] {}; }; // Read specified number of accelerometer samples, calculate average and dispersion. -static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3], - unsigned orient, unsigned samples_num) +static calibrate_return read_accelerometer_avg(accel_worker_data_s *worker_data, unsigned orient, unsigned samples_num) { Vector3f accel_sum[MAX_ACCEL_SENS] {}; unsigned counts[MAX_ACCEL_SENS] {}; unsigned errcount = 0; - // sensor thermal corrections - uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; - sensor_correction_s sensor_correction{}; - sensor_correction_sub.copy(&sensor_correction); - uORB::SubscriptionBlocking accel_sub[MAX_ACCEL_SENS] { {ORB_ID(sensor_accel), 0, 0}, {ORB_ID(sensor_accel), 0, 1}, @@ -187,32 +183,9 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS sensor_accel_s arp; while (accel_sub[accel_index].update(&arp)) { - // fetch optional thermal offset corrections in sensor/board frame - Vector3f offset{0, 0, 0}; - sensor_correction_sub.update(&sensor_correction); - - if (sensor_correction.timestamp > 0 && arp.device_id != 0) { - for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) { - if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) { - switch (correction_index) { - case 0: - offset = Vector3f{sensor_correction.accel_offset_0}; - break; - case 1: - offset = Vector3f{sensor_correction.accel_offset_1}; - break; - case 2: - offset = Vector3f{sensor_correction.accel_offset_2}; - break; - case 3: - offset = Vector3f{sensor_correction.accel_offset_3}; - break; - } - } - } - } - - accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset; + // fetch optional thermal offset corrections + worker_data->calibration[accel_index].SensorCorrectionsUpdate(); + accel_sum[accel_index] += worker_data->calibration[accel_index].Correct(Vector3f(arp.x, arp.y, arp.z)); counts[accel_index]++; } @@ -228,16 +201,9 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS } } - // rotate sensor measurements from sensor to body frame using board rotation matrix - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); - - for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { - accel_sum[s] = board_rotation * accel_sum[s]; - } - for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { const Vector3f avg{accel_sum[s] / counts[s]}; - avg.copyTo(accel_avg[s][orient]); + avg.copyTo(worker_data->accel_ref[s][orient]); } return calibrate_return_ok; @@ -251,7 +217,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); - read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num); + read_accelerometer_avg(worker_data, orientation, samples_num); // check accel for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { @@ -325,18 +291,27 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) { calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; + accel_worker_data_s worker_data{}; + worker_data.mavlink_log_pub = mavlink_log_pub; + unsigned active_sensors = 0; for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) { uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel), cur_accel}; if (accel_sub.advertised() && (accel_sub.get().device_id != 0) && (accel_sub.get().timestamp > 0)) { - calibrations[cur_accel].set_device_id(accel_sub.get().device_id); + worker_data.calibration[cur_accel].set_device_id(accel_sub.get().device_id); + + // clear existing calibration + worker_data.calibration[cur_accel].Reset(); + + // force fetch optional thermal offset corrections + worker_data.calibration[cur_accel].SensorCorrectionsUpdate(true); + active_sensors++; } else { - calibrations[cur_accel].Reset(); + worker_data.calibration[cur_accel].Reset(); } } @@ -353,16 +328,11 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } /* measure and calculate offsets & scales */ - accel_worker_data_s worker_data{}; - worker_data.mavlink_log_pub = mavlink_log_pub; bool data_collected[detect_orientation_side_count] {}; if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data, false) == calibrate_return_ok) { - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); - const Dcmf board_rotation_t = board_rotation.transpose(); - bool param_save = false; bool failed = true; @@ -396,29 +366,22 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G; // update calibration - const Vector3f accel_offs_rotated{board_rotation_t *offset}; - calibrations[i].set_offset(accel_offs_rotated); - - const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation}; - calibrations[i].set_scale(accel_T_rotated.diag()); + worker_data.calibration[i].set_offset(offset); + worker_data.calibration[i].set_scale(accel_T.diag()); #if defined(DEBUD_BUILD) PX4_INFO("accel %d: offset", i); offset.print(); - PX4_INFO("accel %d: bT * offset", i); - accel_offs_rotated.print(); PX4_INFO("accel %d: mat_A", i); mat_A.print(); PX4_INFO("accel %d: accel_T", i); accel_T.print(); - PX4_INFO("accel %d: bT * accel_T * b", i); - accel_T_rotated.print(); #endif // DEBUD_BUILD - calibrations[i].PrintStatus(); + worker_data.calibration[i].PrintStatus(); - if (calibrations[i].ParametersSave(i, true)) { + if (worker_data.calibration[i].ParametersSave(i, true)) { param_save = true; failed = false; From e7145e7b44e6f982cb757e7707ff202236608755 Mon Sep 17 00:00:00 2001 From: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com> Date: Tue, 17 Dec 2024 22:28:40 -0500 Subject: [PATCH 159/211] uxrce_dds_client: include distance sensor in dds_topics.yaml Signed-off-by: dirksavage88 --- src/modules/uxrce_dds_client/dds_topics.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index e9e67cff25ed..acee6afe8d47 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -97,6 +97,9 @@ subscriptions: - topic: /fmu/in/config_control_setpoints type: px4_msgs::msg::VehicleControlMode + - topic: /fmu/in/distance_sensor + type: px4_msgs::msg::DistanceSensor + - topic: /fmu/in/manual_control_input type: px4_msgs::msg::ManualControlSetpoint From 6969e5b6b4d89888a50a294beeea14fa2a9b617b Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 16 Dec 2024 15:23:06 +0100 Subject: [PATCH 160/211] ekf2: do not pre-compute airspeed Kalman gain The generated code is not much faster than the simple matrix-vector multiplication --- .../aid_sources/airspeed/airspeed_fusion.cpp | 8 +- .../EKF/python/ekf_derivation/derivation.py | 10 +- .../generated/compute_airspeed_h.h | 56 +++++++++ .../generated/compute_airspeed_h_and_k.h | 116 ------------------ 4 files changed, 62 insertions(+), 128 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 51f6fc8f707f..14873831fcee 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -44,7 +44,7 @@ #include "ekf.h" -#include +#include #include #include @@ -204,10 +204,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour _fault_status.flags.bad_airspeed = false; - VectorState H; // Observation jacobian - VectorState K; // Kalman gain vector - - sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K); + const VectorState H = sym::ComputeAirspeedH(_state.vector(), FLT_EPSILON); + VectorState K = P * H / aid_src.innovation_variance; if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 86651f9d656c..0ad3fef4ed0e 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -268,10 +268,8 @@ def compute_airspeed_innov_and_innov_var( return (innov, innov_var) -def compute_airspeed_h_and_k( +def compute_airspeed_h( state: VState, - P: MTangent, - innov_var: sf.Scalar, epsilon: sf.Scalar ) -> (VTangent, VTangent): @@ -281,9 +279,7 @@ def compute_airspeed_h_and_k( airspeed_pred = vel_rel.norm(epsilon=epsilon) H = jacobian_chain_rule(airspeed_pred, state) - K = P * H.T / sf.Max(innov_var, epsilon) - - return (H.T, K) + return H.T def compute_wind_init_and_cov_from_airspeed( v_local: sf.V3, @@ -739,7 +735,7 @@ def compute_gravity_z_innov_var_and_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_h, output_names=None) generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"]) generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h new file mode 100644 index 000000000000..999c01d1eebc --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h @@ -0,0 +1,56 @@ +// ----------------------------------------------------------------------------- +// 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_airspeed_h + * + * Args: + * state: Matrix25_1 + * epsilon: Scalar + * + * Outputs: + * res: Matrix24_1 + */ +template +matrix::Matrix ComputeAirspeedH(const matrix::Matrix& state, + const Scalar epsilon) { + // Total ops: 14 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = -state(22, 0) + state(4, 0); + const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + + epsilon + std::pow(state(6, 0), Scalar(2))), + Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp3 = _tmp1 * _tmp2; + const Scalar _tmp4 = _tmp0 * _tmp2; + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(3, 0) = _tmp3; + _res(4, 0) = _tmp4; + _res(5, 0) = _tmp2 * state(6, 0); + _res(21, 0) = -_tmp3; + _res(22, 0) = -_tmp4; + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h deleted file mode 100644 index 52bb16353d36..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h +++ /dev/null @@ -1,116 +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: compute_airspeed_h_and_k - * - * Args: - * state: Matrix25_1 - * P: Matrix24_24 - * innov_var: Scalar - * epsilon: Scalar - * - * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 - */ -template -void ComputeAirspeedHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 256 - - // Input arrays - - // Intermediate terms (7) - const Scalar _tmp0 = -state(23, 0) + state(5, 0); - const Scalar _tmp1 = -state(22, 0) + state(4, 0); - const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + - epsilon + std::pow(state(6, 0), Scalar(2))), - Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp3 = _tmp1 * _tmp2; - const Scalar _tmp4 = _tmp0 * _tmp2; - const Scalar _tmp5 = _tmp2 * state(6, 0); - const Scalar _tmp6 = Scalar(1.0) / (math::max(epsilon, innov_var)); - - // Output terms (2) - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(3, 0) = _tmp3; - _h(4, 0) = _tmp4; - _h(5, 0) = _tmp5; - _h(21, 0) = -_tmp3; - _h(22, 0) = -_tmp4; - } - - if (K != nullptr) { - matrix::Matrix& _k = (*K); - - _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 + - P(0, 5) * _tmp5); - _k(1, 0) = _tmp6 * (-P(1, 21) * _tmp3 - P(1, 22) * _tmp4 + P(1, 3) * _tmp3 + P(1, 4) * _tmp4 + - P(1, 5) * _tmp5); - _k(2, 0) = _tmp6 * (-P(2, 21) * _tmp3 - P(2, 22) * _tmp4 + P(2, 3) * _tmp3 + P(2, 4) * _tmp4 + - P(2, 5) * _tmp5); - _k(3, 0) = _tmp6 * (-P(3, 21) * _tmp3 - P(3, 22) * _tmp4 + P(3, 3) * _tmp3 + P(3, 4) * _tmp4 + - P(3, 5) * _tmp5); - _k(4, 0) = _tmp6 * (-P(4, 21) * _tmp3 - P(4, 22) * _tmp4 + P(4, 3) * _tmp3 + P(4, 4) * _tmp4 + - P(4, 5) * _tmp5); - _k(5, 0) = _tmp6 * (-P(5, 21) * _tmp3 - P(5, 22) * _tmp4 + P(5, 3) * _tmp3 + P(5, 4) * _tmp4 + - P(5, 5) * _tmp5); - _k(6, 0) = _tmp6 * (-P(6, 21) * _tmp3 - P(6, 22) * _tmp4 + P(6, 3) * _tmp3 + P(6, 4) * _tmp4 + - P(6, 5) * _tmp5); - _k(7, 0) = _tmp6 * (-P(7, 21) * _tmp3 - P(7, 22) * _tmp4 + P(7, 3) * _tmp3 + P(7, 4) * _tmp4 + - P(7, 5) * _tmp5); - _k(8, 0) = _tmp6 * (-P(8, 21) * _tmp3 - P(8, 22) * _tmp4 + P(8, 3) * _tmp3 + P(8, 4) * _tmp4 + - P(8, 5) * _tmp5); - _k(9, 0) = _tmp6 * (-P(9, 21) * _tmp3 - P(9, 22) * _tmp4 + P(9, 3) * _tmp3 + P(9, 4) * _tmp4 + - P(9, 5) * _tmp5); - _k(10, 0) = _tmp6 * (-P(10, 21) * _tmp3 - P(10, 22) * _tmp4 + P(10, 3) * _tmp3 + - P(10, 4) * _tmp4 + P(10, 5) * _tmp5); - _k(11, 0) = _tmp6 * (-P(11, 21) * _tmp3 - P(11, 22) * _tmp4 + P(11, 3) * _tmp3 + - P(11, 4) * _tmp4 + P(11, 5) * _tmp5); - _k(12, 0) = _tmp6 * (-P(12, 21) * _tmp3 - P(12, 22) * _tmp4 + P(12, 3) * _tmp3 + - P(12, 4) * _tmp4 + P(12, 5) * _tmp5); - _k(13, 0) = _tmp6 * (-P(13, 21) * _tmp3 - P(13, 22) * _tmp4 + P(13, 3) * _tmp3 + - P(13, 4) * _tmp4 + P(13, 5) * _tmp5); - _k(14, 0) = _tmp6 * (-P(14, 21) * _tmp3 - P(14, 22) * _tmp4 + P(14, 3) * _tmp3 + - P(14, 4) * _tmp4 + P(14, 5) * _tmp5); - _k(15, 0) = _tmp6 * (-P(15, 21) * _tmp3 - P(15, 22) * _tmp4 + P(15, 3) * _tmp3 + - P(15, 4) * _tmp4 + P(15, 5) * _tmp5); - _k(16, 0) = _tmp6 * (-P(16, 21) * _tmp3 - P(16, 22) * _tmp4 + P(16, 3) * _tmp3 + - P(16, 4) * _tmp4 + P(16, 5) * _tmp5); - _k(17, 0) = _tmp6 * (-P(17, 21) * _tmp3 - P(17, 22) * _tmp4 + P(17, 3) * _tmp3 + - P(17, 4) * _tmp4 + P(17, 5) * _tmp5); - _k(18, 0) = _tmp6 * (-P(18, 21) * _tmp3 - P(18, 22) * _tmp4 + P(18, 3) * _tmp3 + - P(18, 4) * _tmp4 + P(18, 5) * _tmp5); - _k(19, 0) = _tmp6 * (-P(19, 21) * _tmp3 - P(19, 22) * _tmp4 + P(19, 3) * _tmp3 + - P(19, 4) * _tmp4 + P(19, 5) * _tmp5); - _k(20, 0) = _tmp6 * (-P(20, 21) * _tmp3 - P(20, 22) * _tmp4 + P(20, 3) * _tmp3 + - P(20, 4) * _tmp4 + P(20, 5) * _tmp5); - _k(21, 0) = _tmp6 * (-P(21, 21) * _tmp3 - P(21, 22) * _tmp4 + P(21, 3) * _tmp3 + - P(21, 4) * _tmp4 + P(21, 5) * _tmp5); - _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 + - P(22, 4) * _tmp4 + P(22, 5) * _tmp5); - _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 + - P(23, 4) * _tmp4 + P(23, 5) * _tmp5); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym From 90b968ba4ea303f93835f60ec7e2f7ce409650c5 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 11 Dec 2024 14:26:23 -0500 Subject: [PATCH 161/211] container: remove pr container push to registry --- .github/workflows/dev_container.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index 6e5c1cad32ee..bcfbdc2c6b18 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -58,7 +58,6 @@ jobs: type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700 type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600 type=ref,event=branch,suffix=,priority=500 - type=ref,event=pr - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 From aca5a709645b27b2eb9a609f0c959504b6de21eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 21 Nov 2024 11:05:56 +0100 Subject: [PATCH 162/211] standard_modes: add vehicle-type specific standard modes See https://mavlink.io/en/messages/development.html#MAV_STANDARD_MODE. The only standard mode that is not set is MAV_STANDARD_MODE_SAFE_RECOVERY, as PX4 uses RTL for that (with configuration parameters). --- src/lib/modes/standard_modes.hpp | 65 ++++++++++++++++++- src/modules/commander/Commander.cpp | 3 +- .../mavlink/streams/AVAILABLE_MODES.hpp | 3 +- src/modules/mavlink/streams/CURRENT_MODE.hpp | 3 +- 4 files changed, 68 insertions(+), 6 deletions(-) diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp index df27f927350e..68d849f09a62 100644 --- a/src/lib/modes/standard_modes.hpp +++ b/src/lib/modes/standard_modes.hpp @@ -57,7 +57,7 @@ enum class StandardMode : uint8_t { /** * @return Get MAVLink standard mode from nav_state */ -static inline StandardMode getStandardModeFromNavState(uint8_t nav_state) +static inline StandardMode getStandardModeFromNavState(uint8_t nav_state, uint8_t vehicle_type, bool is_vtol) { switch (nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME; @@ -67,7 +67,33 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF; - // Note: all other standard modes do not directly map, or are vehicle-type specific + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return StandardMode::ALTITUDE_HOLD; + } + + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return StandardMode::POSITION_HOLD; + } + + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return StandardMode::CRUISE; + } + + break; + + case vehicle_status_s::NAVIGATION_STATE_ORBIT: + if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return StandardMode::ORBIT; + } + + break; } return StandardMode::NON_STANDARD; @@ -76,7 +102,7 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state) /** * @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported */ -static inline uint8_t getNavStateFromStandardMode(StandardMode mode) +static inline uint8_t getNavStateFromStandardMode(StandardMode mode, uint8_t vehicle_type, bool is_vtol) { switch (mode) { case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; @@ -87,6 +113,39 @@ static inline uint8_t getNavStateFromStandardMode(StandardMode mode) case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + case StandardMode::ALTITUDE_HOLD: + if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return vehicle_status_s::NAVIGATION_STATE_ALTCTL; + } + + break; + + case StandardMode::POSITION_HOLD: + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + + break; + + case StandardMode::CRUISE: + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + + break; + + case StandardMode::ORBIT: + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return vehicle_status_s::NAVIGATION_STATE_ORBIT; + } + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + + break; + default: break; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8e41b2ac56df..5fc8ee4d8f3c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1450,7 +1450,8 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: { mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1); - uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode); + uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode, _vehicle_status.vehicle_type, + _vehicle_status.is_vtol); if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) { answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); diff --git a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp index d19aeb46139d..ae077c823991 100644 --- a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp +++ b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp @@ -86,7 +86,8 @@ class MavlinkStreamAvailableModes : public MavlinkStream const bool cannot_be_selected = (vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0; // Set the mode name if not a standard mode - available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state); + available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state, vehicle_status.vehicle_type, + vehicle_status.is_vtol); if (mode_util::isAdvanced(nav_state)) { available_modes.properties |= MAV_MODE_PROPERTY_ADVANCED; diff --git a/src/modules/mavlink/streams/CURRENT_MODE.hpp b/src/modules/mavlink/streams/CURRENT_MODE.hpp index 5527801670e4..2be1c47c4b8d 100644 --- a/src/modules/mavlink/streams/CURRENT_MODE.hpp +++ b/src/modules/mavlink/streams/CURRENT_MODE.hpp @@ -66,7 +66,8 @@ class MavlinkStreamCurrentMode : public MavlinkStream mavlink_current_mode_t current_mode{}; current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data; current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data; - current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state); + current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state, + vehicle_status.vehicle_type, vehicle_status.is_vtol); mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), ¤t_mode); return true; } From 9e9d352eb20a85effdc513cb0ef9f27c832ffe06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 12 Dec 2024 09:41:07 +0100 Subject: [PATCH 163/211] mavlink: update submodule MAV_STANDARD_MODE_SAFE_RECOVERY and MAV_STANDARD_MODE_RETURN_HOME got merged. --- src/lib/modes/standard_modes.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp index 68d849f09a62..b009d815d425 100644 --- a/src/lib/modes/standard_modes.hpp +++ b/src/lib/modes/standard_modes.hpp @@ -47,11 +47,10 @@ enum class StandardMode : uint8_t { ORBIT = 2, CRUISE = 3, ALTITUDE_HOLD = 4, - RETURN_HOME = 5, - SAFE_RECOVERY = 6, - MISSION = 7, - LAND = 8, - TAKEOFF = 9, + SAFE_RECOVERY = 5, + MISSION = 6, + LAND = 7, + TAKEOFF = 8, }; /** @@ -60,7 +59,7 @@ enum class StandardMode : uint8_t { static inline StandardMode getStandardModeFromNavState(uint8_t nav_state, uint8_t vehicle_type, bool is_vtol) { switch (nav_state) { - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::SAFE_RECOVERY; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION; @@ -105,7 +104,7 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state, uint8_ static inline uint8_t getNavStateFromStandardMode(StandardMode mode, uint8_t vehicle_type, bool is_vtol) { switch (mode) { - case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + case StandardMode::SAFE_RECOVERY: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; From 98fde4cbac067908404f8665c609caa460fa669b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Dec 2024 14:05:55 +1300 Subject: [PATCH 164/211] gimbal: correctly set gimbal_device_id When we use a gimbal connected via "RC", so PWM via the Aux channels, we need to set the gimbal_device_id to 1 as per the protocol. This was missing for GIMBAL_DEVICE_ATTITUDE_STATUS, so I added that, and fixed the name of that variable while at it. --- msg/GimbalDeviceAttitudeStatus.msg | 3 +++ msg/GimbalDeviceInformation.msg | 2 +- src/modules/gimbal/output_mavlink.cpp | 8 ++++---- src/modules/gimbal/output_mavlink.h | 2 +- src/modules/gimbal/output_rc.cpp | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 2 +- .../mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp | 5 +++++ 7 files changed, 19 insertions(+), 7 deletions(-) diff --git a/msg/GimbalDeviceAttitudeStatus.msg b/msg/GimbalDeviceAttitudeStatus.msg index 0be66babe121..0007a1c03d16 100644 --- a/msg/GimbalDeviceAttitudeStatus.msg +++ b/msg/GimbalDeviceAttitudeStatus.msg @@ -16,5 +16,8 @@ float32 angular_velocity_y float32 angular_velocity_z uint32 failure_flags +float32 delta_yaw +float32 delta_yaw_velocity +uint8 gimbal_device_id bool received_from_mavlink diff --git a/msg/GimbalDeviceInformation.msg b/msg/GimbalDeviceInformation.msg index 8f7a41643900..07351672120e 100644 --- a/msg/GimbalDeviceInformation.msg +++ b/msg/GimbalDeviceInformation.msg @@ -33,4 +33,4 @@ float32 pitch_max # [rad] float32 yaw_min # [rad] float32 yaw_max # [rad] -uint8 gimbal_device_compid +uint8 gimbal_device_id diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index 90a6596f9d0e..ca562ec87047 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -162,7 +162,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints _last_update = now; } - gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0; + gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0; _publish_gimbal_device_set_attitude(); } @@ -191,7 +191,7 @@ void OutputMavlinkV2::_check_for_gimbal_device_information() if (_gimbal_device_information_sub.update(&gimbal_device_information)) { _gimbal_device_found = true; - _gimbal_device_compid = gimbal_device_information.gimbal_device_compid; + _gimbal_device_id = gimbal_device_information.gimbal_device_id; } } @@ -210,7 +210,7 @@ void OutputMavlinkV2::print_status() const (double)_angle_velocity[2]); if (_gimbal_device_found) { - PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid); + PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_id); } else { PX4_INFO_RAW(" gimbal device compid not found\n"); @@ -222,7 +222,7 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude() gimbal_device_set_attitude_s set_attitude{}; set_attitude.timestamp = hrt_absolute_time(); set_attitude.target_system = (uint8_t)_parameters.mav_sysid; - set_attitude.target_component = _gimbal_device_compid; + set_attitude.target_component = _gimbal_device_id; set_attitude.angular_velocity_x = _angle_velocity[0]; set_attitude.angular_velocity_y = _angle_velocity[1]; diff --git a/src/modules/gimbal/output_mavlink.h b/src/modules/gimbal/output_mavlink.h index 79ef0e46e739..0e9d9f217fec 100644 --- a/src/modules/gimbal/output_mavlink.h +++ b/src/modules/gimbal/output_mavlink.h @@ -81,7 +81,7 @@ class OutputMavlinkV2 : public OutputBase uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; - uint8_t _gimbal_device_compid{0}; + uint8_t _gimbal_device_id{0}; hrt_abstime _last_gimbal_device_checked{0}; bool _gimbal_device_found {false}; }; diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index 113fe71b0d51..dba7b02b1af9 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -115,6 +115,10 @@ void OutputRC::_stream_device_attitude_status() q.copyTo(attitude_status.q); attitude_status.failure_flags = 0; + + // If the output is RC, then we signal this by referring to compid 1. + attitude_status.gimbal_device_id = 1; + _attitude_status_pub.publish(attitude_status); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fdb6e9fee4fd..78bb387fdbca 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3063,7 +3063,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - gimbal_information.gimbal_device_compid = msg->compid; + gimbal_information.gimbal_device_id = msg->compid; _gimbal_device_information_pub.publish(gimbal_information); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 615a797488d4..44593294a7bf 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -94,6 +94,11 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; + msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; + + msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); return true; From 4a73195007039ddeeb5e4b15f215039d895d44ab Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 17 Dec 2024 12:13:42 +0100 Subject: [PATCH 165/211] yaw_est: store attitude as quaternion instead of DCM This saves flash and makes code simpler --- .../ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 92 ++++--------------- .../ekf2/EKF/yaw_estimator/EKFGSF_yaw.h | 7 +- 2 files changed, 21 insertions(+), 78 deletions(-) diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 1278708d98aa..2b2293904c3c 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -175,8 +175,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an // generate attitude solution using simple complementary filter for the selected model const Vector3f ang_rate = delta_ang / fmaxf(delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias; - const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose(); - const Vector3f gravity_direction_bf = R_to_body.col(2); + const Vector3f gravity_direction_bf = _ahrs_ekf_gsf[model_index].q.inversed().dcm_z(); const float ahrs_accel_norm = _ahrs_accel.norm(); @@ -217,8 +216,9 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an const Vector3f delta_angle_corrected = delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * delta_ang_dt; - // Apply delta angle to rotation matrix - _ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected); + // Apply delta angle to attitude + const Quatf dq(AxisAnglef{delta_angle_corrected}); + _ahrs_ekf_gsf[model_index].q = (_ahrs_ekf_gsf[model_index].q * dq).normalized(); } void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel) @@ -228,28 +228,11 @@ void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel) // 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences. // 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity. - // Calculate earth frame Down axis unit vector rotated into body frame - const Vector3f down_in_bf = -delta_vel.normalized(); - - // Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf' - const Vector3f i_vec_bf(1.f, 0.f, 0.f); - Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf)); - north_in_bf.normalize(); - - // Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf' - const Vector3f east_in_bf = down_in_bf % north_in_bf; - - // Each column in a rotation matrix from earth frame to body frame represents the projection of the - // corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column. - // We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body - // frame are copied into corresponding rows instead. - Dcmf R; - R.setRow(0, north_in_bf); - R.setRow(1, east_in_bf); - R.setRow(2, down_in_bf); + // The tilt is simply the rotation between the measured gravity and the vertical axis + Quatf q(delta_vel, Vector3f(0.f, 0.f, -1.f)); for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { - _ahrs_ekf_gsf[model_index].R = R; + _ahrs_ekf_gsf[model_index].q = q; } } @@ -257,10 +240,9 @@ void EKFGSF_yaw::ahrsAlignYaw() { // Align yaw angle for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { - const float yaw = wrap_pi(_ekf_gsf[model_index].X(2)); - const Dcmf R = _ahrs_ekf_gsf[model_index].R; - _ahrs_ekf_gsf[model_index].R = updateYawInRotMat(yaw, R); + const Dcmf R(_ahrs_ekf_gsf[model_index].q); + _ahrs_ekf_gsf[model_index].q = Quatf(updateYawInRotMat(yaw, R)); } } @@ -275,16 +257,18 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang return; } + const Dcmf R(_ahrs_ekf_gsf[model_index].q); + // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock - _ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R); + _ekf_gsf[model_index].X(2) = getEulerYaw(R); // calculate delta velocity in a horizontal front-right frame - const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * delta_vel; + const Vector3f del_vel_NED = R * delta_vel; const float cos_yaw = cosf(_ekf_gsf[model_index].X(2)); const float sin_yaw = sinf(_ekf_gsf[model_index].X(2)); const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw; const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw; - const float daz = Vector3f(_ahrs_ekf_gsf[model_index].R * delta_ang)(2); + const float daz = Vector3f(R * delta_ang)(2); // delta velocity process noise double if we're not in air const float accel_noise = in_air ? _accel_noise : 2.f * _accel_noise; @@ -319,8 +303,7 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co const float vel_obs_var = sq(fmaxf(vel_accuracy, 0.01f)); // calculate velocity observation innovations - _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - vel_NE(0); - _ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - vel_NE(1); + _ekf_gsf[model_index].innov = _ekf_gsf[model_index].X.xy() - vel_NE; matrix::Matrix K; matrix::SquareMatrix P_new; @@ -367,20 +350,10 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co _ekf_gsf[model_index].X.xy() += delta_state.xy(); _ekf_gsf[model_index].X(2) = wrap_pi(_ekf_gsf[model_index].X(2) + yawDelta); - // apply the change in yaw angle to the AHRS - // take advantage of sparseness in the yaw rotation matrix - const float cosYaw = cosf(yawDelta); - const float sinYaw = sinf(yawDelta); - const float R_prev00 = _ahrs_ekf_gsf[model_index].R(0, 0); - const float R_prev01 = _ahrs_ekf_gsf[model_index].R(0, 1); - const float R_prev02 = _ahrs_ekf_gsf[model_index].R(0, 2); - - _ahrs_ekf_gsf[model_index].R(0, 0) = R_prev00 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 0) * sinYaw; - _ahrs_ekf_gsf[model_index].R(0, 1) = R_prev01 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 1) * sinYaw; - _ahrs_ekf_gsf[model_index].R(0, 2) = R_prev02 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 2) * sinYaw; - _ahrs_ekf_gsf[model_index].R(1, 0) = R_prev00 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 0) * cosYaw; - _ahrs_ekf_gsf[model_index].R(1, 1) = R_prev01 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 1) * cosYaw; - _ahrs_ekf_gsf[model_index].R(1, 2) = R_prev02 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 2) * cosYaw; + // Apply the change in yaw angle to the AHRS using left multiplication to rotate + // the attitude around the earth Down axis + const Quatf dq(cosf(yawDelta / 2.f), 0.f, 0.f, sinf(yawDelta / 2.f)); + _ahrs_ekf_gsf[model_index].q = (dq * _ahrs_ekf_gsf[model_index].q).normalized(); return true; } @@ -456,30 +429,3 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const const float delta_accel_g = (ahrs_accel_norm - CONSTANTS_ONE_G) / CONSTANTS_ONE_G; return _tilt_gain * sq(1.f - math::min(attenuation * fabsf(delta_accel_g), 1.f)); } - -Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) -{ - Matrix3f ret = R; - ret(0, 0) += R(0, 1) * g(2) - R(0, 2) * g(1); - ret(0, 1) += R(0, 2) * g(0) - R(0, 0) * g(2); - ret(0, 2) += R(0, 0) * g(1) - R(0, 1) * g(0); - ret(1, 0) += R(1, 1) * g(2) - R(1, 2) * g(1); - ret(1, 1) += R(1, 2) * g(0) - R(1, 0) * g(2); - ret(1, 2) += R(1, 0) * g(1) - R(1, 1) * g(0); - ret(2, 0) += R(2, 1) * g(2) - R(2, 2) * g(1); - ret(2, 1) += R(2, 2) * g(0) - R(2, 0) * g(2); - ret(2, 2) += R(2, 0) * g(1) - R(2, 1) * g(0); - - // Renormalise rows - for (uint8_t r = 0; r < 3; r++) { - const float rowLengthSq = ret.row(r).norm_squared(); - - if (rowLengthSq > FLT_EPSILON) { - // Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0 - const float rowLengthInv = 1.5f - 0.5f * rowLengthSq; - ret.row(r) *= rowLengthInv; - } - } - - return ret; -} diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h index 8140477d9d65..db1e74d5fd76 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h @@ -94,8 +94,8 @@ class EKFGSF_yaw float _true_airspeed{NAN}; // true airspeed used for centripetal accel compensation (m/s) struct { - matrix::Dcmf R{matrix::eye()}; // matrix that rotates a vector from body to earth frame - matrix::Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation + matrix::Quatf q{}; // attitude: rotates a vector from body to earth frame + matrix::Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation } _ahrs_ekf_gsf[N_MODELS_EKFGSF] {}; bool _ahrs_ekf_gsf_tilt_aligned{false}; // true the initial tilt alignment has been calculated @@ -113,9 +113,6 @@ class EKFGSF_yaw // align all AHRS yaw orientations to initial values void ahrsAlignYaw(); - // Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix - matrix::Matrix3f ahrsPredictRotMat(const matrix::Matrix3f &R, const matrix::Vector3f &g); - // Declarations used by a bank of N_MODELS_EKFGSF EKFs struct { From 0561f6c9fc80fd41113d850c4230d5cb4a2fe4de Mon Sep 17 00:00:00 2001 From: Stefano Colli <45536733+StefanoColli@users.noreply.github.com> Date: Wed, 18 Dec 2024 12:15:03 +0100 Subject: [PATCH 166/211] GZ: add gimbal simulation (#23382) --- .../airframes/4019_gz_x500_gimbal | 23 ++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- src/modules/gimbal/gimbal.cpp | 18 +- src/modules/gimbal/gimbal_params.h | 15 ++ src/modules/gimbal/input_mavlink.cpp | 55 ++-- src/modules/gimbal/input_mavlink.h | 2 + src/modules/gimbal/input_rc.cpp | 6 +- src/modules/gimbal/output.cpp | 11 +- src/modules/mavlink/mavlink_receiver.cpp | 8 +- .../streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp | 68 +++-- .../streams/GIMBAL_DEVICE_INFORMATION.hpp | 1 + .../simulation/gz_bridge/CMakeLists.txt | 2 + src/modules/simulation/gz_bridge/GZBridge.cpp | 7 + src/modules/simulation/gz_bridge/GZBridge.hpp | 2 + src/modules/simulation/gz_bridge/GZGimbal.cpp | 253 ++++++++++++++++++ src/modules/simulation/gz_bridge/GZGimbal.hpp | 151 +++++++++++ 17 files changed, 567 insertions(+), 58 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal create mode 100644 src/modules/simulation/gz_bridge/GZGimbal.cpp create mode 100644 src/modules/simulation/gz_bridge/GZGimbal.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal new file mode 100644 index 000000000000..80719ae3d313 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name Gazebo x500 gimbal +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_gimbal} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 + +# Gimbal settings +param set-default MNT_MODE_IN 4 +param set-default MNT_MODE_OUT 2 +param set-default MNT_RC_IN_MODE 1 + +param set-default MNT_MAN_ROLL 1 +param set-default MNT_MAN_PITCH 2 +param set-default MNT_MAN_YAW 3 + +param set-default MNT_RANGE_ROLL 180 +param set-default MNT_RANGE_PITCH 180 +param set-default MNT_RANGE_YAW 720 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index ef208ab31460..d9ef6143c2a6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -91,6 +91,7 @@ px4_add_romfs_files( 4016_gz_x500_lidar_down 4017_gz_x500_lidar_front 4018_gz_quadtailsitter + 4019_gz_x500_gimbal 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 019f63e2d507..f48a17a4b306 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 019f63e2d50763862b8f8d40cce77371b3260c52 +Subproject commit f48a17a4b30601ebb11e2f9e3e678fec8a73ebdc diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 452408063908..9179055b31ca 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -113,7 +113,7 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input; switch (params.mnt_mode_in) { - case 0: + case MNT_MODE_IN_AUTO: // Automatic // MAVLINK_V2 as well as RC input are supported together. // Whichever signal is updated last, gets control, for RC there is a deadzone @@ -123,19 +123,19 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params); break; - case 1: // RC only + case MNT_MODE_IN_RC: // RC only thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params); break; - case 2: // MAVLINK_ROI commands only (to be deprecated) + case MNT_MODE_IN_MAVLINK_ROI: // MAVLINK_ROI commands only (to be deprecated) thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params); break; - case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated) + case MNT_MODE_IN_MAVLINK_DO_MOUNT: // MAVLINK_DO_MOUNT commands only (to be deprecated) thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params); break; - case 4: //MAVLINK_V2 + case MNT_MODE_IN_MAVLINK_V2: //MAVLINK_V2 thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params); break; @@ -165,21 +165,21 @@ static int gimbal_thread_main(int argc, char *argv[]) } switch (params.mnt_mode_out) { - case 0: //AUX + case MNT_MODE_OUT_AUX: //AUX thread_data.output_obj = new OutputRC(params); if (!thread_data.output_obj) { alloc_failed = true; } break; - case 1: //MAVLink gimbal v1 protocol + case MNT_MODE_OUT_MAVLINK_V1: //MAVLink gimbal v1 protocol thread_data.output_obj = new OutputMavlinkV1(params); if (!thread_data.output_obj) { alloc_failed = true; } break; - case 2: //MAVLink gimbal v2 protocol + case MNT_MODE_OUT_MAVLINK_V2: //MAVLink gimbal v2 protocol thread_data.output_obj = new OutputMavlinkV2(params); if (!thread_data.output_obj) { alloc_failed = true; } @@ -276,7 +276,7 @@ static int gimbal_thread_main(int argc, char *argv[]) // Only publish the mount orientation if the mode is not mavlink v1 or v2 // If the gimbal speaks mavlink it publishes its own orientation. - if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2 + if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2) { thread_data.output_obj->publish(); } diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index f1f3d2d514e3..5a265d02373f 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -40,6 +40,21 @@ namespace gimbal { +enum MntModeIn { + MNT_MODE_IN_DISABLED = -1, + MNT_MODE_IN_AUTO, // RC and MAVLink gimbal protocol v2 + MNT_MODE_IN_RC, + MNT_MODE_IN_MAVLINK_ROI, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_IN_MAVLINK_DO_MOUNT, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_IN_MAVLINK_V2 // MAVLink gimbal protocol v2 +}; + +enum MntModeOut { + MNT_MODE_OUT_AUX = 0, + MNT_MODE_OUT_MAVLINK_V1, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_OUT_MAVLINK_V2 // MAVLink gimbal protocol v2 +}; + struct Parameters { int32_t mnt_mode_in; int32_t mnt_mode_out; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index d78e069ba749..e20bf6c552e7 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -473,28 +473,49 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data) { - // TODO: Take gimbal_device_information into account. + gimbal_device_information_s gimbal_device_info; - gimbal_manager_information_s gimbal_manager_info; - gimbal_manager_info.timestamp = hrt_absolute_time(); + if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == MNT_MODE_OUT_MAVLINK_V2) { + gimbal_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); - gimbal_manager_info.cap_flags = - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + gimbal_manager_info.cap_flags = gimbal_device_info.cap_flags; - gimbal_manager_info.pitch_max = M_PI_F / 2; - gimbal_manager_info.pitch_min = -M_PI_F / 2; - gimbal_manager_info.yaw_max = M_PI_F; - gimbal_manager_info.yaw_min = -M_PI_F; + gimbal_manager_info.roll_max = gimbal_device_info.roll_max; + gimbal_manager_info.roll_min = gimbal_device_info.roll_min; + gimbal_manager_info.pitch_max = gimbal_device_info.pitch_max; + gimbal_manager_info.pitch_min = gimbal_device_info.pitch_min; + gimbal_manager_info.yaw_max = gimbal_device_info.yaw_max; + gimbal_manager_info.yaw_min = gimbal_device_info.yaw_min; - gimbal_manager_info.gimbal_device_id = control_data.device_compid; + gimbal_manager_info.gimbal_device_id = control_data.device_compid; + + _gimbal_manager_info_pub.publish(gimbal_manager_info); + + } else if (_parameters.mnt_mode_out == MNT_MODE_OUT_AUX) { + // since we have a non-Mavlink gimbal device, the gimbal manager itself has to act as the gimbal device + gimbal_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); + + gimbal_manager_info.cap_flags = + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + + gimbal_manager_info.pitch_max = _parameters.mnt_range_pitch; + gimbal_manager_info.pitch_min = -_parameters.mnt_range_pitch; + gimbal_manager_info.yaw_max = _parameters.mnt_range_yaw; + gimbal_manager_info.yaw_min = -_parameters.mnt_range_yaw; + + gimbal_manager_info.gimbal_device_id = control_data.device_compid; + + _gimbal_manager_info_pub.publish(gimbal_manager_info); + } - _gimbal_manager_info_pub.publish(gimbal_manager_info); } InputMavlinkGimbalV2::UpdateResult diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index 67bace21846b..a47af82fc48d 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -125,6 +126,7 @@ class InputMavlinkGimbalV2 : public InputBase int _vehicle_command_sub = -1; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index 07de0b30949c..3c2926ffbc30 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -130,9 +130,11 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData if (_parameters.mnt_rc_in_mode == 0) { // We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees. - matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f), + // We use 179.99 instead of 180 so to avoid that the conversion between quaternions and Euler representation + // (when new_aux_value = 1) gives the equivalent angle (e.g., -180 instead of 180). + matrix::Eulerf euler(new_aux_values[0] * math::radians(179.99f), new_aux_values[1] * math::radians(90.f), - new_aux_values[2] * math::radians(180.f)); + new_aux_values[2] * math::radians(179.99f)); matrix::Quatf q(euler); q.copyTo(control_data.type_data.angle.q); diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index 496e15e05d81..ea474bf33ec5 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -258,12 +258,19 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) _angle_outputs[i] -= euler_vehicle(i); } - if (PX4_ISFINITE(_angle_outputs[i])) { - // bring angles into proper range [-pi, pi] + if (PX4_ISFINITE(_angle_outputs[i]) && _parameters.mnt_rc_in_mode == 0) { + // if we are in angle input mode, we bring angles into proper range [-pi, pi] _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); } } + // constrain angle outputs to [-range/2, range/2] + _angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll / 2), + math::radians(_parameters.mnt_range_roll / 2)); + _angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch / 2), + math::radians(_parameters.mnt_range_pitch / 2)); + _angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw / 2), + math::radians(_parameters.mnt_range_yaw / 2)); // constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed if (_landed) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 78bb387fdbca..b3237241c312 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3063,7 +3063,12 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - gimbal_information.gimbal_device_id = msg->compid; + if (gimbal_device_info_msg.gimbal_device_id == 0) { + gimbal_information.gimbal_device_id = msg->compid; + + } else { + gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id; + } _gimbal_device_information_pub.publish(gimbal_information); } @@ -3090,6 +3095,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags; gimbal_attitude_status.received_from_mavlink = true; + gimbal_attitude_status.gimbal_device_id = gimbal_device_attitude_status_msg.gimbal_device_id; _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 44593294a7bf..c8338996ae67 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -74,32 +74,48 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream return false; } - mavlink_gimbal_device_attitude_status_t msg{}; - - msg.target_system = gimbal_device_attitude_status.target_system; - msg.target_component = gimbal_device_attitude_status.target_component; - - msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; - - msg.flags = gimbal_device_attitude_status.device_flags; - - msg.q[0] = gimbal_device_attitude_status.q[0]; - msg.q[1] = gimbal_device_attitude_status.q[1]; - msg.q[2] = gimbal_device_attitude_status.q[2]; - msg.q[3] = gimbal_device_attitude_status.q[3]; - - msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; - msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; - msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; - - msg.failure_flags = gimbal_device_attitude_status.failure_flags; - - msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; - msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; - - msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; - - mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); + if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) { + // A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id + mavlink_gimbal_device_attitude_status_t msg{}; + + msg.target_system = gimbal_device_attitude_status.target_system; + msg.target_component = gimbal_device_attitude_status.target_component; + + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + + msg.flags = gimbal_device_attitude_status.device_flags; + + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; + + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; + + msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; + msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; + + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); + + } else { + // We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID + mavlink_message_t message; + mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, + _mavlink->get_channel(), &message, + gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, + gimbal_device_attitude_status.timestamp / 1000, + gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, + gimbal_device_attitude_status.angular_velocity_x, + gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, + gimbal_device_attitude_status.failure_flags, + 0, 0, 0); + _mavlink->forward_message(&message, _mavlink); + } return true; } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp index d0aa42f9f06f..ff3def5f096d 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp @@ -83,6 +83,7 @@ class MavlinkStreamGimbalDeviceInformation : public MavlinkStream msg.pitch_max = gimbal_device_information.pitch_max; msg.yaw_min = gimbal_device_information.yaw_min; msg.yaw_max = gimbal_device_information.yaw_max; + msg.gimbal_device_id = gimbal_device_information.gimbal_device_id; mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index e3e6b3cc9088..e57c6020451b 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -61,6 +61,8 @@ if(gz-transport_FOUND) GZMixingInterfaceServo.hpp GZMixingInterfaceWheel.cpp GZMixingInterfaceWheel.hpp + GZGimbal.cpp + GZGimbal.hpp DEPENDS mixer_module px4_work_queue diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 61aa5d0aa4e1..f69be21062db 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -268,6 +268,11 @@ int GZBridge::init() return PX4_ERROR; } + if (!_gimbal.init(_world_name, _model_name)) { + PX4_ERR("failed to init gimbal"); + return PX4_ERROR; + } + ScheduleNow(); return OK; } @@ -1005,6 +1010,7 @@ void GZBridge::Run() _mixing_interface_esc.stop(); _mixing_interface_servo.stop(); _mixing_interface_wheel.stop(); + _gimbal.stop(); exit_and_cleanup(); return; @@ -1021,6 +1027,7 @@ void GZBridge::Run() _mixing_interface_esc.updateParams(); _mixing_interface_servo.updateParams(); _mixing_interface_wheel.updateParams(); + _gimbal.updateParams(); } ScheduleDelayed(10_ms); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index e4c6800755e5..27ef4249564e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -36,6 +36,7 @@ #include "GZMixingInterfaceESC.hpp" #include "GZMixingInterfaceServo.hpp" #include "GZMixingInterfaceWheel.hpp" +#include "GZGimbal.hpp" #include #include @@ -184,6 +185,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; + GZGimbal _gimbal{_node, _node_mutex}; px4::atomic _world_time_us{0}; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp new file mode 100644 index 000000000000..e016098fce8a --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -0,0 +1,253 @@ +// #define DEBUG_BUILD +#include "GZGimbal.hpp" + +bool GZGimbal::init(const std::string &world_name, const std::string &model_name) +{ + // Gazebo communication + const std::string gimbal_roll_topic = "/model/" + model_name + "/command/gimbal_roll"; + _gimbal_roll_cmd_publisher = _node.Advertise(gimbal_roll_topic); + + if (!_gimbal_roll_cmd_publisher.Valid()) { + PX4_ERR("failed to advertise %s", gimbal_roll_topic.c_str()); + return false; + } + + const std::string gimbal_pitch_topic = "/model/" + model_name + "/command/gimbal_pitch"; + _gimbal_pitch_cmd_publisher = _node.Advertise(gimbal_pitch_topic); + + if (!_gimbal_pitch_cmd_publisher.Valid()) { + PX4_ERR("failed to advertise %s", gimbal_pitch_topic.c_str()); + return false; + } + + const std::string gimbal_yaw_topic = "/model/" + model_name + "/command/gimbal_yaw"; + _gimbal_yaw_cmd_publisher = _node.Advertise(gimbal_yaw_topic); + + if (!_gimbal_yaw_cmd_publisher.Valid()) { + PX4_ERR("failed to advertise %s", gimbal_yaw_topic.c_str()); + return false; + } + + const std::string gimbal_imu_topic = "/world/" + world_name + "/model/" + model_name + + "/link/camera_link/sensor/camera_imu/imu"; + + if (!_node.Subscribe(gimbal_imu_topic, &GZGimbal::gimbalIMUCallback, this)) { + PX4_ERR("failed to subscribe to %s", gimbal_imu_topic.c_str()); + return false; + } + + // Mount parameters + _mnt_range_roll_handle = param_find("MNT_RANGE_ROLL"); + _mnt_range_pitch_handle = param_find("MNT_RANGE_PITCH"); + _mnt_range_yaw_handle = param_find("MNT_RANGE_YAW"); + _mnt_mode_out_handle = param_find("MNT_MODE_OUT"); + + if (_mnt_range_roll_handle == PARAM_INVALID || + _mnt_range_pitch_handle == PARAM_INVALID || + _mnt_range_yaw_handle == PARAM_INVALID || + _mnt_mode_out_handle == PARAM_INVALID) { + return false; + } + + updateParameters(); + + ScheduleOnInterval(200_ms); // @5Hz + + // Schedule on vehicle command messages + if (!_vehicle_command_sub.registerCallback()) { + return false; + } + + return true; +} + +void GZGimbal::Run() +{ + pthread_mutex_lock(&_node_mutex); + + const hrt_abstime now = hrt_absolute_time(); + const float dt = (now - _last_time_update) / 1e6f; + _last_time_update = now; + + updateParameters(); + + if (pollSetpoint()) { + //TODO handle device flags + publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _roll_min, _roll_max, dt); + publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max, + dt); + publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt); + } + + if (_mnt_mode_out == 2) { + // We have a Mavlink gimbal capable of sending messages + publishDeviceInfo(); + publishDeviceAttitude(); + } + + pthread_mutex_unlock(&_node_mutex); +} + +void GZGimbal::stop() +{ + ScheduleClear(); +} + +void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) +{ + pthread_mutex_lock(&_node_mutex); + + static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f); + const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(), + IMU_data.orientation().x(), + IMU_data.orientation().y(), + IMU_data.orientation().z()); + _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); + + pthread_mutex_unlock(&_node_mutex); +} + +void GZGimbal::updateParameters() +{ + param_get(_mnt_range_roll_handle, &_mnt_range_roll); + param_get(_mnt_range_pitch_handle, &_mnt_range_pitch); + param_get(_mnt_range_yaw_handle, &_mnt_range_yaw); + param_get(_mnt_mode_out_handle, &_mnt_mode_out); +} + +bool GZGimbal::pollSetpoint() +{ + if (_gimbal_device_set_attitude_sub.updated()) { + gimbal_device_set_attitude_s msg; + + if (_gimbal_device_set_attitude_sub.copy(&msg)) { + const matrix::Eulerf gimbal_att_stp(matrix::Quatf(msg.q)); + _roll_stp = gimbal_att_stp.phi(); + _pitch_stp = gimbal_att_stp.theta(); + _yaw_stp = gimbal_att_stp.psi(); + _roll_rate_stp = msg.angular_velocity_x; + _pitch_rate_stp = msg.angular_velocity_y; + _yaw_rate_stp = msg.angular_velocity_z; + _gimbal_device_flags = msg.flags; + + return true; + } + + } else if (_gimbal_controls_sub.updated()) { + gimbal_controls_s msg; + + if (_gimbal_controls_sub.copy(&msg)) { + // map control inputs from [-1;1] to [min_angle; max_angle] using the range parameters + _roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll / 2), _roll_min, _roll_max); + _pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch / 2), _pitch_min, + _pitch_max); + _yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max); + + return true; + } + } + + return false; +} + +void GZGimbal::publishDeviceInfo() +{ + if (_vehicle_command_sub.updated()) { + vehicle_command_s cmd; + _vehicle_command_sub.copy(&cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE && + (uint16_t)cmd.param1 == vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION) { + // Acknowledge the command + vehicle_command_ack_s command_ack{}; + + command_ack.command = cmd.command; + command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + command_ack.timestamp = hrt_absolute_time(); + + _vehicle_command_ack_pub.publish(command_ack); + + // Send the requested message + gimbal_device_information_s device_info{}; + + memcpy(device_info.vendor_name, _vendor_name, sizeof(_vendor_name)); + memcpy(device_info.model_name, _model_name, sizeof(_model_name)); + memcpy(device_info.custom_name, _custom_name, sizeof(_custom_name)); + device_info.firmware_version = _firmware_version; + device_info.hardware_version = _hardware_version; + device_info.uid = _uid; + device_info.cap_flags = _cap_flags; + device_info.custom_cap_flags = _custom_cap_flags; + device_info.roll_min = _roll_min; + device_info.roll_max = _roll_max; + device_info.pitch_min = _pitch_min; + device_info.pitch_max = _pitch_max; + device_info.yaw_min = _yaw_min; + device_info.yaw_max = _yaw_max; + device_info.gimbal_device_id = _gimbal_device_id; + device_info.timestamp = hrt_absolute_time(); + + _gimbal_device_information_pub.publish(device_info); + } + } +} + +void GZGimbal::publishDeviceAttitude() +{ + // TODO handle flags + + gimbal_device_attitude_status_s gimbal_att{}; + + gimbal_att.target_system = 0; // Broadcast + gimbal_att.target_component = 0; // Broadcast + gimbal_att.device_flags = 0; + _q_gimbal.copyTo(gimbal_att.q); + gimbal_att.angular_velocity_x = _gimbal_rate[0]; + gimbal_att.angular_velocity_y = _gimbal_rate[1]; + gimbal_att.angular_velocity_z = _gimbal_rate[2]; + gimbal_att.failure_flags = 0; + gimbal_att.timestamp = hrt_absolute_time(); + + _gimbal_device_attitude_status_pub.publish(gimbal_att); +} + +void GZGimbal::publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp, + float &last_stp, const float min_stp, const float max_stp, const float dt) +{ + gz::msgs::Double msg; + + float new_stp = computeJointSetpoint(att_stp, rate_stp, last_stp, dt); + new_stp = math::constrain(new_stp, min_stp, max_stp); + last_stp = new_stp; + msg.set_data(new_stp); + + publisher.Publish(msg); +} + +float GZGimbal::computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt) +{ + + if (PX4_ISFINITE(rate_stp)) { + const float rate_diff = dt * rate_stp; + const float stp_from_rate = last_stp + rate_diff; + + if (PX4_ISFINITE(att_stp)) { + // We use the attitude rate setpoint but we constrain it by the desired angle + return rate_diff > 0 ? math::min(att_stp, stp_from_rate) : math::max(att_stp, stp_from_rate); + + } else { + // The rate setpoint is valid while the angle one is not + return stp_from_rate; + } + + } else if (PX4_ISFINITE(att_stp)) { + // Only the angle setpoint is valid + return att_stp; + + } else { + // Neither setpoint is valid so we steer the gimbal to the default position (roll = pitch = yaw = 0) + return 0.0f; + } +} diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp new file mode 100644 index 000000000000..00b46becc48f --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -0,0 +1,151 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +using namespace time_literals; + +class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams +{ +public: + GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl), + ModuleParams(nullptr), + _node(node), + _node_mutex(node_mutex) + {} + +private: + friend class GZBridge; + + gz::transport::Node &_node; + pthread_mutex_t &_node_mutex; + + uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; + uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)}; + uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)}; + + uORB::Publication _gimbal_device_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Publication _gimbal_device_information_pub{ORB_ID(gimbal_device_information)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + + gz::transport::Node::Publisher _gimbal_roll_cmd_publisher; + gz::transport::Node::Publisher _gimbal_pitch_cmd_publisher; + gz::transport::Node::Publisher _gimbal_yaw_cmd_publisher; + + float _roll_stp = NAN; + float _pitch_stp = NAN; + float _yaw_stp = NAN; + + float _last_roll_stp = 0.0f; + float _last_pitch_stp = 0.0f; + float _last_yaw_stp = 0.0f; + + float _roll_rate_stp = NAN; + float _pitch_rate_stp = NAN; + float _yaw_rate_stp = NAN; + + hrt_abstime _last_time_update; + + // Mount parameters + param_t _mnt_range_pitch_handle = PARAM_INVALID; + param_t _mnt_range_roll_handle = PARAM_INVALID; + param_t _mnt_range_yaw_handle = PARAM_INVALID; + param_t _mnt_mode_out_handle = PARAM_INVALID; + float _mnt_range_pitch = 0.0f; + float _mnt_range_roll = 0.0f; + float _mnt_range_yaw = 0.0f; + int32_t _mnt_mode_out = 0; + + matrix::Quatf _q_gimbal = matrix::Quatf(1.0f, 0.0f, 0.0f, 0.0f); + float _gimbal_rate[3] = {NAN}; + + // Device information attributes + const char _vendor_name[32] = "PX4"; + const char _model_name[32] = "Gazebo Gimbal"; + const char _custom_name[32] = ""; + const uint8_t _firmware_dev_version = 0; + const uint8_t _firmware_patch_version = 0; + const uint8_t _firmware_minor_version = 0; + const uint8_t _firmware_major_version = 1; + const uint32_t _firmware_version = (_firmware_dev_version & 0xff) << 24 | (_firmware_patch_version & 0xff) << 16 | + (_firmware_minor_version & 0xff) << 8 | (_firmware_major_version & 0xff); + const uint8_t _hardware_dev_version = 0; + const uint8_t _hardware_patch_version = 0; + const uint8_t _hardware_minor_version = 0; + const uint8_t _hardware_major_version = 1; + const uint32_t _hardware_version = (_hardware_dev_version & 0xff) << 24 | (_hardware_patch_version & 0xff) << 16 | + (_hardware_minor_version & 0xff) << 8 | (_hardware_major_version & 0xff); + const uint64_t _uid = 0x9a77a55b3c10971f ; + const uint16_t _cap_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW; + const uint16_t _custom_cap_flags = 0; + + // This module act as the gimbal driver. In case of a Mavlink compatible gimbal, the driver is aware of + // its mechanical limits. So the values below have to match the characteristics of the simulated gimbal + const float _roll_min = -0.785398f; + const float _roll_max = 0.785398f; + const float _pitch_min = -2.35619f; + const float _pitch_max = 0.785398f; + const float _yaw_min = NAN; // infinite yaw + const float _yaw_max = NAN; // infinite yaw + + const uint8_t _gimbal_device_id = 154; // TODO the implementation differs from the protocol + uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS + + bool init(const std::string &world_name, const std::string &model_name); + void Run() override; + void stop(); + void gimbalIMUCallback(const gz::msgs::IMU &IMU_data); + void updateParameters(); + /// @brief Poll for new gimbal setpoints either from mavlink gimbal v2 protocol (gimbal_device_set_attitude topic) or from RC inputs (gimbal_controls topic). + /// @return true if a new setpoint has been requested; false otherwise. + bool pollSetpoint(); + /// @brief Respond to the gimbal manager when it requests GIMBAL_DEVICE_INFORMATION messages. + void publishDeviceInfo(); + /// @brief Broadcast gimbal device attitude status message. + void publishDeviceAttitude(); + /// @brief Compute joint position setpoint taking into account both desired position and velocity. Then publish the command using the specified gazebo node. + /// @param publisher Gazebo node that will publish the setpoint + /// @param att_stp desired joint attitude [rad] + /// @param rate_stp desired joint attitude rate [rad/s] + /// @param last_stp last joint attitude setpoint [rad] + /// @param min_stp minimum joint attitude [rad] + /// @param max_stp maximum joint attitude [rad] + /// @param dt time interval since the last computation [s] + static void publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp, + float &last_stp, + const float min_stp, const float max_stp, const float dt); + /// @brief Compute joint position setpoint taking into account both desired position and velocity. + /// @param att_stp desired joint attitude [rad] + /// @param rate_stp desired joint attitude rate [rad/s] + /// @param last_stp last joint attitude setpoint [rad] + /// @param dt time interval since the last computation [s] + /// @return new joint setpoint + static float computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt); +}; From 37401d6fd1e56a0cad08425642ee0cb4a1c37066 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 30 Sep 2024 11:19:57 +0200 Subject: [PATCH 167/211] AlphaFilter: allow setting dt in update call --- src/lib/mathlib/math/filter/AlphaFilter.hpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 4e651cfee960..b270bd43f944 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -71,6 +71,8 @@ class AlphaFilter if (denominator > FLT_EPSILON) { setAlpha(sample_interval / denominator); } + + _time_constant = time_constant; } bool setCutoffFreq(float sample_freq, float cutoff_freq) @@ -82,8 +84,7 @@ class AlphaFilter return false; } - setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq)); - _cutoff_freq = cutoff_freq; + setParameters(1.f / sample_freq, 1.f / (M_TWOPI_F * cutoff_freq)); return true; } @@ -112,14 +113,19 @@ class AlphaFilter return _filter_state; } + const T update(const T &sample, float dt) + { + setParameters(dt, _time_constant); + return update(sample); + } + const T &getState() const { return _filter_state; } - float getCutoffFreq() const { return _cutoff_freq; } + float getCutoffFreq() const { return 1.f / (M_TWOPI_F * _time_constant); } protected: T updateCalculation(const T &sample); - - float _cutoff_freq{0.f}; + float _time_constant{0.f}; float _alpha{0.f}; T _filter_state{}; }; From fa5a781e20cd8c17081305c4e1b33eefb5e7eac3 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 30 Sep 2024 12:51:09 +0200 Subject: [PATCH 168/211] AlphaFilter: set time constant instead of alpha Then the update function can set the dt at every iteration if needed --- src/lib/mathlib/math/filter/AlphaFilter.hpp | 3 ++- src/modules/ekf2/EKF/ekf.h | 16 +++++++++------- .../tasks/Utility/StickYaw.cpp | 7 +++---- .../tasks/Utility/StickYaw.hpp | 4 ++-- 4 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index b270bd43f944..8536f33976b4 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -52,7 +52,8 @@ class AlphaFilter { public: AlphaFilter() = default; - explicit AlphaFilter(float alpha) : _alpha(alpha) {} + explicit AlphaFilter(float sample_interval, float time_constant) { setParameters(sample_interval, time_constant); } + explicit AlphaFilter(float time_constant) : _time_constant(time_constant) {}; ~AlphaFilter() = default; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b831d096d153..fc4aa0e41ab5 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -518,7 +518,7 @@ class Ekf final : public EstimatorInterface Vector3f _ref_body_rate{}; Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) - AlphaFilter _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) + AlphaFilter _flow_vel_body_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) uint32_t _flow_counter{0}; ///< number of flow samples read for initialization Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive @@ -583,14 +583,15 @@ class Ekf final : public EstimatorInterface // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; - 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) + static constexpr float _kSensorLpfTimeConstant = 0.09f; + AlphaFilter _accel_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilter _gyro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< 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 _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) + AlphaFilter _baro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< 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}; @@ -600,12 +601,12 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) // used by magnetometer fusion mode selection - AlphaFilter _mag_heading_innov_lpf{0.1f}; + AlphaFilter _mag_heading_innov_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time estimator_aid_source3d_s _aid_src_mag{}; - AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) + AlphaFilter _mag_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< 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 @@ -1011,7 +1012,8 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref}; - AlphaFilter _ev_q_error_filt{0.001f}; + static constexpr float _kQuatErrorLpfTimeConstant = 10.f; + AlphaFilter _ev_q_error_filt{_dt_ekf_avg, _kQuatErrorLpfTimeConstant}; bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 608e4443c330..0118e53cd434 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -57,9 +57,8 @@ void StickYaw::ekfResetHandler(const float 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); + const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw, deltatime); if (reset_setpoint) { yaw_setpoint = NAN; @@ -71,7 +70,7 @@ void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev); } -bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw) +bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw, const float deltatime) { if (!PX4_ISFINITE(unaided_yaw)) { _yaw_correction = 0.f; @@ -84,7 +83,7 @@ bool StickYaw::updateYawCorrection(const float yaw, const float 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); + _yaw_error_lpf.update(yaw_error, deltatime); const bool was_converging = _yaw_estimate_converging; _yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index f15b4dd574a3..a1fdaae9f0b2 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -60,11 +60,11 @@ class StickYaw : public ModuleParams 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 + AlphaFilter _yaw_error_lpf{_kYawErrorTimeConstant}; ///< 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); + bool updateYawCorrection(float yaw, float unaided_yaw, float deltatime); /** * Lock yaw when not currently turning From 480c232bfd761784435bae4998702fada831e030 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 16 Dec 2024 12:01:36 +0100 Subject: [PATCH 169/211] ekf2: use alpha filter class --- .../ekf2/EKF/aid_sources/magnetometer/mag_control.cpp | 2 +- src/modules/ekf2/EKF/covariance.cpp | 5 +++-- src/modules/ekf2/EKF/ekf.cpp | 11 ++--------- src/modules/ekf2/EKF/ekf.h | 9 +++++++-- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 85979a24472e..ae1142c86b64 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -482,7 +482,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) // Check if there has been enough change in horizontal velocity to make yaw observable const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; - if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) { + if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) { // yaw angle must be observable to consider consistency _control_status.flags.mag_heading_consistent = true; } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f148a99d9124..0c9a5df6cba2 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -202,8 +202,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_WIND) // wind vel: add process noise - float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt); + const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate)); + const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; for (unsigned index = 0; index < State::wind_vel.dof; index++) { const unsigned i = State::wind_vel.idx + index; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a0df2a2ad710..febe46f90e2c 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -272,15 +272,8 @@ void Ekf::predictState(const imuSample &imu_delayed) // constrain states _state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit); - - // calculate a filtered horizontal acceleration with a 1 sec time constant - // this are used for manoeuvre detection elsewhere - const float alpha = 1.0f - imu_delayed.delta_vel_dt; - _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - - // 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; + // calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere + _accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt); } bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude, diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fc4aa0e41ab5..8df41e6ba823 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -490,8 +490,13 @@ class Ekf final : public EstimatorInterface Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) - float _height_rate_lpf{0.0f}; + static constexpr float _kAccelHorizLpfTimeConstant = 1.f; + AlphaFilter _accel_horiz_lpf{_kAccelHorizLpfTimeConstant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) + +#if defined(CONFIG_EKF2_WIND) + static constexpr float _kHeightRateLpfTimeConstant = 10.f; + AlphaFilter _height_rate_lpf{_kHeightRateLpfTimeConstant}; +#endif // CONFIG_EKF2_WIND SquareMatrixState P{}; ///< state covariance matrix From 6b10f1ca12a5d7d2a47ccda02479a0732d7e5eb4 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 16 Dec 2024 13:48:00 +0100 Subject: [PATCH 170/211] ekf: update change indicator The calculation of "alpha" in the accel_horiz and height_rate lowpass filters is slightly different than before. --- .../test/change_indication/ekf_gsf_reset.csv | 68 +++++++++---------- .../ekf2/test/change_indication/iris_gps.csv | 38 +++++------ 2 files changed, 53 insertions(+), 53 deletions(-) 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 fbc6d2b054a5..9799320ab265 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -146,33 +146,33 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 14390000,0.71,0.00064,-0.014,0.71,0.007,0.0049,-0.037,0,0,-4.9e+02,-0.0011,-0.0058,-8.9e-05,0.0018,0.0047,-0.11,0.21,-1.1e-06,0.43,-0.00016,0.00075,-3.7e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 14490000,0.71,0.00053,-0.014,0.71,0.008,0.0064,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,0.00054,0.0041,-0.11,0.21,-5.6e-07,0.43,-0.00014,0.00072,-3.7e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 14590000,0.71,0.00043,-0.013,0.71,0.0061,0.0048,-0.041,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.00033,0.0039,-0.11,0.21,-4.7e-07,0.43,-0.00013,0.00069,-5.1e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14690000,0.71,0.00038,-0.013,0.71,0.0078,0.0029,-0.038,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.00028,0.003,-0.11,0.21,-7.5e-08,0.43,-0.00012,0.0007,-3.6e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14690000,0.71,0.00038,-0.013,0.71,0.0078,0.0029,-0.038,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.00028,0.003,-0.11,0.21,-7.4e-08,0.43,-0.00012,0.0007,-3.6e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 14790000,0.71,0.00036,-0.013,0.71,0.0055,0.0014,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-8.6e-05,-0.00044,0.0028,-0.12,0.21,-1.3e-07,0.43,-0.00012,0.00068,-3.7e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0098,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0031,-0.037,0,0,-4.9e+02,-0.001,-0.0058,-8.5e-05,-0.00068,0.0022,-0.12,0.21,1.4e-07,0.43,-0.00011,0.00068,-3.4e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0097,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -14990000,0.71,0.00028,-0.013,0.71,0.0068,0.0021,-0.032,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.0012,0.0027,-0.12,0.21,-6.2e-08,0.43,-0.00012,0.00066,-4.6e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.013,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +14990000,0.71,0.00028,-0.013,0.71,0.0068,0.0021,-0.032,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.0012,0.0027,-0.12,0.21,-6.1e-08,0.43,-0.00012,0.00066,-4.6e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.013,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 15090000,0.71,0.00021,-0.013,0.71,0.0075,0.0021,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.0013,0.0031,-0.12,0.21,-1.6e-07,0.43,-0.00013,0.00064,-4.7e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.031,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0093,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 15190000,0.71,0.00017,-0.013,0.71,0.0073,0.0027,-0.033,0,0,-4.9e+02,-0.00099,-0.0059,-8.9e-05,-0.0019,0.0034,-0.12,0.21,-1.5e-07,0.43,-0.00014,0.00061,-5.4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.0091,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15290000,0.71,0.00019,-0.013,0.71,0.0077,0.0037,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.00097,0.0032,-0.12,0.21,-9e-08,0.43,-0.00015,0.00061,-2.9e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15290000,0.71,0.00019,-0.013,0.71,0.0077,0.0037,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.00097,0.0032,-0.12,0.21,-8.9e-08,0.43,-0.00015,0.00061,-2.9e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 15390000,0.71,0.0002,-0.013,0.71,0.007,0.005,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.3e-05,-0.00013,0.0017,-0.12,0.21,2.8e-07,0.43,-0.00014,0.00064,-5.8e-06,0,0,-4.9e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15490000,0.71,0.00022,-0.013,0.71,0.0086,0.0041,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.6e-05,-0.00053,0.003,-0.12,0.21,-1.8e-07,0.43,-0.00016,0.00061,-2.4e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15490000,0.71,0.00022,-0.013,0.71,0.0086,0.0041,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.6e-05,-0.00052,0.003,-0.12,0.21,-1.8e-07,0.43,-0.00016,0.00061,-2.4e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 15590000,0.71,0.00018,-0.013,0.71,0.0072,0.0031,-0.027,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.0012,0.0037,-0.12,0.21,-5.4e-07,0.43,-0.00016,0.0006,-4.9e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0032,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.00068,0.0041,-0.12,0.21,-8.5e-07,0.43,-0.00017,0.0006,-5.2e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 15790000,0.71,0.00019,-0.013,0.71,0.0081,0.0017,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.00088,0.0043,-0.12,0.21,-9.6e-07,0.43,-0.00018,0.00059,-5.6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0082,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 15890000,0.71,0.0002,-0.013,0.71,0.0088,0.0016,-0.028,0,0,-4.9e+02,-0.0011,-0.0059,-8.8e-05,-0.00015,0.0046,-0.12,0.21,-1.2e-06,0.43,-0.00019,0.00059,-4.7e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.2e-06,3.8e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 15990000,0.71,0.00018,-0.013,0.71,0.0085,0.0017,-0.024,0,0,-4.9e+02,-0.0011,-0.0059,-8.3e-05,0.00049,0.0038,-0.12,0.21,-1e-06,0.43,-0.0002,0.0006,-2.9e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 16090000,0.71,0.00021,-0.013,0.71,0.011,0.0034,-0.02,0,0,-4.9e+02,-0.0011,-0.0059,-7.9e-05,0.0013,0.0025,-0.12,0.21,-7.5e-07,0.43,-0.00017,0.00065,-1.3e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.049,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16190000,0.71,0.00026,-0.013,0.71,0.01,0.0036,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.002,0.0027,-0.12,0.21,-1.1e-06,0.43,-0.00018,0.00065,-6.5e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16290000,0.71,0.00028,-0.014,0.71,0.012,0.0046,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0024,0.0014,-0.12,0.21,-5.8e-07,0.43,-0.00017,0.00067,8.3e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.048,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16190000,0.71,0.00026,-0.013,0.71,0.01,0.0036,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.002,0.0027,-0.12,0.21,-1.1e-06,0.43,-0.00018,0.00065,-6.4e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16290000,0.71,0.00028,-0.014,0.71,0.012,0.0046,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0024,0.0014,-0.12,0.21,-5.8e-07,0.43,-0.00017,0.00067,8.4e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.048,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 16390000,0.71,0.00033,-0.014,0.71,0.01,0.0031,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.0035,0.0025,-0.12,0.21,-1.4e-06,0.43,-0.00019,0.00066,1.3e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.023,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 16490000,0.71,0.00042,-0.014,0.71,0.0088,0.0044,-0.022,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0044,0.0027,-0.12,0.21,-1.6e-06,0.43,-0.0002,0.00066,2.7e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 16590000,0.71,0.00052,-0.013,0.71,0.0067,0.0053,-0.023,0,0,-4.9e+02,-0.0012,-0.0058,-7.5e-05,0.0045,0.0024,-0.12,0.21,-1.6e-06,0.43,-0.00019,0.00065,2.3e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0095,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 16690000,0.71,0.00049,-0.013,0.71,0.0076,0.0056,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.0039,0.0031,-0.12,0.21,-1.8e-06,0.43,-0.00019,0.00065,1.2e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16790000,0.71,0.00048,-0.013,0.71,0.0057,0.0063,-0.018,0,0,-4.9e+02,-0.0011,-0.0058,-7.9e-05,0.0037,0.0027,-0.12,0.21,-1.7e-06,0.43,-0.00017,0.00065,-6e-07,0,0,-4.9e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0093,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16890000,0.71,0.00054,-0.013,0.71,0.0055,0.0072,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.0041,0.0033,-0.13,0.21,-2e-06,0.43,-0.00018,0.00064,4e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -16990000,0.71,0.00051,-0.013,0.71,0.0055,0.0049,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.004,0.0043,-0.13,0.21,-2.5e-06,0.43,-0.0002,0.00063,-6.1e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +16790000,0.71,0.00048,-0.013,0.71,0.0057,0.0063,-0.018,0,0,-4.9e+02,-0.0011,-0.0058,-7.9e-05,0.0037,0.0027,-0.12,0.21,-1.7e-06,0.43,-0.00017,0.00065,-5.6e-07,0,0,-4.9e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0093,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16890000,0.71,0.00054,-0.013,0.71,0.0055,0.0072,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.0041,0.0033,-0.13,0.21,-2e-06,0.43,-0.00018,0.00064,4.1e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +16990000,0.71,0.00051,-0.013,0.71,0.0055,0.0049,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.004,0.0043,-0.13,0.21,-2.5e-06,0.43,-0.0002,0.00063,-6e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 17090000,0.71,0.00055,-0.013,0.71,0.0058,0.0064,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8e-05,0.005,0.0046,-0.13,0.21,-2.8e-06,0.43,-0.00022,0.00063,6.2e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 17190000,0.71,0.0006,-0.013,0.71,0.0059,0.0074,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-7.6e-05,0.0057,0.0045,-0.13,0.21,-3.1e-06,0.43,-0.00022,0.00064,8.8e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0068,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17290000,0.71,0.00063,-0.013,0.71,0.0077,0.0081,-0.011,0,0,-4.9e+02,-0.0012,-0.0059,-7.8e-05,0.0061,0.0055,-0.13,0.21,-3.4e-06,0.43,-0.00023,0.00063,1.1e-05,0,0,-4.9e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0086,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17290000,0.71,0.00063,-0.013,0.71,0.0077,0.0081,-0.011,0,0,-4.9e+02,-0.0012,-0.0059,-7.8e-05,0.0061,0.0055,-0.13,0.21,-3.4e-06,0.43,-0.00023,0.00063,1.2e-05,0,0,-4.9e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0086,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 17390000,0.71,0.00068,-0.013,0.71,0.0075,0.0085,-0.0095,0,0,-4.9e+02,-0.0012,-0.0059,-7.2e-05,0.0069,0.0052,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00063,2.9e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.5e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 17490000,0.71,0.00063,-0.013,0.71,0.0091,0.0086,-0.0078,0,0,-4.9e+02,-0.0012,-0.0059,-7.2e-05,0.0063,0.005,-0.13,0.21,-3.4e-06,0.43,-0.00025,0.00063,2.4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 17590000,0.71,0.0006,-0.013,0.71,0.0099,0.0079,-0.0024,0,0,-4.9e+02,-0.0012,-0.0059,-6.9e-05,0.0067,0.0052,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00064,2.2e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0065,0.0083,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 @@ -180,10 +180,10 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 17790000,0.71,0.00056,-0.013,0.71,0.012,0.01,-0.0042,0,0,-4.9e+02,-0.0012,-0.0059,-5.9e-05,0.0077,0.0039,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00066,4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.0081,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 17890000,0.71,0.00055,-0.013,0.71,0.015,0.011,-0.004,0,0,-4.9e+02,-0.0012,-0.0059,-5.7e-05,0.0076,0.0032,-0.13,0.21,-3.2e-06,0.43,-0.00025,0.00067,4.4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.02,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 17990000,0.71,0.00048,-0.013,0.71,0.016,0.0084,-0.0027,0,0,-4.9e+02,-0.0012,-0.0059,-5.6e-05,0.0073,0.0035,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00066,4.1e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0078,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -18090000,0.71,0.00047,-0.013,0.71,0.017,0.0076,-0.00044,0,0,-4.9e+02,-0.0012,-0.0059,-6.1e-05,0.0068,0.0045,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.6e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18090000,0.71,0.00047,-0.013,0.71,0.017,0.0076,-0.00044,0,0,-4.9e+02,-0.0012,-0.0059,-6.1e-05,0.0068,0.0044,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.6e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 18190000,0.71,0.00043,-0.013,0.71,0.018,0.0086,0.001,0,0,-4.9e+02,-0.0012,-0.0059,-5.7e-05,0.007,0.0041,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00065,3.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.041,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0077,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 18290000,0.71,0.00035,-0.013,0.71,0.018,0.0081,0.0022,0,0,-4.9e+02,-0.0012,-0.0059,-5.9e-05,0.0066,0.0044,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18390000,0.71,0.00031,-0.013,0.71,0.02,0.01,0.0035,0,0,-4.9e+02,-0.0012,-0.0059,-5.3e-05,0.0063,0.0036,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00065,3.3e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.006,0.0075,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18390000,0.71,0.00031,-0.013,0.71,0.02,0.01,0.0035,0,0,-4.9e+02,-0.0012,-0.0059,-5.3e-05,0.0064,0.0036,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00065,3.3e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.006,0.0075,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 18490000,0.71,0.00037,-0.013,0.71,0.021,0.011,0.0031,0,0,-4.9e+02,-0.0012,-0.0059,-5.2e-05,0.0069,0.0037,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00066,3.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 18590000,0.71,0.00039,-0.013,0.71,0.02,0.012,0.0014,0,0,-4.9e+02,-0.0012,-0.0059,-4.4e-05,0.0077,0.0031,-0.13,0.21,-3.6e-06,0.43,-0.00026,0.00067,4.2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 18690000,0.71,0.0003,-0.013,0.71,0.022,0.012,-0.00039,0,0,-4.9e+02,-0.0012,-0.0059,-4.6e-05,0.007,0.0031,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00066,3.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 @@ -206,25 +206,25 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 20390000,0.71,0.00066,-0.013,0.7,0.0086,0.0091,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.4e-05,0.012,0.001,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00076,6.8e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.0051,0.006,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 20490000,0.71,0.00071,-0.013,0.7,0.0087,0.009,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.012,0.0013,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00075,6.6e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.006,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 20590000,0.71,0.00074,-0.013,0.7,0.0078,0.007,0.0099,0,0,-4.9e+02,-0.0013,-0.0058,4.2e-05,0.012,0.0018,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00074,6.6e-05,0,0,-4.9e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0059,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20690000,0.71,0.00077,-0.013,0.7,0.0085,0.007,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.012,0.0016,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00075,6.7e-05,0,0,-4.9e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.005,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20690000,0.71,0.00077,-0.013,0.7,0.0085,0.007,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.012,0.0016,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00075,6.8e-05,0,0,-4.9e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.005,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 20790000,0.71,0.00081,-0.013,0.7,0.0063,0.0066,0.012,0,0,-4.9e+02,-0.0013,-0.0058,5e-05,0.013,0.0017,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00075,6.2e-05,0,0,-4.9e+02,9.8e-05,8.8e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0058,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 20890000,0.71,0.00081,-0.013,0.7,0.0062,0.0062,0.011,0,0,-4.9e+02,-0.0013,-0.0058,5.8e-05,0.013,0.0013,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00077,6.7e-05,0,0,-4.9e+02,9.9e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 20990000,0.71,0.00083,-0.013,0.7,0.0045,0.0039,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.2e-05,0.013,0.0015,-0.13,0.21,-4.8e-06,0.43,-0.00034,0.00077,6.4e-05,0,0,-4.9e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0049,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 21090000,0.71,0.00081,-0.013,0.7,0.0054,0.0032,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.013,0.0011,-0.13,0.21,-4.6e-06,0.43,-0.00033,0.00078,6.1e-05,0,0,-4.9e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 21190000,0.71,0.00082,-0.013,0.7,0.0057,0.0023,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.013,0.0012,-0.13,0.21,-4.6e-06,0.43,-0.00033,0.00077,5.8e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.3e-07,9.5e-07,2e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 21290000,0.71,0.0009,-0.013,0.7,0.005,0.0023,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.8e-05,0.013,0.00077,-0.13,0.21,-4.6e-06,0.43,-0.00034,0.0008,6.2e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.5e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21390000,0.71,0.00088,-0.013,0.7,0.004,0.00034,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.013,0.00093,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00079,6.2e-05,0,0,-4.9e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0055,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21490000,0.71,0.00088,-0.013,0.7,0.0045,0.00077,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.013,0.00058,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00079,6.7e-05,0,0,-4.9e+02,9.4e-05,8.3e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21390000,0.71,0.00088,-0.013,0.7,0.004,0.00035,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.013,0.00093,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00079,6.2e-05,0,0,-4.9e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0055,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21490000,0.71,0.00088,-0.013,0.7,0.0045,0.00078,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.013,0.00058,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00079,6.7e-05,0,0,-4.9e+02,9.4e-05,8.3e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 21590000,0.71,0.00087,-0.013,0.7,0.0034,0.0013,0.012,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.013,0.00061,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00079,6.4e-05,0,0,-4.9e+02,9.1e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0054,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 21690000,0.71,0.00084,-0.013,0.7,0.005,0.0016,0.014,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.013,0.0002,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00079,6.4e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 21790000,0.71,0.00084,-0.013,0.7,0.0031,0.0037,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7e-05,0.013,0.00038,-0.13,0.21,-5.4e-06,0.43,-0.00033,0.00079,6.6e-05,0,0,-4.9e+02,9e-05,8.1e-05,0.036,0.012,0.015,0.0079,0.037,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0043,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.013,0.00029,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00079,6.4e-05,0,0,-4.9e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21990000,0.71,0.00085,-0.013,0.7,0.0026,0.005,0.014,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00012,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.5e-05,0,0,-4.9e+02,8.9e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.5e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22090000,0.71,0.00088,-0.013,0.7,0.0025,0.0065,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00017,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.5e-05,0,0,-4.9e+02,8.9e-05,8e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22190000,0.71,0.00085,-0.013,0.7,0.002,0.0065,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.4e-05,0.014,0.00023,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22290000,0.71,0.00088,-0.013,0.7,0.0013,0.0062,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.014,0.00029,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.00079,5.6e-05,0,0,-4.9e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +21990000,0.71,0.00085,-0.013,0.7,0.0026,0.005,0.014,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00012,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.6e-05,0,0,-4.9e+02,8.9e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.5e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22090000,0.71,0.00088,-0.013,0.7,0.0025,0.0065,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00017,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.6e-05,0,0,-4.9e+02,8.9e-05,8e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22190000,0.71,0.00085,-0.013,0.7,0.002,0.0065,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.5e-05,0.014,0.00023,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22290000,0.71,0.00088,-0.013,0.7,0.0013,0.0062,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.014,0.00028,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.00079,5.6e-05,0,0,-4.9e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 22390000,0.71,0.0009,-0.013,0.7,-0.00098,0.006,0.015,0,0,-4.9e+02,-0.0013,-0.0058,7.9e-05,0.014,0.00049,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.7e-05,0,0,-4.9e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22490000,0.71,0.00094,-0.013,0.7,-0.0021,0.0068,0.016,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.014,0.00063,-0.13,0.21,-5.4e-06,0.43,-0.00036,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22490000,0.71,0.00094,-0.013,0.7,-0.0021,0.0068,0.016,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.014,0.00063,-0.13,0.21,-5.4e-06,0.43,-0.00037,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 22590000,0.71,0.00096,-0.013,0.7,-0.0038,0.0062,0.015,0,0,-4.9e+02,-0.0014,-0.0058,8.3e-05,0.015,0.0008,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.00081,5.2e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 22690000,0.71,0.001,-0.013,0.7,-0.0051,0.0076,0.016,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.015,0.00069,-0.13,0.21,-5.3e-06,0.43,-0.00039,0.00082,5.1e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 22790000,0.71,0.001,-0.013,0.7,-0.0071,0.0065,0.017,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.015,0.0015,-0.13,0.21,-5.5e-06,0.43,-0.00038,0.00081,5.7e-05,0,0,-4.9e+02,8.3e-05,7.5e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.8e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 @@ -238,27 +238,27 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 23590000,0.71,0.0086,-0.0027,0.7,-0.027,0.0029,-0.045,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.014,0.0013,-0.13,0.21,-5.2e-06,0.43,-0.00034,0.00087,0.00013,0,0,-4.9e+02,7.9e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 23690000,0.71,0.0082,0.003,0.71,-0.058,-0.0049,-0.095,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.014,0.0013,-0.13,0.21,-5.2e-06,0.43,-0.00034,0.00081,0.0001,0,0,-4.9e+02,8e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0047,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 23790000,0.71,0.0053,-0.00025,0.71,-0.083,-0.017,-0.15,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.014,0.00079,-0.13,0.21,-4.5e-06,0.43,-0.0004,0.0008,0.00045,0,0,-4.9e+02,7.8e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23890000,0.71,0.0026,-0.0063,0.71,-0.1,-0.025,-0.2,0,0,-4.9e+02,-0.0013,-0.0058,9e-05,0.014,0.00093,-0.13,0.21,-4.4e-06,0.43,-0.00042,0.00086,0.00036,0,0,-4.9e+02,7.9e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23990000,0.71,0.0013,-0.011,0.71,-0.1,-0.029,-0.26,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.014,0.00094,-0.13,0.21,-4e-06,0.43,-0.0004,0.00086,0.00034,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24090000,0.71,0.0025,-0.0096,0.71,-0.1,-0.028,-0.3,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.014,0.00064,-0.13,0.21,-3.6e-06,0.43,-0.00042,0.00083,0.00037,0,0,-4.9e+02,7.8e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23890000,0.71,0.0026,-0.0063,0.71,-0.1,-0.025,-0.2,0,0,-4.9e+02,-0.0013,-0.0058,9e-05,0.014,0.00092,-0.13,0.21,-4.4e-06,0.43,-0.00042,0.00086,0.00036,0,0,-4.9e+02,7.9e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23990000,0.71,0.0013,-0.011,0.71,-0.1,-0.029,-0.26,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.014,0.00093,-0.13,0.21,-4e-06,0.43,-0.0004,0.00086,0.00034,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24090000,0.71,0.0025,-0.0096,0.71,-0.1,-0.028,-0.3,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.014,0.00063,-0.13,0.21,-3.6e-06,0.43,-0.00042,0.00083,0.00037,0,0,-4.9e+02,7.8e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 24190000,0.71,0.0036,-0.0073,0.71,-0.11,-0.03,-0.35,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.014,0.00052,-0.13,0.21,-2.9e-06,0.43,-0.00043,0.00086,0.00037,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 24290000,0.71,0.0041,-0.0065,0.71,-0.12,-0.033,-0.41,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.013,0.00049,-0.13,0.21,-2.6e-06,0.43,-0.00046,0.0009,0.00044,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 24390000,0.71,0.0042,-0.0067,0.71,-0.13,-0.041,-0.46,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.013,0.0011,-0.13,0.21,2.4e-07,0.43,-0.00034,0.00095,0.00043,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 24490000,0.71,0.0051,-0.0025,0.71,-0.14,-0.046,-0.51,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.013,0.0011,-0.13,0.21,2.4e-07,0.43,-0.00034,0.00096,0.00042,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 24590000,0.71,0.0055,0.0012,0.71,-0.16,-0.057,-0.56,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.013,0.0011,-0.13,0.21,1.3e-06,0.43,1.3e-05,0.00061,0.00037,0,0,-4.9e+02,7.4e-05,6.7e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 24690000,0.71,0.0056,0.0021,0.71,-0.18,-0.07,-0.64,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.013,0.00096,-0.13,0.21,2.3e-06,0.43,-3.2e-05,0.00065,0.00056,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24790000,0.71,0.0053,0.00083,0.71,-0.2,-0.084,-0.73,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.012,0.001,-0.13,0.21,1.6e-06,0.43,1.7e-07,0.00062,0.00032,0,0,-4.9e+02,7.3e-05,6.6e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24790000,0.71,0.0053,0.00083,0.71,-0.2,-0.084,-0.73,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.012,0.001,-0.13,0.21,1.6e-06,0.43,1.6e-07,0.00062,0.00032,0,0,-4.9e+02,7.3e-05,6.6e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 24890000,0.71,0.0071,0.0025,0.71,-0.22,-0.095,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.012,0.0011,-0.13,0.21,2.4e-06,0.43,-0.00011,0.00077,0.00034,0,0,-4.9e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24990000,0.71,0.0089,0.0043,0.71,-0.24,-0.1,-0.81,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.012,0.00071,-0.13,0.21,1.8e-06,0.43,-0.00019,0.00087,-3.6e-06,0,0,-4.9e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24990000,0.71,0.0089,0.0043,0.71,-0.24,-0.1,-0.81,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.012,0.0007,-0.13,0.21,1.8e-06,0.43,-0.00019,0.00087,-3.7e-06,0,0,-4.9e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 25090000,0.71,0.0092,0.0037,0.71,-0.27,-0.11,-0.86,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.012,0.0008,-0.13,0.21,1.4e-06,0.43,-0.0002,0.00088,-3.9e-05,0,0,-4.9e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25190000,0.71,0.0087,0.0023,0.71,-0.3,-0.13,-0.91,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.011,0.0011,-0.13,0.21,6.8e-06,0.43,5e-05,0.00085,9.7e-05,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25190000,0.71,0.0087,0.0023,0.71,-0.3,-0.13,-0.91,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.011,0.0011,-0.13,0.21,6.8e-06,0.43,5e-05,0.00085,9.6e-05,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 25290000,0.71,0.011,0.0091,0.71,-0.33,-0.14,-0.96,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.011,0.0011,-0.13,0.21,6.6e-06,0.43,7.6e-05,0.0008,0.0001,0,0,-4.9e+02,7.2e-05,6.5e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.011,0.00086,-0.13,0.21,1e-05,0.43,0.00048,0.00048,0.00014,0,0,-4.9e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,0.00014,0.011,0.00074,-0.13,0.21,9.2e-06,0.43,0.00065,0.00016,0.00033,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,0.00015,0.0099,0.0011,-0.13,0.21,1.5e-05,0.43,0.00095,0.00016,0.00034,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 25690000,0.71,0.015,0.022,0.71,-0.49,-0.23,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0099,0.0011,-0.13,0.21,1.6e-05,0.43,0.00094,0.00018,0.00042,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0092,0.00041,-0.13,0.21,1.9e-05,0.43,0.0013,-6.7e-05,-3e-05,0,0,-4.9e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25890000,0.71,0.018,0.028,0.71,-0.62,-0.29,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00017,0.0094,0.00034,-0.13,0.21,2.1e-05,0.43,0.0014,7.9e-07,-9.6e-05,0,0,-4.9e+02,7.1e-05,6.3e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25890000,0.71,0.018,0.028,0.71,-0.62,-0.29,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00017,0.0094,0.00034,-0.13,0.21,2.1e-05,0.43,0.0014,7.5e-07,-9.6e-05,0,0,-4.9e+02,7.1e-05,6.3e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 25990000,0.7,0.017,0.025,0.71,-0.67,-0.32,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0085,0.00068,-0.13,0.21,2.8e-05,0.43,0.0023,-0.00056,-0.00055,0,0,-4.9e+02,7e-05,6.2e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0086,0.00085,-0.13,0.21,2.4e-05,0.43,0.0024,-0.00048,-0.0012,0,0,-4.9e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0075,-0.00028,-0.13,0.21,3.8e-05,0.43,0.0023,0.00041,-0.0014,0,0,-4.9e+02,7.1e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 @@ -285,8 +285,8 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0021,-0.00091,-0.12,0.21,-0.0001,0.44,-0.0075,0.0013,-0.0069,0,0,-4.9e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,0,0,-4.9e+02,-0.00094,-0.0059,0.00023,-0.002,-0.00063,-0.12,0.21,-8.9e-05,0.44,-0.0076,0.0013,-0.007,0,0,-4.9e+02,8.3e-05,6.2e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 28490000,0.73,0.0028,0.0059,0.69,-2.8,-1.2,1.1,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0016,-0.00036,-0.12,0.21,-7e-05,0.44,-0.0077,0.0014,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28590000,0.73,0.00089,0.0024,0.69,-2.7,-1.2,0.97,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0017,-0.00031,-0.12,0.21,-7.3e-05,0.44,-0.0076,0.0015,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28690000,0.73,0.00019,0.0015,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0014,4.3e-05,-0.12,0.21,-5.6e-05,0.44,-0.0077,0.0014,-0.0068,0,0,-4.9e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 +28590000,0.73,0.00089,0.0024,0.69,-2.7,-1.2,0.97,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0017,-0.0003,-0.12,0.21,-7.3e-05,0.44,-0.0076,0.0015,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28690000,0.73,0.00019,0.0015,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0014,4.4e-05,-0.12,0.21,-5.6e-05,0.44,-0.0077,0.0014,-0.0068,0,0,-4.9e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 28790000,0.73,-1.2e-05,0.0014,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00098,-0.0059,0.00024,-0.00092,0.00026,-0.12,0.21,-9.6e-05,0.44,-0.0092,0.0006,-0.006,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 28890000,0.73,-2.3e-06,0.0016,0.69,-2.5,-1.2,0.97,0,0,-4.9e+02,-0.00099,-0.0059,0.00024,-0.00064,0.00058,-0.12,0.21,-8e-05,0.44,-0.0093,0.00056,-0.0059,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 28990000,0.73,0.00035,0.0022,0.68,-2.5,-1.1,0.97,0,0,-4.9e+02,-0.001,-0.0059,0.00025,0.00066,0.001,-0.12,0.21,-0.00011,0.44,-0.011,-0.00036,-0.0047,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.0003,0.00039,0.00039,1,1,0.01 @@ -313,7 +313,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 31090000,0.73,0.0027,0.0047,0.68,-1.8,-1,0.79,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0069,0.0025,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00041,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 31190000,0.73,0.0026,0.0043,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.5e-05,0.0072,0.0024,-0.12,0.21,-0.00027,0.43,-0.011,-0.0028,-0.00024,0,0,-4.9e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 31290000,0.73,0.0023,0.0037,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.8e-05,0.0069,0.0028,-0.11,0.21,-0.00028,0.43,-0.011,-0.0027,-0.00027,0,0,-4.9e+02,8.2e-05,6e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00035,0.00037,1,1,0.01 -31390000,0.73,0.0022,0.003,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.0074,0.0026,-0.11,0.21,-0.00031,0.43,-0.011,-0.0028,-2e-05,0,0,-4.9e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31390000,0.73,0.0022,0.003,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.0074,0.0026,-0.11,0.21,-0.00031,0.43,-0.011,-0.0028,-1.9e-05,0,0,-4.9e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 31490000,0.73,0.0019,0.0024,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.0073,0.0032,-0.11,0.2,-0.0003,0.43,-0.011,-0.0029,1.8e-05,0,0,-4.9e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 31590000,0.73,0.002,0.0019,0.68,-1.7,-0.97,0.77,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.0082,0.0028,-0.11,0.2,-0.00029,0.43,-0.01,-0.0029,0.00026,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 31690000,0.73,0.0017,0.0012,0.68,-1.6,-0.97,0.78,0,0,-4.9e+02,-0.0013,-0.0058,4.7e-05,0.0079,0.0031,-0.11,0.2,-0.0003,0.43,-0.01,-0.0029,0.00022,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 @@ -322,7 +322,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 31990000,0.73,0.0012,-0.00093,0.69,-1.6,-0.93,0.77,0,0,-4.9e+02,-0.0013,-0.0058,-8.8e-06,0.0093,0.0034,-0.11,0.2,-0.0003,0.43,-0.0094,-0.003,0.00076,0,0,-4.9e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 32090000,0.73,0.00083,-0.0017,0.69,-1.5,-0.93,0.78,0,0,-4.9e+02,-0.0013,-0.0058,-9.7e-06,0.0091,0.0039,-0.11,0.2,-0.00029,0.43,-0.0094,-0.003,0.00077,0,0,-4.9e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 32190000,0.73,0.00063,-0.0026,0.69,-1.5,-0.91,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-4.4e-05,0.0096,0.0039,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.6e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32290000,0.73,0.00035,-0.0034,0.69,-1.5,-0.91,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-4.3e-05,0.0094,0.0045,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00035,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32290000,0.73,0.00035,-0.0034,0.69,-1.5,-0.91,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-4.4e-05,0.0094,0.0045,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00035,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 32390000,0.73,0.00026,-0.0041,0.69,-1.5,-0.89,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-6.2e-05,0.0099,0.0044,-0.11,0.2,-0.0003,0.43,-0.0085,-0.0031,0.0012,0,0,-4.9e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 32490000,0.72,0.00011,-0.0044,0.69,-1.4,-0.88,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-6e-05,0.0097,0.0048,-0.11,0.2,-0.0003,0.43,-0.0085,-0.0031,0.0011,0,0,-4.9e+02,7.2e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 32590000,0.72,0.00016,-0.0047,0.69,-1.4,-0.87,0.78,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.01,0.0048,-0.11,0.2,-0.00031,0.43,-0.0081,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 @@ -336,7 +336,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 33390000,0.56,0.014,-0.0038,0.83,-1.2,-0.77,0.88,0,0,-4.9e+02,-0.0014,-0.0057,-0.00013,0.011,0.0067,-0.11,0.2,-0.00035,0.43,-0.0064,-0.0032,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 33490000,0.43,0.007,-0.0013,0.9,-1.2,-0.76,0.89,0,0,-4.9e+02,-0.0014,-0.0057,-0.00015,0.011,0.0068,-0.11,0.21,-0.00044,0.43,-0.0059,-0.002,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 33590000,0.27,0.00087,-0.0037,0.96,-1.2,-0.75,0.86,0,0,-4.9e+02,-0.0014,-0.0057,-0.00018,0.011,0.0068,-0.11,0.21,-0.00068,0.43,-0.0039,-0.0014,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 -33690000,0.098,-0.0027,-0.0067,1,-1.1,-0.74,0.87,0,0,-4.9e+02,-0.0014,-0.0057,-0.00019,0.011,0.0068,-0.11,0.21,-0.00074,0.43,-0.0036,-0.001,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 +33690000,0.098,-0.0027,-0.0067,1,-1.1,-0.74,0.87,0,0,-4.9e+02,-0.0014,-0.0057,-0.00019,0.011,0.0068,-0.11,0.21,-0.00074,0.43,-0.0036,-0.001,0.0021,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 33790000,-0.074,-0.0046,-0.0085,1,-1.1,-0.72,0.85,0,0,-4.9e+02,-0.0014,-0.0057,-0.00021,0.011,0.0068,-0.11,0.21,-0.0009,0.43,-0.0021,-0.001,0.0023,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 33890000,-0.24,-0.006,-0.0091,0.97,-0.99,-0.69,0.83,0,0,-4.9e+02,-0.0014,-0.0057,-0.00022,0.011,0.0068,-0.11,0.21,-0.001,0.43,-0.0013,-0.0011,0.0024,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 33990000,-0.39,-0.0048,-0.012,0.92,-0.94,-0.64,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.011,0.0069,-0.11,0.21,-0.001,0.43,-0.0011,-0.00064,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 @@ -371,7 +371,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 36890000,-0.68,-0.013,-0.0038,0.74,1.2,1.2,-0.041,0,0,-4.9e+02,-0.0016,-0.0058,-9.5e-05,-0.013,0.037,-0.11,0.21,-0.001,0.43,0.0002,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0057,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00036,1,1,0.3 36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00036,1,1,0.33 37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00036,1,1,0.35 -37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.6e-05,8.6e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.38 +37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.38 37290000,-0.68,-0.013,-0.0037,0.74,1.3,1.3,-0.019,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00024,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.41 37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00026,0.00027,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00036,1,1,0.43 37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0088,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00036,1,1,0.46 @@ -386,6 +386,6 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 38390000,-0.68,-0.014,-0.0035,0.74,1.7,1.8,0.052,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.6e-05,0.00096,0.28,0.33,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00036,1,1,0.69 38490000,-0.68,-0.014,-0.0035,0.74,1.7,1.8,0.058,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00036,1,1,0.72 38590000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.064,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00032,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00036,1,1,0.75 -38690000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.069,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00036,1,1,0.77 +38690000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.069,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.77 38790000,-0.68,-0.014,-0.0034,0.74,1.8,2,0.075,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.0004,0.00031,0.0038,0,0,-4.9e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index f83bc0ad462c..211fc9fcd09a 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -121,7 +121,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 11890000,0.98,-0.0077,-0.012,0.18,0.0037,0.0014,0.017,0,0,-4.9e+02,-0.001,-0.0059,-0.00012,-0.00021,0.0003,-0.14,0.2,-2.5e-06,0.43,-2.3e-05,-0.0001,-0.00026,0,0,-4.9e+02,0.0012,0.00096,0.039,0.066,0.082,0.036,0.056,0.058,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 11990000,0.98,-0.0077,-0.012,0.18,0.0065,0.0036,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00027,0.00047,-0.14,0.2,-2.8e-06,0.43,-4.1e-05,-0.00011,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.057,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 12090000,0.98,-0.0077,-0.012,0.18,0.0096,0.0012,0.017,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00045,0.00022,-0.14,0.2,-2.5e-06,0.43,-4.1e-05,-7.5e-05,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.064,0.079,0.029,0.055,0.056,0.06,1e-05,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12190000,0.98,-0.0077,-0.012,0.18,0.011,-5e-06,0.016,0,0,-4.9e+02,-0.00097,-0.0058,-0.00012,-0.0011,-0.00042,-0.14,0.2,-2.4e-06,0.43,-6.2e-06,-4e-05,-0.00028,0,0,-4.9e+02,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12190000,0.98,-0.0077,-0.012,0.18,0.011,-4.8e-06,0.016,0,0,-4.9e+02,-0.00097,-0.0058,-0.00012,-0.0011,-0.00042,-0.14,0.2,-2.4e-06,0.43,-6.2e-06,-4e-05,-0.00028,0,0,-4.9e+02,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 12290000,0.98,-0.0079,-0.012,0.18,0.0079,-0.0035,0.015,0,0,-4.9e+02,-0.00096,-0.0058,-0.00013,-0.0014,-0.00025,-0.14,0.2,-2.2e-06,0.43,2e-05,-4.9e-05,-0.00028,0,0,-4.9e+02,0.00095,0.00081,0.039,0.062,0.075,0.024,0.054,0.056,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 12390000,0.98,-0.0079,-0.012,0.18,0.0078,-0.0047,0.013,0,0,-4.9e+02,-0.00095,-0.0058,-0.00013,-0.0017,-0.00072,-0.14,0.2,-2.2e-06,0.43,4.6e-05,-2.8e-05,-0.0003,0,0,-4.9e+02,0.00085,0.00075,0.039,0.053,0.062,0.022,0.047,0.048,0.056,6.9e-06,5.6e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 12490000,0.98,-0.008,-0.012,0.18,0.0094,-0.0062,0.017,0,0,-4.9e+02,-0.00093,-0.0058,-0.00013,-0.002,-0.00096,-0.14,0.2,-2.1e-06,0.43,5.3e-05,1.9e-06,-0.00031,0,0,-4.9e+02,0.00085,0.00075,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.8e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 @@ -129,7 +129,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 12690000,0.98,-0.0081,-0.012,0.18,0.012,-0.014,0.018,0,0,-4.9e+02,-0.00091,-0.0058,-0.00013,-0.0023,-0.00078,-0.14,0.2,-1.9e-06,0.43,7.5e-05,5.9e-06,-0.00033,0,0,-4.9e+02,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.055,0.054,5.7e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.02,0,0,-4.9e+02,-0.00093,-0.0058,-0.00013,-0.0016,-0.0012,-0.14,0.2,-2e-06,0.43,3.9e-05,3e-05,-0.0003,0,0,-4.9e+02,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4.1e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 12890000,0.98,-0.0081,-0.012,0.18,0.015,-0.013,0.021,0,0,-4.9e+02,-0.00094,-0.0058,-0.00013,-0.0012,-0.0014,-0.14,0.2,-2e-06,0.43,1.9e-05,3.4e-05,-0.00029,0,0,-4.9e+02,0.00071,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0,0,-4.9e+02,-0.00098,-0.0058,-0.00012,-0.0011,-0.0015,-0.14,0.2,-2.5e-06,0.43,3.2e-05,9.2e-06,-0.00031,0,0,-4.9e+02,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0,0,-4.9e+02,-0.00098,-0.0058,-0.00012,-0.0011,-0.0015,-0.14,0.2,-2.5e-06,0.43,3.2e-05,9.3e-06,-0.00031,0,0,-4.9e+02,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00055,-0.0017,-0.14,0.2,-2.6e-06,0.43,8.6e-06,1.4e-05,-0.0003,0,0,-4.9e+02,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 13190000,0.98,-0.0079,-0.012,0.18,0.0096,-0.0078,0.017,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00031,-0.0022,-0.14,0.2,-2.7e-06,0.43,3.1e-05,1.2e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00051,-0.0027,-0.14,0.2,-2.6e-06,0.43,3.9e-05,3.2e-05,-0.0003,0,0,-4.9e+02,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.6e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 @@ -192,7 +192,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 18990000,0.98,-0.0087,-0.011,0.18,0.024,-0.019,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0068,-0.019,-0.13,0.2,-5.8e-06,0.43,0.00036,0.00025,-0.00022,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 19090000,0.98,-0.0088,-0.011,0.18,0.023,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.3e-05,0.0074,-0.019,-0.13,0.2,-5.7e-06,0.43,0.00035,0.00025,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 19190000,0.98,-0.0087,-0.011,0.18,0.02,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19290000,0.98,-0.0087,-0.011,0.18,0.021,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19290000,0.98,-0.0087,-0.011,0.18,0.021,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.4e-06,0.43,0.00037,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.017,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0079,-0.019,-0.13,0.2,-6.9e-06,0.43,0.00036,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 19490000,0.98,-0.0089,-0.011,0.18,0.021,-0.019,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0073,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00024,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 19590000,0.98,-0.0088,-0.011,0.18,0.018,-0.02,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0076,-0.02,-0.13,0.2,-6.8e-06,0.43,0.00037,0.00024,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 @@ -247,19 +247,19 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 24490000,0.98,-0.011,-0.015,0.18,0.073,-0.071,0.081,0,0,-4.9e+02,-0.0013,-0.0058,-2e-05,0.023,-0.02,-0.13,0.2,-6.5e-06,0.43,7.6e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 24590000,0.98,-0.012,-0.015,0.18,0.069,-0.067,0.077,0,0,-4.9e+02,-0.0013,-0.0058,-3.2e-05,0.024,-0.021,-0.13,0.2,-5.2e-06,0.43,0.00012,0.00044,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 24690000,0.98,-0.012,-0.015,0.18,0.067,-0.067,0.076,0,0,-4.9e+02,-0.0013,-0.0058,-3e-05,0.024,-0.021,-0.13,0.2,-5.4e-06,0.43,0.0001,0.00047,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24790000,0.98,-0.012,-0.015,0.18,0.064,-0.065,0.068,0,0,-4.9e+02,-0.0013,-0.0058,-3.8e-05,0.026,-0.022,-0.13,0.2,-4.8e-06,0.43,9.9e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24790000,0.98,-0.012,-0.015,0.18,0.064,-0.065,0.068,0,0,-4.9e+02,-0.0013,-0.0058,-3.8e-05,0.026,-0.022,-0.13,0.2,-4.8e-06,0.43,9.8e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 24890000,0.98,-0.012,-0.014,0.18,0.063,-0.068,0.057,0,0,-4.9e+02,-0.0013,-0.0058,-3.3e-05,0.026,-0.022,-0.13,0.2,-4.6e-06,0.43,9.6e-05,0.00048,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 24990000,0.98,-0.012,-0.014,0.18,0.054,-0.065,0.05,0,0,-4.9e+02,-0.0014,-0.0058,-4.7e-05,0.027,-0.023,-0.13,0.2,-4.8e-06,0.43,6e-05,0.00059,0.00027,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25090000,0.98,-0.012,-0.014,0.18,0.051,-0.064,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-4.7e-05,0.028,-0.023,-0.13,0.2,-5.4e-06,0.43,4.1e-05,0.00066,0.00031,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25190000,0.98,-0.012,-0.014,0.18,0.045,-0.058,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-6.5e-05,0.029,-0.024,-0.13,0.2,-5.4e-06,0.43,1.9e-05,0.00069,0.0003,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25290000,0.98,-0.012,-0.013,0.18,0.041,-0.06,0.043,0,0,-4.9e+02,-0.0014,-0.0058,-7.2e-05,0.029,-0.024,-0.13,0.2,-6e-06,0.43,1e-05,0.00072,0.00027,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25390000,0.98,-0.012,-0.013,0.18,0.032,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-8.8e-05,0.031,-0.025,-0.13,0.2,-6.9e-06,0.43,-3e-05,0.00077,0.00029,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25390000,0.98,-0.012,-0.013,0.18,0.032,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-8.8e-05,0.031,-0.025,-0.13,0.2,-6.8e-06,0.43,-3e-05,0.00077,0.00029,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25490000,0.98,-0.013,-0.013,0.18,0.028,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9e-05,0.031,-0.025,-0.13,0.2,-6.6e-06,0.43,-3.6e-05,0.00073,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25590000,0.98,-0.013,-0.013,0.18,0.023,-0.05,0.042,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.032,-0.026,-0.13,0.2,-7.4e-06,0.43,-6.9e-05,0.00075,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25690000,0.98,-0.012,-0.012,0.18,0.023,-0.05,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,0.032,-0.026,-0.13,0.2,-7.5e-06,0.43,-6.4e-05,0.00078,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25790000,0.98,-0.012,-0.012,0.18,0.011,-0.041,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.033,-0.026,-0.13,0.2,-8e-06,0.43,-0.00012,0.00071,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 25890000,0.98,-0.012,-0.012,0.18,0.0057,-0.04,0.033,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.033,-0.026,-0.13,0.2,-8.2e-06,0.43,-0.00014,0.00068,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25990000,0.98,-0.012,-0.012,0.18,-0.0033,-0.033,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.035,-0.027,-0.13,0.2,-9.7e-06,0.43,-0.0002,0.00066,0.00017,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25990000,0.98,-0.012,-0.012,0.18,-0.0033,-0.033,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.035,-0.027,-0.13,0.2,-9.6e-06,0.43,-0.0002,0.00066,0.00017,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 26090000,0.98,-0.012,-0.012,0.18,-0.0079,-0.033,0.025,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.027,-0.13,0.2,-9.1e-06,0.43,-0.0002,0.00065,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.026,0.021,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.027,-0.13,0.2,-9.5e-06,0.43,-0.00022,0.00064,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.026,0.015,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.027,-0.13,0.2,-1e-05,0.43,-0.00023,0.00066,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 @@ -290,16 +290,16 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.036,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00037,0.00013,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 28890000,0.98,-0.0074,-0.014,0.18,-0.08,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00037,0.00017,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 28990000,0.98,-0.0072,-0.014,0.18,-0.077,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-2e-05,0.43,-0.00041,1.5e-05,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29090000,0.98,-0.007,-0.014,0.18,-0.08,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00043,-4e-06,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29090000,0.98,-0.007,-0.014,0.18,-0.08,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00043,-4.3e-06,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.034,-0.029,-0.12,0.2,-2.1e-05,0.43,-0.00046,-0.00014,0.00028,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.034,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00045,-0.00016,0.00028,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29390000,0.98,-0.0076,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.034,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00049,-0.00029,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29490000,0.98,-0.0076,-0.013,0.18,-0.08,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-9.9e-05,0.034,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00051,-0.00026,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-8.2e-05,0.033,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00056,-0.00037,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-7.5e-05,0.033,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00057,-0.00033,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-5.9e-05,0.032,-0.029,-0.12,0.2,-2.6e-05,0.43,-0.00063,-0.00045,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-5.9e-05,0.032,-0.028,-0.12,0.2,-2.6e-05,0.43,-0.00063,-0.00045,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 29890000,0.98,-0.0068,-0.014,0.18,-0.08,0.055,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.3e-05,0.032,-0.028,-0.12,0.2,-2.6e-05,0.43,-0.00065,-0.00043,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29990000,0.98,-0.007,-0.014,0.18,-0.075,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-4.2e-05,0.031,-0.029,-0.12,0.2,-2.7e-05,0.43,-0.00063,-0.00048,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29990000,0.98,-0.007,-0.014,0.18,-0.075,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-4.1e-05,0.031,-0.029,-0.12,0.2,-2.7e-05,0.43,-0.00063,-0.00048,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 30090000,0.98,-0.0071,-0.014,0.18,-0.077,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.2e-05,0.031,-0.029,-0.12,0.2,-2.8e-05,0.43,-0.00058,-0.00055,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 30190000,0.98,-0.0071,-0.014,0.18,-0.071,0.047,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.03,-0.029,-0.12,0.2,-3.1e-05,0.43,-0.00059,-0.00077,0.00036,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 30290000,0.98,-0.0071,-0.014,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.3e-05,0.03,-0.029,-0.12,0.2,-3e-05,0.43,-0.00063,-0.00082,0.00037,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 @@ -307,14 +307,14 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 30490000,0.98,-0.0071,-0.014,0.18,-0.068,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-2.9e-05,0.03,-0.03,-0.12,0.2,-3.1e-05,0.43,-0.00081,-0.001,0.00035,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 30590000,0.98,-0.0074,-0.014,0.17,-0.064,0.041,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.4e-05,0.029,-0.03,-0.12,0.2,-3.2e-05,0.43,-0.00092,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 30690000,0.98,-0.0078,-0.014,0.18,-0.062,0.039,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.4e-05,0.029,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00089,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30790000,0.98,-0.0075,-0.014,0.17,-0.056,0.034,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-9.6e-06,0.029,-0.031,-0.12,0.2,-3.3e-05,0.43,-0.00097,-0.0013,0.00039,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30790000,0.98,-0.0075,-0.014,0.17,-0.056,0.034,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-9.5e-06,0.029,-0.031,-0.12,0.2,-3.3e-05,0.43,-0.00097,-0.0013,0.00039,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 30890000,0.98,-0.0069,-0.014,0.17,-0.056,0.03,0.79,0,0,-4.9e+02,-0.0014,-0.0058,-1.6e-05,0.029,-0.031,-0.12,0.2,-3.4e-05,0.43,-0.00087,-0.0012,0.00041,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30990000,0.98,-0.0071,-0.013,0.17,-0.048,0.025,0.8,0,0,-4.9e+02,-0.0013,-0.0057,-8.9e-06,0.028,-0.032,-0.12,0.2,-3.5e-05,0.43,-0.00091,-0.0013,0.00044,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30990000,0.98,-0.0071,-0.013,0.17,-0.048,0.025,0.8,0,0,-4.9e+02,-0.0013,-0.0057,-8.8e-06,0.028,-0.032,-0.12,0.2,-3.5e-05,0.43,-0.00091,-0.0013,0.00044,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 31090000,0.98,-0.0073,-0.014,0.17,-0.048,0.024,0.79,0,0,-4.9e+02,-0.0013,-0.0057,-1.3e-05,0.028,-0.033,-0.12,0.2,-3.5e-05,0.43,-0.00086,-0.0013,0.00045,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31190000,0.98,-0.0075,-0.014,0.17,-0.043,0.02,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.6e-06,0.028,-0.033,-0.12,0.2,-3.6e-05,0.43,-0.00096,-0.0013,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.017,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.7e-06,0.029,-0.034,-0.12,0.2,-3.5e-05,0.43,-0.001,-0.0012,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31190000,0.98,-0.0075,-0.014,0.17,-0.043,0.02,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.7e-06,0.028,-0.033,-0.12,0.2,-3.6e-05,0.43,-0.00096,-0.0013,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.017,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.8e-06,0.029,-0.034,-0.12,0.2,-3.5e-05,0.43,-0.001,-0.0012,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.1e-06,0.028,-0.034,-0.12,0.2,-3.4e-05,0.43,-0.0011,-0.0016,0.00048,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0078,0.8,0,0,-4.9e+02,-0.0013,-0.0057,5.4e-06,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0011,-0.0018,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0078,0.8,0,0,-4.9e+02,-0.0013,-0.0057,5.5e-06,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0011,-0.0018,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 31590000,0.98,-0.0071,-0.014,0.17,-0.032,0.0059,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.5e-05,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0013,-0.0019,0.00052,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 31690000,0.98,-0.0071,-0.015,0.17,-0.036,0.0053,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0014,-0.0019,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 31790000,0.99,-0.0074,-0.015,0.17,-0.026,0.0027,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.8e-05,0.028,-0.034,-0.12,0.2,-3.2e-05,0.43,-0.0015,-0.002,0.00053,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 @@ -324,14 +324,14 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 32190000,0.99,-0.008,-0.015,0.17,-0.013,-0.0076,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.029,-0.035,-0.12,0.2,-3e-05,0.43,-0.0016,-0.0025,0.00067,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 32290000,0.99,-0.0079,-0.015,0.17,-0.012,-0.01,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3e-05,0.029,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0017,-0.0026,0.00068,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 32390000,0.99,-0.008,-0.015,0.17,-0.0061,-0.012,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.7e-05,0.03,-0.035,-0.12,0.2,-2.8e-05,0.43,-0.0017,-0.0028,0.00074,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32490000,0.99,-0.011,-0.013,0.17,0.019,-0.078,-0.076,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0028,0.00073,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32490000,0.99,-0.011,-0.013,0.17,0.019,-0.078,-0.076,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0028,0.00073,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 32590000,0.99,-0.011,-0.013,0.17,0.022,-0.079,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0024,0.0008,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32690000,0.99,-0.011,-0.013,0.17,0.017,-0.085,-0.08,0,0,-4.9e+02,-0.0013,-0.0057,2.1e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0025,0.00079,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32690000,0.99,-0.011,-0.013,0.17,0.017,-0.085,-0.08,0,0,-4.9e+02,-0.0013,-0.0057,2.2e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0025,0.00079,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 32790000,0.99,-0.011,-0.013,0.17,0.018,-0.084,-0.081,0,0,-4.9e+02,-0.0013,-0.0057,2e-05,0.03,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0016,-0.0023,0.00086,0,0,-4.9e+02,0.00028,0.00027,0.035,0.011,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.03,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0016,-0.0022,0.00088,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.03,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0017,-0.0021,0.00094,0,0,-4.9e+02,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 33090000,0.98,-0.011,-0.013,0.17,0.013,-0.094,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.03,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0017,-0.0021,0.00094,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,1.6e-05,0.031,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0016,-0.0021,0.00096,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,1.7e-05,0.031,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0016,-0.0021,0.00096,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 33290000,0.99,-0.01,-0.013,0.17,0.0078,-0.096,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,2.7e-05,0.031,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0018,-0.0022,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 33390000,0.98,-0.01,-0.013,0.17,0.0056,-0.091,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,2e-05,0.033,-0.033,-0.12,0.2,-2e-05,0.43,-0.0018,-0.0021,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 33490000,0.99,-0.01,-0.013,0.17,0.0003,-0.094,-0.075,0,0,-4.9e+02,-0.0014,-0.0057,2.5e-05,0.033,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0019,-0.0023,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 @@ -347,5 +347,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 34490000,0.98,-0.0095,-0.014,0.17,-0.024,-0.089,-0.052,0,0,-4.9e+02,-0.0014,-0.0056,2.2e-05,0.041,-0.031,-0.12,0.2,-5.4e-06,0.43,-0.0019,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.31 34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.082,0.74,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.043,-0.03,-0.12,0.2,-2.9e-06,0.43,-0.0018,-0.0015,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 34690000,0.98,-0.0094,-0.013,0.17,-0.031,-0.08,1.7,0,0,-4.9e+02,-0.0014,-0.0056,1.8e-05,0.043,-0.031,-0.12,0.2,-2.6e-06,0.43,-0.0019,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 -34790000,0.98,-0.0093,-0.013,0.17,-0.034,-0.071,2.7,0,0,-4.9e+02,-0.0014,-0.0056,1.2e-05,0.045,-0.031,-0.12,0.2,6.3e-08,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 -34890000,0.98,-0.0092,-0.013,0.17,-0.04,-0.069,3.7,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.045,-0.031,-0.12,0.2,2.3e-07,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 +34790000,0.98,-0.0093,-0.013,0.17,-0.034,-0.071,2.7,0,0,-4.9e+02,-0.0014,-0.0056,1.2e-05,0.045,-0.031,-0.12,0.2,7.2e-08,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 +34890000,0.98,-0.0092,-0.013,0.17,-0.04,-0.069,3.7,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.045,-0.031,-0.12,0.2,2.4e-07,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 From 8a9391321dc79f4eea9f3fd97f5ebb172fdb0fdf Mon Sep 17 00:00:00 2001 From: Perre Date: Wed, 18 Dec 2024 16:25:12 +0100 Subject: [PATCH 171/211] Adding gimbal rate in gz simulation (#24125) * Adding gimbal rate in gz simulation * add submodule --- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZGimbal.cpp | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index f48a17a4b306..75318c05c0c8 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit f48a17a4b30601ebb11e2f9e3e678fec8a73ebdc +Subproject commit 75318c05c0c84d98ac786963d3c333f3bde0d718 diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index e016098fce8a..976200020e7e 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -104,6 +104,14 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) IMU_data.orientation().z()); _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); + matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(), + IMU_data.angular_velocity().y(), + IMU_data.angular_velocity().z())); + + _gimbal_rate[0] = rate(0); + _gimbal_rate[1] = rate(1); + _gimbal_rate[2] = rate(2); + pthread_mutex_unlock(&_node_mutex); } From ef823e5bb162fdea499163dfd9844eab5efb0438 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Dec 2024 17:36:01 +0100 Subject: [PATCH 172/211] gz submodule: switch to hash on submodule main branch after https://github.com/PX4/PX4-gazebo-models/pull/71 was merged --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 75318c05c0c8..19fd9248c499 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 75318c05c0c84d98ac786963d3c333f3bde0d718 +Subproject commit 19fd9248c499b05975b5b57ad72a07826834c1ab From 491ee01ac4dc0447bf8a9ebf2b4664490ef1cf2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 4 Mar 2024 19:14:16 +0100 Subject: [PATCH 173/211] rpm_capture: add rpm capture driver, publising pwm_input (for now) --- ROMFS/px4fmu_common/init.d/rcS | 5 + src/drivers/rpm_capture/CMakeLists.txt | 47 +++++ src/drivers/rpm_capture/Kconfig | 5 + src/drivers/rpm_capture/RPMCapture.cpp | 179 +++++++++++++++++++ src/drivers/rpm_capture/RPMCapture.hpp | 79 ++++++++ src/drivers/rpm_capture/rpm_capture_params.c | 43 +++++ src/lib/mixer_module/output_functions.yaml | 6 + 7 files changed, 364 insertions(+) create mode 100644 src/drivers/rpm_capture/CMakeLists.txt create mode 100644 src/drivers/rpm_capture/Kconfig create mode 100644 src/drivers/rpm_capture/RPMCapture.cpp create mode 100644 src/drivers/rpm_capture/RPMCapture.hpp create mode 100644 src/drivers/rpm_capture/rpm_capture_params.c diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3352cddad659..5cdd34e69898 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -354,6 +354,11 @@ else then pps_capture start fi + # RPM capture driver + if param greater -s RPM_CAP_ENABLE 0 + then + rpm_capture start + fi # Camera capture driver if param greater -s CAM_CAP_FBACK 0 then diff --git a/src/drivers/rpm_capture/CMakeLists.txt b/src/drivers/rpm_capture/CMakeLists.txt new file mode 100644 index 000000000000..db52504b4fcb --- /dev/null +++ b/src/drivers/rpm_capture/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + +px4_add_module( + MODULE drivers__rpm_capture + MAIN rpm_capture + COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" + SRCS + RPMCapture.cpp + ) diff --git a/src/drivers/rpm_capture/Kconfig b/src/drivers/rpm_capture/Kconfig new file mode 100644 index 000000000000..6f9f074fea7c --- /dev/null +++ b/src/drivers/rpm_capture/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RPM_CAPTURE + bool "rpm_capture" + default n + ---help--- + Enable support for rpm_capture diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp new file mode 100644 index 000000000000..397f6060c97e --- /dev/null +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include "RPMCapture.hpp" +#include +#include +#include +#include +#include + +RPMCapture::RPMCapture() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ + _pwm_input_pub.advertise(); +} + +RPMCapture::~RPMCapture() +{ + if (_channel >= 0) { + io_timer_unallocate_channel(_channel); + px4_arch_gpiosetevent(_rpm_capture_gpio, false, false, false, nullptr, nullptr); + } +} + +bool RPMCapture::init() +{ + bool success = false; + + for (unsigned i = 0; i < 16; ++i) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); + param_t function_handle = param_find(param_name); + int32_t function; + + if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { + if (function == 2070) { // RPM_Input + _channel = i; + } + } + } + + if (_channel == -1) { + PX4_WARN("No RPM channel configured"); + return false; + } + + int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_Other); // TODO: add IOTimerChanMode_RPM + + if (ret != PX4_OK) { + PX4_ERR("gpio alloc failed (%i) for RPM at channel (%d)", ret, _channel); + return false; + } + + _rpm_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel)); + int ret_val = px4_arch_gpiosetevent(_rpm_capture_gpio, true, false, true, &RPMCapture::gpio_interrupt_callback, this); + + if (ret_val == PX4_OK) { + success = true; + } + + return success; +} + +void RPMCapture::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + pwm_input_s pwm_input{}; + pwm_input.timestamp = hrt_absolute_time(); + pwm_input.period = _hrt_timestamp - _hrt_timestamp_prev; + pwm_input.error_count = _error_count; + _hrt_timestamp_prev = _hrt_timestamp; + _value_processed.store(true); + _pwm_input_pub.publish(pwm_input); +} + +int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg) +{ + RPMCapture *instance = static_cast(arg); + + if (!instance->_value_processed.load()) { + ++instance->_error_count; + } + + instance->_hrt_timestamp = hrt_absolute_time(); + instance->_value_processed.store(false); + instance->ScheduleNow(); + + return PX4_OK; +} + +int RPMCapture::task_spawn(int argc, char *argv[]) +{ + RPMCapture *instance = new RPMCapture(); + + 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 RPMCapture::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RPMCapture::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_USAGE_NAME("rpm_capture", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +void RPMCapture::stop() +{ + exit_and_cleanup(); +} + +extern "C" __EXPORT int rpm_capture_main(int argc, char *argv[]) +{ + if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running()) { + RPMCapture::stop(); + } + + return RPMCapture::main(argc, argv); +} diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp new file mode 100644 index 000000000000..b386c46ac79e --- /dev/null +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 RPMCapture : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + RPMCapture(); + virtual ~RPMCapture(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + static int gpio_interrupt_callback(int irq, void *context, void *arg); + + /** RPMCapture is an interrupt-driven task and needs to be manually stopped */ + static void stop(); + +private: + void Run() override; + + int _channel{-1}; + uint32_t _rpm_capture_gpio{0}; + uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; + + hrt_abstime _hrt_timestamp{0}; + hrt_abstime _hrt_timestamp_prev{0}; + uint32_t _error_count{0}; + px4::atomic _value_processed{true}; +}; diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c new file mode 100644 index 000000000000..7397177370c0 --- /dev/null +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * RPM Capture Enable + * + * Enables the RPM capture module on FMU channel 5. + * + * @boolean + * @group System + * @reboot_required true + */ +PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0); diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 7543ed388bce..a9965a5454ac 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -60,3 +60,9 @@ functions: condition: "PPS_CAP_ENABLE==0" text: "PPS needs to be enabled via PPS_CAP_ENABLE parameter." exclude_from_actuator_testing: true + RPM_Input: + start: 2070 + note: + condition: "RPM_CAP_ENABLE==0" + text: "RPM needs to be enabled via RPM_CAP_ENABLE parameter." + exclude_from_actuator_testing: true From 1bcdb3ef8c8a45369fe05ad9314f06e8ba6a6af8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 26 Nov 2024 17:41:00 +0100 Subject: [PATCH 174/211] RPMCapture: add hardcoded rpm processing Such that we can continue development on this part. The implementation was already used before porting things into the RPM capture module. --- msg/Rpm.msg | 3 +++ src/drivers/rpm_capture/RPMCapture.cpp | 25 +++++++++++++++++++++++-- src/drivers/rpm_capture/RPMCapture.hpp | 6 ++++++ 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index baab7c6a6787..7a6b179b8420 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -2,3 +2,6 @@ uint64 timestamp # time since system start (microseconds) float32 indicated_frequency_rpm # indicated rotor Frequency in Revolution per minute float32 estimated_accurancy_rpm # estimated accuracy in Revolution per minute + +float32 rpm_raw # measured rpm +float32 rpm_estimate # filtered rpm diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 397f6060c97e..3df4b0f3ea4c 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -99,13 +99,34 @@ void RPMCapture::Run() return; } + hrt_abstime now = hrt_absolute_time(); + const hrt_abstime period = _hrt_timestamp - _hrt_timestamp_prev; + pwm_input_s pwm_input{}; - pwm_input.timestamp = hrt_absolute_time(); - pwm_input.period = _hrt_timestamp - _hrt_timestamp_prev; + pwm_input.timestamp = now; + pwm_input.period = period; pwm_input.error_count = _error_count; _hrt_timestamp_prev = _hrt_timestamp; _value_processed.store(true); _pwm_input_pub.publish(pwm_input); + + float rpm_raw{0.f}; + + if ((1 < period) && (period < 1_s)) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + rpm_raw = 60.f * 1e6f / (static_cast(period) * 1.f); + } + + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); + _timestamp_last_update = now; + _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.update(rpm_raw); + + rpm_s rpm{}; + rpm.timestamp = now; + rpm.rpm_raw = rpm_raw; + rpm.rpm_estimate = _rpm_filter.getState(); + _rpm_pub.publish(rpm); } int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg) diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index b386c46ac79e..309f29a03b40 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -34,12 +34,14 @@ #pragma once #include +#include #include #include #include #include #include #include +#include using namespace time_literals; @@ -71,9 +73,13 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem int _channel{-1}; uint32_t _rpm_capture_gpio{0}; uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; + uORB::Publication _rpm_pub{ORB_ID(rpm)}; hrt_abstime _hrt_timestamp{0}; hrt_abstime _hrt_timestamp_prev{0}; uint32_t _error_count{0}; px4::atomic _value_processed{true}; + + hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt + AlphaFilter _rpm_filter; }; From 41b55724bc1c88c742bb8392228fb6649fb02f7c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Nov 2024 14:39:00 +0100 Subject: [PATCH 175/211] RPMCapture: Detect rpm sensing timeout --- src/drivers/rpm_capture/RPMCapture.cpp | 27 ++++++++++++++++---------- src/drivers/rpm_capture/RPMCapture.hpp | 1 + 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 3df4b0f3ea4c..e58b82f8ac1c 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -43,6 +43,7 @@ RPMCapture::RPMCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { _pwm_input_pub.advertise(); + ScheduleOnInterval(1_s); } RPMCapture::~RPMCapture() @@ -100,21 +101,27 @@ void RPMCapture::Run() } hrt_abstime now = hrt_absolute_time(); - const hrt_abstime period = _hrt_timestamp - _hrt_timestamp_prev; - pwm_input_s pwm_input{}; - pwm_input.timestamp = now; - pwm_input.period = period; - pwm_input.error_count = _error_count; - _hrt_timestamp_prev = _hrt_timestamp; - _value_processed.store(true); - _pwm_input_pub.publish(pwm_input); + if (!_value_processed.load()) { + _period = _hrt_timestamp - _hrt_timestamp_prev; + _hrt_timestamp_prev = _hrt_timestamp; + _value_processed.store(true); + + pwm_input_s pwm_input{}; + pwm_input.timestamp = now; + pwm_input.period = _period; + pwm_input.error_count = _error_count; + _pwm_input_pub.publish(pwm_input); + + } else if (now > _hrt_timestamp_prev + 1_s) { + _period = 0; + } float rpm_raw{0.f}; - if ((1 < period) && (period < 1_s)) { + if ((1 < _period) && (_period < 1_s)) { // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - rpm_raw = 60.f * 1e6f / (static_cast(period) * 1.f); + rpm_raw = 60.f * 1e6f / (static_cast(_period) * 1.f); } const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 309f29a03b40..63103ae9558e 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -77,6 +77,7 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem hrt_abstime _hrt_timestamp{0}; hrt_abstime _hrt_timestamp_prev{0}; + uint32_t _period{0}; uint32_t _error_count{0}; px4::atomic _value_processed{true}; From a60591378cca4a2434763052850f7329f6f265de Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Nov 2024 14:46:39 +0100 Subject: [PATCH 176/211] RPMCapture: switch to PublicationMulti to be compatible with the existing rpm drivers --- src/drivers/rpm_capture/RPMCapture.cpp | 1 + src/drivers/rpm_capture/RPMCapture.hpp | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index e58b82f8ac1c..e75091888695 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -90,6 +90,7 @@ bool RPMCapture::init() success = true; } + success = success && _rpm_pub.advertise(); return success; } diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 63103ae9558e..7f7f4037ec52 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include @@ -72,8 +72,8 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem int _channel{-1}; uint32_t _rpm_capture_gpio{0}; - uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; - uORB::Publication _rpm_pub{ORB_ID(rpm)}; + uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; + uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; hrt_abstime _hrt_timestamp{0}; hrt_abstime _hrt_timestamp_prev{0}; From c463cc42d0f7743a3621aba188e7bd097f584f15 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Nov 2024 14:51:28 +0100 Subject: [PATCH 177/211] RPM: clean up message --- msg/Rpm.msg | 7 ++----- src/drivers/rpm/pcf8583/PCF8583.cpp | 4 +--- src/drivers/rpm/rpm_simulator/rpm_simulator.cpp | 3 +-- src/modules/mavlink/streams/RAW_RPM.hpp | 2 +- .../simulator_mavlink/SimulatorMavlink.cpp | 14 ++++++-------- 5 files changed, 11 insertions(+), 19 deletions(-) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index 7a6b179b8420..fcec7edaf89c 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,7 +1,4 @@ -uint64 timestamp # time since system start (microseconds) - -float32 indicated_frequency_rpm # indicated rotor Frequency in Revolution per minute -float32 estimated_accurancy_rpm # estimated accuracy in Revolution per minute +uint64 timestamp # time since system start (microseconds) +float32 rpm_estimate # filtered revolutions per minute float32 rpm_raw # measured rpm -float32 rpm_estimate # filtered rpm diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index 085015247f4b..f72c200efb85 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/src/drivers/rpm/pcf8583/PCF8583.cpp @@ -196,12 +196,10 @@ void PCF8583::RunImpl() // Calculate RPM and accuracy estimation float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f; - float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1e6f) * 60.f; // publish data to uorb rpm_s msg{}; - msg.indicated_frequency_rpm = indicated_rpm; - msg.estimated_accurancy_rpm = estimated_accurancy; + msg.rpm_estimate = indicated_rpm; msg.timestamp = hrt_absolute_time(); _rpm_pub.publish(msg); diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 5e1e8b016ca2..4926d1ca9f6b 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp @@ -66,8 +66,7 @@ int rpm_simulator_main(int argc, char *argv[]) // prpepare RPM data message rpm.timestamp = timestamp_us; - rpm.indicated_frequency_rpm = frequency; - rpm.estimated_accurancy_rpm = frequency / 100.0f; + rpm.rpm_estimate = frequency; // Publish data and let the user know what was published rpm_pub.publish(rpm); diff --git a/src/modules/mavlink/streams/RAW_RPM.hpp b/src/modules/mavlink/streams/RAW_RPM.hpp index 9d2088123ff5..cfc4ceb54dfa 100644 --- a/src/modules/mavlink/streams/RAW_RPM.hpp +++ b/src/modules/mavlink/streams/RAW_RPM.hpp @@ -68,7 +68,7 @@ class MavlinkStreamRawRpm : public MavlinkStream mavlink_raw_rpm_t msg{}; msg.index = i; - msg.frequency = rpm.indicated_frequency_rpm; + msg.frequency = rpm.rpm_estimate; mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg); updated = true; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index c1bdfb6dc1d5..58e6ece48108 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -388,14 +388,12 @@ void SimulatorMavlink::handle_message(const mavlink_message_t *msg) break; case MAVLINK_MSG_ID_RAW_RPM: - mavlink_raw_rpm_t rpm; - mavlink_msg_raw_rpm_decode(msg, &rpm); - rpm_s rpmmsg{}; - rpmmsg.timestamp = hrt_absolute_time(); - rpmmsg.indicated_frequency_rpm = rpm.frequency; - rpmmsg.estimated_accurancy_rpm = 0; - - _rpm_pub.publish(rpmmsg); + mavlink_raw_rpm_t rpm_mavlink; + mavlink_msg_raw_rpm_decode(msg, &rpm_mavlink); + rpm_s rpm_uorb{}; + rpm_uorb.timestamp = hrt_absolute_time(); + rpm_uorb.rpm_estimate = rpm_mavlink.frequency; + _rpm_pub.publish(rpm_uorb); break; } } From 8486f74b2a255c3ba26b50c6e128d5c53f7b4c5d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Nov 2024 16:02:34 +0100 Subject: [PATCH 178/211] RPMCapture: much better timeout scheduling Not timing out based on a random interval but based on the time after the last inerrupt. --- src/drivers/rpm_capture/RPMCapture.cpp | 15 ++++++++++++--- src/drivers/rpm_capture/RPMCapture.hpp | 2 ++ 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index e75091888695..9d042b5ade27 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -43,7 +43,7 @@ RPMCapture::RPMCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { _pwm_input_pub.advertise(); - ScheduleOnInterval(1_s); + ScheduleNow(); } RPMCapture::~RPMCapture() @@ -104,6 +104,7 @@ void RPMCapture::Run() hrt_abstime now = hrt_absolute_time(); if (!_value_processed.load()) { + // There was an interrupt _period = _hrt_timestamp - _hrt_timestamp_prev; _hrt_timestamp_prev = _hrt_timestamp; _value_processed.store(true); @@ -114,15 +115,23 @@ void RPMCapture::Run() pwm_input.error_count = _error_count; _pwm_input_pub.publish(pwm_input); - } else if (now > _hrt_timestamp_prev + 1_s) { + // Schedule for next timeout + ScheduleClear(); + ScheduleDelayed(RPM_PULSE_TIMEOUT); + + } else { + // Timeout for no interrupts _period = 0; } float rpm_raw{0.f}; - if ((1 < _period) && (_period < 1_s)) { + if ((1 < _period) && (_period < RPM_PULSE_TIMEOUT)) { // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute rpm_raw = 60.f * 1e6f / (static_cast(_period) * 1.f); + + } else { + _rpm_filter.reset(rpm_raw); } const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 7f7f4037ec52..a2156437a48e 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -68,6 +68,8 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem static void stop(); private: + static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; + void Run() override; int _channel{-1}; From 6241db74dbfd248317d1ef31a9760f63212f5d39 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Nov 2024 16:08:03 +0100 Subject: [PATCH 179/211] mavlink: stream RAW_RPM message for ONBOAR_LOW_BANDWIDTH --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 820a2d4b2229..1c63f8adeeee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1777,6 +1777,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("PING", 0.1f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); + configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 20.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); From f687e3d8a406574d7671e7f9967c4a06da130eb4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 27 Nov 2024 18:03:18 +0100 Subject: [PATCH 180/211] RPM capture: publish every 1s without new data Signed-off-by: Silvan Fuhrer --- src/drivers/rpm_capture/RPMCapture.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 9d042b5ade27..4783e5b7ea1d 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -122,6 +122,7 @@ void RPMCapture::Run() } else { // Timeout for no interrupts _period = 0; + ScheduleDelayed(RPM_PULSE_TIMEOUT); } float rpm_raw{0.f}; From 1b105c937d9e32cec0c704966e5562da9925e58d Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 29 Nov 2024 09:30:13 +0100 Subject: [PATCH 181/211] RpmControl: remove raw RPM spikes using median filter --- src/drivers/rpm_capture/RPMCapture.cpp | 2 +- src/drivers/rpm_capture/RPMCapture.hpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 4783e5b7ea1d..432bb21d4bf3 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -138,7 +138,7 @@ void RPMCapture::Run() const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); _timestamp_last_update = now; _rpm_filter.setParameters(dt, 0.5f); - _rpm_filter.update(rpm_raw); + _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); rpm_s rpm{}; rpm.timestamp = now; diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index a2156437a48e..a986fc9ceb67 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -85,4 +86,5 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt AlphaFilter _rpm_filter; + MedianFilter _rpm_median_filter; }; From d31271c38ed23c6c4822f348c10eee2847398816 Mon Sep 17 00:00:00 2001 From: oravla5 Date: Tue, 10 Dec 2024 15:45:30 +0100 Subject: [PATCH 182/211] RPMCapture: added pulse per rev parameter. Do not publish if pulses are to fast --- src/drivers/rpm_capture/RPMCapture.cpp | 37 +++++++++++--------- src/drivers/rpm_capture/RPMCapture.hpp | 3 ++ src/drivers/rpm_capture/rpm_capture_params.c | 12 +++++++ 3 files changed, 36 insertions(+), 16 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 432bb21d4bf3..c9c831b6db15 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -58,6 +58,9 @@ bool RPMCapture::init() { bool success = false; + param_get(param_find("RPM_PULSES_PER_REV"), &_pulses_per_revolution); + _min_pulse_period_us = static_cast(60.f * 1e6f / static_cast(RPM_MAX_VALUE * _pulses_per_revolution)); + for (unsigned i = 0; i < 16; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); @@ -125,26 +128,28 @@ void RPMCapture::Run() ScheduleDelayed(RPM_PULSE_TIMEOUT); } - float rpm_raw{0.f}; - if ((1 < _period) && (_period < RPM_PULSE_TIMEOUT)) { - // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - rpm_raw = 60.f * 1e6f / (static_cast(_period) * 1.f); + if (_period > _min_pulse_period_us) { + // Only update if the period is above the min pulse period threshold + float rpm_raw{0.f}; - } else { - _rpm_filter.reset(rpm_raw); - } + if (_period < RPM_PULSE_TIMEOUT) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + rpm_raw = 60.f * 1e6f / static_cast(_pulses_per_revolution * _period); + } + + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); + _timestamp_last_update = now; + _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); - _timestamp_last_update = now; - _rpm_filter.setParameters(dt, 0.5f); - _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); + rpm_s rpm{}; + rpm.timestamp = now; + rpm.rpm_raw = rpm_raw; + rpm.rpm_estimate = _rpm_filter.getState(); + _rpm_pub.publish(rpm); + } - rpm_s rpm{}; - rpm.timestamp = now; - rpm.rpm_raw = rpm_raw; - rpm.rpm_estimate = _rpm_filter.getState(); - _rpm_pub.publish(rpm); } int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg) diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index a986fc9ceb67..872ab06a5bd1 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -70,11 +70,14 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem private: static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; + static constexpr uint32_t RPM_MAX_VALUE = 15000; void Run() override; int _channel{-1}; uint32_t _rpm_capture_gpio{0}; + uint32_t _pulses_per_revolution{1}; + uint32_t _min_pulse_period_us{1}; ///< [us] minimum pulse period uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c index 7397177370c0..08502f6982b6 100644 --- a/src/drivers/rpm_capture/rpm_capture_params.c +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -41,3 +41,15 @@ * @reboot_required true */ PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0); + +/** + * RPM Pulses per Revolution + * + * Number of pulses per revolution for the RPM sensor. + * + * @group System + * @min 1 + * @max 50 + * @reboot_required true + */ +PARAM_DEFINE_INT32(RPM_PULSES_PER_REV, 1); From 825dc47fad64612d9e1d15d57383e3f11f67b40f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:30:25 +0100 Subject: [PATCH 183/211] RPMCapture: fix pulses per revolution parameter name, description, type -> use module params --- src/drivers/rpm_capture/RPMCapture.cpp | 9 +++++---- src/drivers/rpm_capture/RPMCapture.hpp | 8 ++++++-- src/drivers/rpm_capture/rpm_capture_params.c | 6 +++--- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index c9c831b6db15..69ddb1ca4919 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -40,7 +40,8 @@ #include RPMCapture::RPMCapture() : - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ModuleParams(nullptr) { _pwm_input_pub.advertise(); ScheduleNow(); @@ -58,8 +59,8 @@ bool RPMCapture::init() { bool success = false; - param_get(param_find("RPM_PULSES_PER_REV"), &_pulses_per_revolution); - _min_pulse_period_us = static_cast(60.f * 1e6f / static_cast(RPM_MAX_VALUE * _pulses_per_revolution)); + _min_pulse_period_us = static_cast(60.f * 1e6f / static_cast(RPM_MAX_VALUE * + _param_rpm_puls_per_rev.get())); for (unsigned i = 0; i < 16; ++i) { char param_name[17]; @@ -135,7 +136,7 @@ void RPMCapture::Run() if (_period < RPM_PULSE_TIMEOUT) { // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - rpm_raw = 60.f * 1e6f / static_cast(_pulses_per_revolution * _period); + rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); } const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 872ab06a5bd1..5d7093bb0c5a 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -46,7 +47,7 @@ using namespace time_literals; -class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem +class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, public ModuleParams { public: RPMCapture(); @@ -76,7 +77,6 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem int _channel{-1}; uint32_t _rpm_capture_gpio{0}; - uint32_t _pulses_per_revolution{1}; uint32_t _min_pulse_period_us{1}; ///< [us] minimum pulse period uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; @@ -90,4 +90,8 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt AlphaFilter _rpm_filter; MedianFilter _rpm_median_filter; + + DEFINE_PARAMETERS( + (ParamInt) _param_rpm_puls_per_rev + ) }; diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c index 08502f6982b6..88f0df613c63 100644 --- a/src/drivers/rpm_capture/rpm_capture_params.c +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -43,13 +43,13 @@ PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0); /** - * RPM Pulses per Revolution + * Voltage pulses per revolution * - * Number of pulses per revolution for the RPM sensor. + * Number of voltage pulses per one rotor revolution on the capturing pin. * * @group System * @min 1 * @max 50 * @reboot_required true */ -PARAM_DEFINE_INT32(RPM_PULSES_PER_REV, 1); +PARAM_DEFINE_INT32(RPM_PULS_PER_REV, 1); From ec4ed75264564647ba6342c13c41c8a9cca7e1cd Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:44:16 +0100 Subject: [PATCH 184/211] RPMCapture: define period as UINT32_MAX when timed out --- src/drivers/rpm_capture/RPMCapture.cpp | 2 +- src/drivers/rpm_capture/RPMCapture.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 69ddb1ca4919..81eaa7276734 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -125,7 +125,7 @@ void RPMCapture::Run() } else { // Timeout for no interrupts - _period = 0; + _period = UINT32_MAX; ScheduleDelayed(RPM_PULSE_TIMEOUT); } diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 5d7093bb0c5a..bbc1b2f66b42 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -83,7 +83,7 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, hrt_abstime _hrt_timestamp{0}; hrt_abstime _hrt_timestamp_prev{0}; - uint32_t _period{0}; + uint32_t _period{UINT32_MAX}; uint32_t _error_count{0}; px4::atomic _value_processed{true}; From c406e0a3a220c794a1f35a7d941b38c1ab8d6110 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:34:47 +0100 Subject: [PATCH 185/211] RPMCapture: refactor to clarify when an interrupt happened --- src/drivers/rpm_capture/RPMCapture.cpp | 8 ++++---- src/drivers/rpm_capture/RPMCapture.hpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 81eaa7276734..0da3f941df23 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -107,11 +107,11 @@ void RPMCapture::Run() hrt_abstime now = hrt_absolute_time(); - if (!_value_processed.load()) { + if (_interrupt_happened.load()) { // There was an interrupt _period = _hrt_timestamp - _hrt_timestamp_prev; _hrt_timestamp_prev = _hrt_timestamp; - _value_processed.store(true); + _interrupt_happened.store(false); pwm_input_s pwm_input{}; pwm_input.timestamp = now; @@ -157,12 +157,12 @@ int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg) { RPMCapture *instance = static_cast(arg); - if (!instance->_value_processed.load()) { + if (instance->_interrupt_happened.load()) { ++instance->_error_count; } instance->_hrt_timestamp = hrt_absolute_time(); - instance->_value_processed.store(false); + instance->_interrupt_happened.store(true); instance->ScheduleNow(); return PX4_OK; diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index bbc1b2f66b42..0744c3951f44 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -85,7 +85,7 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, hrt_abstime _hrt_timestamp_prev{0}; uint32_t _period{UINT32_MAX}; uint32_t _error_count{0}; - px4::atomic _value_processed{true}; + px4::atomic _interrupt_happened{false}; hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt AlphaFilter _rpm_filter; From 9ca06be4f5776f17efbd41274070968975ca1c23 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:37:37 +0100 Subject: [PATCH 186/211] RPMCapture: refactor to clarify scheduling --- src/drivers/rpm_capture/RPMCapture.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 0da3f941df23..1f949e7078c0 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -119,16 +119,14 @@ void RPMCapture::Run() pwm_input.error_count = _error_count; _pwm_input_pub.publish(pwm_input); - // Schedule for next timeout - ScheduleClear(); - ScheduleDelayed(RPM_PULSE_TIMEOUT); + ScheduleClear(); // Do not run on previously scheduled timeout } else { // Timeout for no interrupts _period = UINT32_MAX; - ScheduleDelayed(RPM_PULSE_TIMEOUT); } + ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout if (_period > _min_pulse_period_us) { // Only update if the period is above the min pulse period threshold From d027f8bf39af48ab7b6e2c5053a5eeec5fb0ee7b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:50:35 +0100 Subject: [PATCH 187/211] RPMCapture: publish outliers in rpm message but don't update the filters and estimate --- src/drivers/rpm_capture/RPMCapture.cpp | 29 +++++++++++--------------- src/drivers/rpm_capture/RPMCapture.hpp | 3 +-- 2 files changed, 13 insertions(+), 19 deletions(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 1f949e7078c0..93aa3c637e69 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -59,9 +59,6 @@ bool RPMCapture::init() { bool success = false; - _min_pulse_period_us = static_cast(60.f * 1e6f / static_cast(RPM_MAX_VALUE * - _param_rpm_puls_per_rev.get())); - for (unsigned i = 0; i < 16; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); @@ -127,28 +124,26 @@ void RPMCapture::Run() } ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout + float rpm_raw{0.f}; - if (_period > _min_pulse_period_us) { - // Only update if the period is above the min pulse period threshold - float rpm_raw{0.f}; - - if (_period < RPM_PULSE_TIMEOUT) { - // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); - } + if (_period < RPM_PULSE_TIMEOUT) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); + } + if (rpm_raw < RPM_MAX_VALUE) { + // Don't update RPM filter with outliers const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); _timestamp_last_update = now; _rpm_filter.setParameters(dt, 0.5f); _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); - - rpm_s rpm{}; - rpm.timestamp = now; - rpm.rpm_raw = rpm_raw; - rpm.rpm_estimate = _rpm_filter.getState(); - _rpm_pub.publish(rpm); } + rpm_s rpm{}; + rpm.timestamp = now; + rpm.rpm_raw = rpm_raw; + rpm.rpm_estimate = _rpm_filter.getState(); + _rpm_pub.publish(rpm); } int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg) diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 0744c3951f44..aec1b126e4c2 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -71,13 +71,12 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, private: static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; - static constexpr uint32_t RPM_MAX_VALUE = 15000; + static constexpr float RPM_MAX_VALUE = 50e3f; void Run() override; int _channel{-1}; uint32_t _rpm_capture_gpio{0}; - uint32_t _min_pulse_period_us{1}; ///< [us] minimum pulse period uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; From b7b384ab2e84221a90041785bed1d6eeec19db94 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:52:04 +0100 Subject: [PATCH 188/211] RPMCapture: add back filter reset on timeout --- src/drivers/rpm_capture/RPMCapture.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 93aa3c637e69..d210249712bb 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -129,6 +129,9 @@ void RPMCapture::Run() if (_period < RPM_PULSE_TIMEOUT) { // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); + + } else { + _rpm_filter.reset(rpm_raw); } if (rpm_raw < RPM_MAX_VALUE) { From f4c8e0e35f2ccc55ef00020b9f27806786213fb4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Dec 2024 19:53:57 +0100 Subject: [PATCH 189/211] RPMCapture: fix dt should not have a minimum which would limit to 6000rpm --- src/drivers/rpm_capture/RPMCapture.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index d210249712bb..8a9f2f795b8e 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -136,7 +136,7 @@ void RPMCapture::Run() if (rpm_raw < RPM_MAX_VALUE) { // Don't update RPM filter with outliers - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); + const float dt = math::min((now - _timestamp_last_update) * 1e-6f, 1.f); _timestamp_last_update = now; _rpm_filter.setParameters(dt, 0.5f); _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); From 267fa06da89c85facc7dbd634594d1bbe32c7da1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 12 Dec 2024 12:08:39 +0100 Subject: [PATCH 190/211] RPMCapture: fix copyright year, improve comments --- msg/Rpm.msg | 2 +- src/drivers/camera_capture/camera_capture.cpp | 2 +- .../camera_trigger/interfaces/src/camera_interface.cpp | 2 +- src/drivers/pps_capture/PPSCapture.cpp | 2 +- src/drivers/rpm_capture/CMakeLists.txt | 2 +- src/drivers/rpm_capture/RPMCapture.cpp | 4 ++-- src/drivers/rpm_capture/RPMCapture.hpp | 2 +- src/drivers/rpm_capture/rpm_capture_params.c | 2 +- 8 files changed, 9 insertions(+), 9 deletions(-) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index fcec7edaf89c..ca69e50fdb1c 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,4 @@ uint64 timestamp # time since system start (microseconds) float32 rpm_estimate # filtered revolutions per minute -float32 rpm_raw # measured rpm +float32 rpm_raw diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 7bf381354281..2d62d8508696 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -81,7 +81,7 @@ CameraCapture::CameraCapture() : int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2032) { // Camera_Capture + if (function == 2032) { // Camera_Capture see mixer_module/output_functions.yaml parameter metadata definition _capture_channel = i; } } diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index 02e9119cae99..e15468b03a88 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -51,7 +51,7 @@ void CameraInterface::get_pins() int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2000) { // Camera_Trigger + if (function == 2000) { // Camera_Trigger see mixer_module/output_functions.yaml parameter metadata definition _pins[pin_index++] = i; } } diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index 399f034fbf5f..8a13c48c3584 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -70,7 +70,7 @@ bool PPSCapture::init() int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2064) { // PPS_Input + if (function == 2064) { // PPS_Input see mixer_module/output_functions.yaml parameter metadata definition _channel = i; } } diff --git a/src/drivers/rpm_capture/CMakeLists.txt b/src/drivers/rpm_capture/CMakeLists.txt index db52504b4fcb..b2639cba1de5 100644 --- a/src/drivers/rpm_capture/CMakeLists.txt +++ b/src/drivers/rpm_capture/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2024 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/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 8a9f2f795b8e..8330f77d1305 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -66,7 +66,7 @@ bool RPMCapture::init() int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2070) { // RPM_Input + if (function == 2070) { // RPM_Input see mixer_module/output_functions.yaml parameter metadata definition _channel = i; } } diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index aec1b126e4c2..61281257118c 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c index 88f0df613c63..13bd3fb7a739 100644 --- a/src/drivers/rpm_capture/rpm_capture_params.c +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 From 8ce6ad6662d6258ef79d500d17ec2d5196aa5203 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 12 Dec 2024 13:27:09 +0100 Subject: [PATCH 191/211] camera_capture: refactor to simplify capture channel initialization --- src/drivers/camera_capture/camera_capture.cpp | 3 --- src/drivers/camera_capture/camera_capture.hpp | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 2d62d8508696..77386f272616 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -71,9 +71,6 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - // get the capture channel from function configuration params - _capture_channel = -1; - for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 92dd2790d1d4..3074b5cc8db1 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -96,7 +96,7 @@ class CameraCapture : public px4::ScheduledWorkItem static struct work_s _work_publisher; private: - int _capture_channel = 5; ///< by default, use FMU output 6 + int _capture_channel{-1}; // Publishers uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; From b85ad98a980f4ad02b8a7fc01d43f4826c2dafa5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 12 Dec 2024 13:30:21 +0100 Subject: [PATCH 192/211] Use defines instead of hardcoded magic values for maximum number of output channels and rpm filter time constant --- src/drivers/camera_capture/camera_capture.cpp | 2 +- .../camera_trigger/interfaces/src/camera_interface.cpp | 3 ++- src/drivers/pps_capture/PPSCapture.cpp | 2 +- src/drivers/rpm_capture/RPMCapture.cpp | 4 ++-- src/drivers/rpm_capture/RPMCapture.hpp | 1 + 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 77386f272616..843020902089 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -71,7 +71,7 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS && _capture_channel == -1; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index e15468b03a88..cf601ef41456 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "camera_interface.h" +#include #include #include @@ -44,7 +45,7 @@ void CameraInterface::get_pins() unsigned pin_index = 0; - for (unsigned i = 0; i < 16 && pin_index < arraySize(_pins); ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS && pin_index < arraySize(_pins); ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index 8a13c48c3584..9bb8d47fba8d 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -63,7 +63,7 @@ bool PPSCapture::init() { bool success = false; - for (unsigned i = 0; i < 16; ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 8330f77d1305..127fc633c315 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -59,7 +59,7 @@ bool RPMCapture::init() { bool success = false; - for (unsigned i = 0; i < 16; ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); @@ -138,7 +138,7 @@ void RPMCapture::Run() // Don't update RPM filter with outliers const float dt = math::min((now - _timestamp_last_update) * 1e-6f, 1.f); _timestamp_last_update = now; - _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.setParameters(dt, RPM_FILTER_TIME_CONSTANT); _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); } diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp index 61281257118c..63d742684840 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -72,6 +72,7 @@ class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, private: static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; static constexpr float RPM_MAX_VALUE = 50e3f; + static constexpr float RPM_FILTER_TIME_CONSTANT = .5f; void Run() override; From 077ade4f8f9f049a9aada8937a782b0de24e3447 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 12 Dec 2024 15:35:18 +0100 Subject: [PATCH 193/211] Add IO timer channel mode for RPM and also sync up rpi configuration to work with all these drivers --- Tools/kconfig/allyesconfig.py | 1 + platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h | 3 ++- .../nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h | 3 ++- .../nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h | 3 ++- .../nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h | 3 ++- .../nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h | 3 ++- .../nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h | 4 ++++ .../src/px4/stm/stm32_common/include/px4_arch/io_timer.h | 3 ++- src/drivers/rpm_capture/RPMCapture.cpp | 2 +- 9 files changed, 18 insertions(+), 7 deletions(-) diff --git a/Tools/kconfig/allyesconfig.py b/Tools/kconfig/allyesconfig.py index 889b659195ed..04d89477816d 100644 --- a/Tools/kconfig/allyesconfig.py +++ b/Tools/kconfig/allyesconfig.py @@ -40,6 +40,7 @@ 'DRIVERS_DISTANCE_SENSOR_SRF05', # Requires hardcoded GPIO_ULTRASOUND 'DRIVERS_PPS_CAPTURE', # Requires PPS GPIO config 'DRIVERS_PWM_INPUT', # Requires PWM config + 'DRIVERS_RPM_CAPTURE', # Requires PPS GPIO config 'DRIVERS_TEST_PPM', # PIN config not portable 'DRIVERS_TATTU_CAN', # Broken needs fixing 'MODULES_REPLAY', # Fails on NuttX targets maybe force POSIX dependency? diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index 9ce7b374456d..f9e341a43cd9 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -68,7 +68,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index 788da87485fd..836e214472b8 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -67,7 +67,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h index 9945e28de6c0..ca93f14a4f84 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h @@ -68,7 +68,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h index 2454056a0354..82369986b517 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -63,7 +63,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h index d093130b5180..d6e0ad6eb56a 100644 --- a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h @@ -63,7 +63,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h index c9fbedd8f0bc..0d042d85ce05 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h @@ -73,6 +73,10 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 31d6e9b1c7e4..8f64a825b664 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -79,7 +79,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 127fc633c315..1e054719032b 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -77,7 +77,7 @@ bool RPMCapture::init() return false; } - int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_Other); // TODO: add IOTimerChanMode_RPM + int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_RPM); if (ret != PX4_OK) { PX4_ERR("gpio alloc failed (%i) for RPM at channel (%d)", ret, _channel); From bdc6957c11159a3ec84b9ebea89aa509f9cc8404 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Wed, 18 Dec 2024 13:59:11 +0100 Subject: [PATCH 194/211] dont use BROACAST_MODE_ON during unit testing --- posix-configs/SITL/init/test/cmd_template.in | 2 +- posix-configs/SITL/init/test/test_mavlink | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index d12ab234422b..3ae9b82d0a61 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -15,7 +15,7 @@ pwm_out_sim start ver all -mavlink start -x -u 14556 -r 2000000 -p +mavlink start -x -u 14556 -r 2000000 mavlink boot_complete @cmd_name@ start diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index 174ab2cf0819..57afc0ca5e11 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -15,7 +15,7 @@ pwm_out_sim start ver all -mavlink start -x -u 14556 -r 2000000 -p +mavlink start -x -u 14556 -r 2000000 mavlink boot_complete mavlink_tests From 741e3e8589fcb28892205d911dfd3bdb4b4d4022 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 18 Dec 2024 14:50:37 +0100 Subject: [PATCH 195/211] ADSB: pass now timestamp as arguemnt of get_traffic_state() That enables us to time the unit tests better plus saves some flash and CPU. Signed-off-by: Silvan Fuhrer --- src/lib/adsb/AdsbConflict.cpp | 15 ++++++------ src/lib/adsb/AdsbConflict.h | 2 +- src/lib/adsb/AdsbConflictTest.cpp | 39 ++++++++++++++++++++----------- 3 files changed, 34 insertions(+), 22 deletions(-) diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index 8fa11a8f2a72..08524148263d 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -118,9 +118,8 @@ void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } -void AdsbConflict::get_traffic_state() +void AdsbConflict::get_traffic_state(hrt_abstime now) { - const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address); const bool old_conflict = (traffic_index >= 0); @@ -130,7 +129,7 @@ void AdsbConflict::get_traffic_state() bool old_conflict_warning_expired = false; if (old_conflict && _conflict_detected) { - old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT); + old_conflict_warning_expired = now > _traffic_buffer.timestamp[traffic_index] + CONFLICT_WARNING_TIMEOUT; } if (new_traffic && _conflict_detected && !_traffic_buffer_full) { @@ -142,7 +141,7 @@ void AdsbConflict::get_traffic_state() } else if (old_conflict && _conflict_detected && old_conflict_warning_expired) { - _traffic_buffer.timestamp[traffic_index] = hrt_absolute_time(); + _traffic_buffer.timestamp[traffic_index] = now; _traffic_state = TRAFFIC_STATE::REMIND_CONFLICT; } else if (old_conflict && !_conflict_detected) { @@ -152,7 +151,6 @@ void AdsbConflict::get_traffic_state() } else { _traffic_state = TRAFFIC_STATE::NO_CONFLICT; } - } void AdsbConflict::remove_expired_conflicts() @@ -172,8 +170,9 @@ void AdsbConflict::remove_expired_conflicts() bool AdsbConflict::handle_traffic_conflict() { + const hrt_abstime now = hrt_absolute_time(); - get_traffic_state(); + get_traffic_state(now); bool take_action = false; @@ -192,7 +191,7 @@ bool AdsbConflict::handle_traffic_conflict() events::send(events::ID("navigator_traffic_resolved"), events::Log::Notice, "Traffic Conflict Resolved {1}!", _transponder_report.icao_address); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; } break; @@ -201,7 +200,7 @@ bool AdsbConflict::handle_traffic_conflict() if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) && (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) { events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on"); - _last_buffer_full_warning_time = hrt_absolute_time(); + _last_buffer_full_warning_time = now; } //disable conflict warnings when buffer is full diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index 9942a6b921b6..ebe15881611e 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -111,7 +111,7 @@ class AdsbConflict void add_icao_address_from_conflict_list(uint32_t icao_address); - void get_traffic_state(); + void get_traffic_state(hrt_abstime now); void set_conflict_detection_params(float crosstrack_separation, float vertical_separation, int collision_time_threshold, uint8_t traffic_avoidance_mode); diff --git a/src/lib/adsb/AdsbConflictTest.cpp b/src/lib/adsb/AdsbConflictTest.cpp index 5d6b6ccbe630..f0ddea70d198 100644 --- a/src/lib/adsb/AdsbConflictTest.cpp +++ b/src/lib/adsb/AdsbConflictTest.cpp @@ -126,17 +126,19 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(used_buffer); bool conflict_detected = false; + hrt_abstime now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 00001; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); conflict_detected = true; + now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); @@ -144,9 +146,10 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(empty_buffer); conflict_detected = true; + now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); @@ -154,25 +157,28 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(full_buffer); conflict_detected = true; + now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL); conflict_detected = false; + now = 2_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); conflict_detected = false; + now = 3_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); @@ -180,9 +186,10 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(used_buffer); conflict_detected = false; + now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); @@ -235,17 +242,19 @@ TEST_F(AdsbConflictTest, trafficReminder) adsb_conflict.set_traffic_buffer(used_buffer); bool conflict_detected = true; + hrt_abstime now = 200_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); conflict_detected = true; + now = 201_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); @@ -253,33 +262,37 @@ TEST_F(AdsbConflictTest, trafficReminder) adsb_conflict.set_traffic_buffer(full_buffer); conflict_detected = true; + now = 400_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); conflict_detected = true; + now = 401_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); conflict_detected = false; + now = 600_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); conflict_detected = true; + now = 700_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); From 1a1fd593025bcf6cbabc0bffe3520c3d4caf6a4c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 18 Dec 2024 14:56:19 +0100 Subject: [PATCH 196/211] ADSB: pass now timestamp as arguemnt of send_traffic_warning() Saves some flash and CPU by not having to call hrt_absolute_time(). Signed-off-by: Silvan Fuhrer --- src/lib/adsb/AdsbConflict.cpp | 15 ++++++++------- src/lib/adsb/AdsbConflict.h | 2 +- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index 08524148263d..14113e649e73 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -183,7 +183,8 @@ bool AdsbConflict::handle_traffic_conflict() take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f), (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, _transponder_report.callsign, - _transponder_report.icao_address); + _transponder_report.icao_address, + now); } break; @@ -233,7 +234,7 @@ void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, fl bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address) + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address, hrt_abstime now) { switch (_conflict_detection_params.traffic_avoidance_mode) { @@ -250,7 +251,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper } - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; break; } @@ -277,7 +278,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; break; } @@ -293,7 +294,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; return true; @@ -311,7 +312,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; return true; @@ -330,7 +331,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; return true; diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index ebe15881611e..e35850fd408f 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -118,7 +118,7 @@ class AdsbConflict bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address); + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address, hrt_abstime now); transponder_report_s _transponder_report{}; From 30e51cb80ec56b881d96266a46947d52f1876c1f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 18 Dec 2024 17:19:07 +0100 Subject: [PATCH 197/211] ADSB: improve readability of unit tests Signed-off-by: Silvan Fuhrer --- src/lib/adsb/AdsbConflictTest.cpp | 56 +++++++++++++++++++------------ 1 file changed, 35 insertions(+), 21 deletions(-) diff --git a/src/lib/adsb/AdsbConflictTest.cpp b/src/lib/adsb/AdsbConflictTest.cpp index f0ddea70d198..bba64cd87b63 100644 --- a/src/lib/adsb/AdsbConflictTest.cpp +++ b/src/lib/adsb/AdsbConflictTest.cpp @@ -4,7 +4,6 @@ #include "AdsbConflictTest.h" - class TestAdsbConflict : public AdsbConflict { public: @@ -25,8 +24,6 @@ class TestAdsbConflict : public AdsbConflict TEST_F(AdsbConflictTest, detectTrafficConflict) { - - int collision_time_threshold = 60; float crosstrack_separation = 500.0f; @@ -42,19 +39,15 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) uint32_t traffic_dataset_size = sizeof(traffic_dataset) / sizeof(traffic_dataset[0]); - TestAdsbConflict adsb_conflict; adsb_conflict.set_conflict_detection_params(crosstrack_separation, vertical_separation, collision_time_threshold, 1); for (uint32_t i = 0; i < traffic_dataset_size; i++) { - - //printf("---------------%d--------------\n", i); - struct traffic_data_s traffic = traffic_dataset[i]; - + // GIVEN traffic dataset (which should result in conflict) adsb_conflict._transponder_report.lat = traffic.lat_traffic; adsb_conflict._transponder_report.lon = traffic.lon_traffic; adsb_conflict._transponder_report.altitude = traffic.alt_traffic; @@ -62,22 +55,17 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) adsb_conflict._transponder_report.hor_velocity = traffic.vxy_traffic; adsb_conflict._transponder_report.ver_velocity = traffic.vz_traffic; + // WHEN detect traffic conflict is called adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now); - //printf("conflict_detected %d \n", adsb_conflict._conflict_detected); - - //printf("------------------------------\n"); - //printf("------------------------------\n \n"); - + // THEN expect conflict to be detected EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict); } - } TEST_F(AdsbConflictTest, trafficAlerts) { - struct traffic_buffer_s used_buffer; used_buffer.icao_address.push_back(2345); used_buffer.icao_address.push_back(1234); @@ -116,84 +104,98 @@ TEST_F(AdsbConflictTest, trafficAlerts) full_buffer.timestamp.push_back(1000_s); full_buffer.timestamp.push_back(58943_s); - - struct traffic_buffer_s empty_buffer = {}; - TestAdsbConflict adsb_conflict; + // GIVEN used buffer adsb_conflict.set_traffic_buffer(used_buffer); + // WHEN no conflict detected bool conflict_detected = false; hrt_abstime now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 00001; adsb_conflict.get_traffic_state(now); + // THEN expect no conflict printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // GIVEN conflict detected conflict_detected = true; now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be added to buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); + // GIVEN empty buffer adsb_conflict.set_traffic_buffer(empty_buffer); + // WHEN conflict detected conflict_detected = true; now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be added to buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); + // GIVEN full buffer adsb_conflict.set_traffic_buffer(full_buffer); + // WHEN conflict detected conflict_detected = true; now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; adsb_conflict.get_traffic_state(now); + // THEN expect buffer full printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL); + // WHEN conflict set to false again conflict_detected = false; now = 2_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; adsb_conflict.get_traffic_state(now); + // THEN expect no conflict message printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // WHEN existing conflict is set to false conflict_detected = false; now = 3_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be removed from buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); + // GIVEN used buffer adsb_conflict.set_traffic_buffer(used_buffer); + // WHEN conflict is set to false again conflict_detected = false; now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be removed from buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); - } TEST_F(AdsbConflictTest, trafficReminder) @@ -236,65 +238,77 @@ TEST_F(AdsbConflictTest, trafficReminder) full_buffer.timestamp.push_back(100_s); full_buffer.timestamp.push_back(5843_s); - TestAdsbConflict adsb_conflict; + // GIVEN buffer with 8685 at t=100 adsb_conflict.set_traffic_buffer(used_buffer); + // WHEN conflict detected at t=200 bool conflict_detected = true; hrt_abstime now = 200_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN expect conflict reminder printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); + // WHEN INSTEAD conflict is detected only 1s later conflict_detected = true; now = 201_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN do not sent conflict notification again printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // GIVEN full buffer with 8685 at t=100 adsb_conflict.set_traffic_buffer(full_buffer); + // WHEN conflict detected at t=400 conflict_detected = true; now = 400_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN expect conflict reminder printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); + // WHEN INSTEAD conflict is detected only 1s later conflict_detected = true; now = 401_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN do not sent conflict notification again printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // WHEN conflict is set to false conflict_detected = false; now = 600_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be removed from buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); + // WHEN new conflict is detected conflict_detected = true; now = 700_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; adsb_conflict.get_traffic_state(now); + // THEN expect new conflict to be added to buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); - } From c69f96ec04a1142427583fae34d23a16427f6d5c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Dec 2024 19:20:15 +0100 Subject: [PATCH 198/211] ADSB: replace last excess hrt_absolute_time() in the loop --- src/lib/adsb/AdsbConflict.cpp | 6 +++--- src/lib/adsb/AdsbConflict.h | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index 14113e649e73..22f393cd3083 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -111,9 +111,9 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index) PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } -void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) +void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address, hrt_abstime now) { - _traffic_buffer.timestamp.push_back(hrt_absolute_time()); + _traffic_buffer.timestamp.push_back(now); _traffic_buffer.icao_address.push_back(icao_address); PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } @@ -133,7 +133,7 @@ void AdsbConflict::get_traffic_state(hrt_abstime now) } if (new_traffic && _conflict_detected && !_traffic_buffer_full) { - add_icao_address_from_conflict_list(_transponder_report.icao_address); + add_icao_address_from_conflict_list(_transponder_report.icao_address, now); _traffic_state = TRAFFIC_STATE::ADD_CONFLICT; } else if (new_traffic && _conflict_detected && _traffic_buffer_full) { diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index e35850fd408f..5b6dc4edd923 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -109,7 +109,7 @@ class AdsbConflict void remove_icao_address_from_conflict_list(int traffic_index); - void add_icao_address_from_conflict_list(uint32_t icao_address); + void add_icao_address_from_conflict_list(uint32_t icao_address, hrt_abstime now); void get_traffic_state(hrt_abstime now); @@ -129,8 +129,7 @@ class AdsbConflict float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav, double lon_uav, float &alt_uav); - void run_fake_traffic(double &lat_uav, double &lon_uav, - float &alt_uav); + void run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav); void remove_expired_conflicts(); From f1855de0060488c2c3e5c1add962317f73448cdd Mon Sep 17 00:00:00 2001 From: Pernilla Date: Fri, 15 Nov 2024 15:10:48 +0100 Subject: [PATCH 199/211] Adding tiltrotor model --- .../init.d-posix/airframes/4020_gz_tiltrotor | 112 ++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + 2 files changed, 113 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor new file mode 100644 index 000000000000..c798650b5473 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -0,0 +1,112 @@ +#!/bin/sh +# +# @name VTOL Tiltrotor +# +# @type VTOL Tiltrotor +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + +param set-default MAV_TYPE 21 + +param set-default CA_AIRFRAME 3 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.1515 +param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.1515 +param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.1515 +param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.1515 +param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_ROTOR0_TILT 1 +param set-default CA_ROTOR2_TILT 2 +param set-default CA_SV_TL0_MAXA 90 +param set-default CA_SV_TL0_MINA 0 +param set-default CA_SV_TL0_TD 0 +param set-default CA_SV_TL0_CT 1 +param set-default CA_SV_TL1_MAXA 90 +param set-default CA_SV_TL1_MINA 0 +param set-default CA_SV_TL1_TD 0 +param set-default CA_SV_TL1_CT 1 + +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS2_TRQ_P 1 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_TL_COUNT 2 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 + +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MIN4 10 + +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_FUNC2 202 +param set-default SIM_GZ_SV_FUNC3 203 +param set-default SIM_GZ_SV_FUNC4 204 +param set-default SIM_GZ_SV_FUNC5 205 + +param set-default SIM_GZ_SV_MAXA4 90 +param set-default SIM_GZ_SV_MINA4 0 +param set-default SIM_GZ_SV_MAXA5 90 +param set-default SIM_GZ_SV_MINA5 0 + +param set-default NPFG_PERIOD 12 +param set-default FW_PR_FF 0.2 +param set-default FW_PR_P 0.9 +param set-default FW_PSP_OFF 2 +param set-default FW_P_LIM_MIN -15 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_P 0.3 +param set-default FW_THR_TRIM 0.38 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 + +param set-default MC_AIRMODE 1 +param set-default MC_YAWRATE_P 0.4 +param set-default MC_YAWRATE_I 0.1 + +param set-default MPC_XY_VEL_P_ACC 3 +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_D_ACC 0.1 + +param set-default MIS_TAKEOFF_ALT 10 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_FWD_THRUST_SC 0.6 +param set-default VT_TILT_TRANS 0.6 +param set-default VT_TYPE 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index d9ef6143c2a6..da741d2a64db 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -92,6 +92,7 @@ px4_add_romfs_files( 4017_gz_x500_lidar_front 4018_gz_quadtailsitter 4019_gz_x500_gimbal + 4020_gz_tiltrotor 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post From be42c7c88a2906d55b65d42a8e2469049b5e4671 Mon Sep 17 00:00:00 2001 From: Pernilla Date: Wed, 18 Dec 2024 16:10:33 +0100 Subject: [PATCH 200/211] Adding min-max angles to use for normalized servo outputs --- .../gz_bridge/GZMixingInterfaceServo.cpp | 78 ++++++++++++++++++- .../gz_bridge/GZMixingInterfaceServo.hpp | 36 +++++++++ src/modules/simulation/gz_bridge/module.yaml | 39 +++++++++- 3 files changed, 149 insertions(+), 4 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..2fc656074c9b 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -33,6 +33,74 @@ #include "GZMixingInterfaceServo.hpp" + +float +GZMixingInterfaceServo::get_servo_angle_max(const size_t index) +{ + float angle; + + switch (index) { + case 0: {angle = _servo_max_angle_1.get(); break;} + + case 1: {angle = _servo_max_angle_2.get(); break;} + + case 2: {angle = _servo_max_angle_3.get(); break;} + + case 3: {angle = _servo_max_angle_4.get(); break;} + + case 4: {angle = _servo_max_angle_5.get(); break;} + + case 5: {angle = _servo_max_angle_6.get(); break;} + + case 6: {angle = _servo_max_angle_7.get(); break;} + + case 7: {angle = _servo_max_angle_8.get(); break;} + + default: {angle = NAN; break;} + } + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("max_angle: index out of range, i= %ld, expected i < 8", index); + return NAN; + } + + return math::radians(angle); +} + +float +GZMixingInterfaceServo::get_servo_angle_min(const size_t index) +{ + float angle; + + switch (index) { + case 0: {angle = _servo_min_angle_1.get(); break;} + + case 1: {angle = _servo_min_angle_2.get(); break;} + + case 2: {angle = _servo_min_angle_3.get(); break;} + + case 3: {angle = _servo_min_angle_4.get(); break;} + + case 4: {angle = _servo_min_angle_5.get(); break;} + + case 5: {angle = _servo_min_angle_6.get(); break;} + + case 6: {angle = _servo_min_angle_7.get(); break;} + + case 7: {angle = _servo_min_angle_8.get(); break;} + + default: {angle = NAN; break;} + + } + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("min_angle: index out of range, i= %ld, expected i < 8", index); + return NAN; + } + + return math::radians(angle); +} + bool GZMixingInterfaceServo::init(const std::string &model_name) { // /model/rascal_110_0/servo_2 @@ -46,6 +114,11 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) PX4_ERR("failed to advertise %s", servo_topic.c_str()); return false; } + + double min_val = get_servo_angle_min(i); + double max_val = get_servo_angle_max(i); + _angle_min_rad.push_back(min_val); + _angular_range_rad.push_back(max_val - min_val); } ScheduleNow(); @@ -64,8 +137,9 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { gz::msgs::Double servo_output; - ///TODO: Normalize output data - double output = (outputs[i] - 500) / 500.0; + + double output_range = _mixing_output.maxValue(i) - _mixing_output.minValue(i); + double output = _angle_min_rad[i] + _angular_range_rad[i] * (outputs[i] - _mixing_output.minValue(i)) / output_range; // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; servo_output.set_data(output); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 63345b3aed89..180f6cbca204 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -36,6 +36,7 @@ #include #include +#include // GZBridge mixing class for Servos. // It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling @@ -67,10 +68,45 @@ class GZMixingInterfaceServo : public OutputModuleInterface void Run() override; + /** + * @brief Get maximum configured angle of servo. + * @param index: servo index + * @return angle_max [rad] + */ + float get_servo_angle_max(const size_t index); + + /** + * @brief Get minimum configured angle of servo. + * @param index: servo index + * @return angle_min [rad] + */ + float get_servo_angle_min(const size_t index); + gz::transport::Node &_node; pthread_mutex_t &_node_mutex; MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; std::vector _servos_pub; + std::vector _angle_min_rad; + std::vector _angular_range_rad; + + DEFINE_PARAMETERS( + (ParamFloat) _servo_max_angle_1, + (ParamFloat) _servo_min_angle_1, + (ParamFloat) _servo_max_angle_2, + (ParamFloat) _servo_min_angle_2, + (ParamFloat) _servo_max_angle_3, + (ParamFloat) _servo_min_angle_3, + (ParamFloat) _servo_max_angle_4, + (ParamFloat) _servo_min_angle_4, + (ParamFloat) _servo_max_angle_5, + (ParamFloat) _servo_min_angle_5, + (ParamFloat) _servo_max_angle_6, + (ParamFloat) _servo_min_angle_6, + (ParamFloat) _servo_max_angle_7, + (ParamFloat) _servo_min_angle_7, + (ParamFloat) _servo_max_angle_8, + (ParamFloat) _servo_min_angle_8 + ) }; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 1b38e13d9883..e41aedb1e324 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -1,3 +1,6 @@ +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 + module_name: SIM_GZ actuator_output: show_subgroups_if: 'SIM_GZ_EN>=1' @@ -15,7 +18,7 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } - num_channels: 8 + num_channels: *max_num_servos - param_prefix: SIM_GZ_SV group_label: 'Servos' channel_label: 'Servo' @@ -24,7 +27,7 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } - num_channels: 8 + num_channels: *max_num_servos - param_prefix: SIM_GZ_WH group_label: 'Wheels' channel_label: 'Wheels' @@ -34,3 +37,35 @@ actuator_output: max: { min: 0, max: 200, default: 200 } failsafe: { min: 0, max: 200 } num_channels: 4 + +parameters: + - group: Geometry + definitions: + SIM_GZ_SV_MAXA${i}: + description: + short: Servo ${i} Angle at Maximum + long: | + Defines the angle when the servo is at the maximum. + Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MAXA. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_servos + instance_start: 1 + min: -180.0 + max: 180.0 + default: 45.0 + SIM_GZ_SV_MINA${i}: + description: + short: Servo ${i} Angle at Minimum + long: | + Defines the angle when the servo is at the minimum. + Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MINA. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_servos + instance_start: 1 + min: -180.0 + max: 180.0 + default: -45.0 From 1f48448beb7d1ce0052a58d82c2a757c9a79e383 Mon Sep 17 00:00:00 2001 From: Pernilla Date: Wed, 18 Dec 2024 16:13:21 +0100 Subject: [PATCH 201/211] Update other airframes with min max values different than default --- .../init.d-posix/airframes/4011_gz_lawnmower | 26 ++++++++++++------- .../airframes/4012_gz_rover_ackermann | 3 +++ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index d4c80a29092f..0fec76b5c4f6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -60,19 +60,25 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): -param set-default SIM_GZ_SV_FUNC3 203 -param set-default SIM_GZ_SV_MIN3 0 -param set-default SIM_GZ_SV_MAX3 1000 -param set-default SIM_GZ_SV_DIS3 500 -param set-default SIM_GZ_SV_FAIL3 500 +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_MIN1 0 +param set-default SIM_GZ_SV_MAX1 1000 +param set-default SIM_GZ_SV_DIS1 500 +param set-default SIM_GZ_SV_FAIL1 500 +param set-default SIM_GZ_SV_MAXA1 90 +param set-default SIM_GZ_SV_MINA1 -90 # Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204): # - on minimum when disarmed or failed: -param set-default SIM_GZ_SV_FUNC4 204 -param set-default SIM_GZ_SV_MIN4 0 -param set-default SIM_GZ_SV_MAX4 1000 -param set-default SIM_GZ_SV_DIS4 500 -param set-default SIM_GZ_SV_FAIL4 500 +param set-default SIM_GZ_SV_FUNC2 202 +param set-default SIM_GZ_SV_MIN2 0 +param set-default SIM_GZ_SV_MAX2 1000 +param set-default SIM_GZ_SV_DIS2 500 +param set-default SIM_GZ_SV_FAIL2 500 +param set-default SIM_GZ_SV_MAXA2 90 +param set-default SIM_GZ_SV_MINA2 -90 + +param set-default CA_SV_CS_COUNT 2 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 9e54c4015909..e058b87dc42b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -47,5 +47,8 @@ param set-default SIM_GZ_WH_MAX1 200 param set-default SIM_GZ_WH_DIS1 100 # Steering +param set-default SIM_GZ_SV_MAXA1 30 +param set-default SIM_GZ_SV_MINA1 -30 +param set-default CA_SV_CS_COUNT 1 param set-default SIM_GZ_SV_FUNC1 201 param set-default SIM_GZ_SV_REV 1 From 367a8a7fc4c4b7d07fc57fb00100ecc779a7d7be Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 19 Dec 2024 09:40:01 +0100 Subject: [PATCH 202/211] update gz submodule to get new tiltrotor sim Signed-off-by: Silvan Fuhrer --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 19fd9248c499..230450cc817d 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 19fd9248c499b05975b5b57ad72a07826834c1ab +Subproject commit 230450cc817dd7675612ed5ec72ee59b6989d367 From 839010eeab8b0ff6f472d102d89d367eddcbaff9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 19 Dec 2024 07:13:20 +0100 Subject: [PATCH 203/211] Navigator: make ADSB handling optional --- boards/bitcraze/crazyflie21/default.px4board | 1 + boards/diatone/mamba-f405-mk2/default.px4board | 1 + boards/flywoo/gn-f405/default.px4board | 1 + boards/holybro/kakutef7/default.px4board | 1 + boards/omnibus/f4sd/default.px4board | 1 + boards/px4/fmu-v2/default.px4board | 1 + boards/raspberrypi/pico/default.px4board | 1 + src/modules/navigator/Kconfig | 8 ++++++++ src/modules/navigator/navigator.h | 12 ++++++++---- src/modules/navigator/navigator_main.cpp | 12 +++++++++--- 10 files changed, 32 insertions(+), 7 deletions(-) diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 0c268b6983a6..13c44d61d964 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index e8ee5ec656c8..14e8219028ff 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -36,6 +36,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/flywoo/gn-f405/default.px4board b/boards/flywoo/gn-f405/default.px4board index 54d17325bc92..bd7ea9015d2a 100644 --- a/boards/flywoo/gn-f405/default.px4board +++ b/boards/flywoo/gn-f405/default.px4board @@ -27,6 +27,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 8aaa037cc55a..49b8f9e0d5e5 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 8d3c58a7f0f5..470b95db5db1 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -31,6 +31,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index b796d1bfbf49..479e394f6f55 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board index 410f8d4788a9..d481cf3478c4 100644 --- a/boards/raspberrypi/pico/default.px4board +++ b/boards/raspberrypi/pico/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/src/modules/navigator/Kconfig b/src/modules/navigator/Kconfig index 83e42cc594db..429acd60e763 100644 --- a/src/modules/navigator/Kconfig +++ b/src/modules/navigator/Kconfig @@ -19,3 +19,11 @@ menuconfig MODE_NAVIGATOR_VTOL_TAKEOFF 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 + +menuconfig NAVIGATOR_ADSB + bool "Include traffic reporting and avoidance" + default y + depends on MODULES_NAVIGATOR + ---help--- + Add support for acting on ADSB transponder_report or ADSB_VEHICLE MAVLink messages. + Actions are warnings, Loiter, Land and RTL without climb. diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b3057e53753f..5e30d22ccc95 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,8 +57,11 @@ #include "GeofenceBreachAvoidance/geofence_breach_avoidance.h" +#if CONFIG_NAVIGATOR_ADSB #include +#endif // CONFIG_NAVIGATOR_ADSB #include +#include #include #include #include @@ -136,14 +139,14 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_vehicle_cmd(vehicle_command_s *vcmd); +#if CONFIG_NAVIGATOR_ADSB /** * Check nearby traffic for potential collisions */ void check_traffic(); - void take_traffic_conflict_action(); - void run_fake_traffic(); +#endif // CONFIG_NAVIGATOR_ADSB /** * Setters @@ -365,7 +368,10 @@ class Navigator : public ModuleBase, public ModuleParams Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ +#if CONFIG_NAVIGATOR_ADSB AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */ + traffic_buffer_s _traffic_buffer{}; +#endif // CONFIG_NAVIGATOR_ADSB NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */ @@ -381,8 +387,6 @@ class Navigator : public ModuleBase, public ModuleParams float _cruising_speed_current_mode{-1.0f}; float _mission_throttle{NAN}; - traffic_buffer_s _traffic_buffer{}; - bool _is_capturing_images{false}; // keep track if we need to stop capturing images diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5f397786fbe4..cce3031ba1bf 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -143,9 +142,11 @@ void Navigator::params_update() } _mission.set_command_timeout(_param_mis_command_tout.get()); +#if CONFIG_NAVIGATOR_ADSB _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), _param_nav_traff_a_ver.get(), _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); +#endif // CONFIG_NAVIGATOR_ADSB } void Navigator::run() @@ -752,8 +753,10 @@ void Navigator::run() } } +#if CONFIG_NAVIGATOR_ADSB /* Check for traffic */ check_traffic(); +#endif // CONFIG_NAVIGATOR_ADSB /* Check geofence violation */ geofence_breach_check(); @@ -1222,6 +1225,7 @@ void Navigator::load_fence_from_file(const char *filename) _geofence.loadFromFile(filename); } +#if CONFIG_NAVIGATOR_ADSB void Navigator::take_traffic_conflict_action() { @@ -1251,12 +1255,10 @@ void Navigator::take_traffic_conflict_action() } } - } void Navigator::run_fake_traffic() { - _adsb_conflict.run_fake_traffic(get_global_position()->lat, get_global_position()->lon, get_global_position()->alt); } @@ -1283,6 +1285,7 @@ void Navigator::check_traffic() _adsb_conflict.remove_expired_conflicts(); } +#endif // CONFIG_NAVIGATOR_ADSB bool Navigator::abort_landing() { @@ -1325,11 +1328,14 @@ int Navigator::custom_command(int argc, char *argv[]) get_instance()->load_fence_from_file(GEOFENCE_FILENAME); return 0; +#if CONFIG_NAVIGATOR_ADSB + } else if (!strcmp(argv[0], "fake_traffic")) { get_instance()->run_fake_traffic(); return 0; +#endif // CONFIG_NAVIGATOR_ADSB } return print_usage("unknown command"); From 11d7dd41fdfc4e445ca3effd3ad2e7266220931c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Dec 2024 15:40:46 +0100 Subject: [PATCH 204/211] FlightTaskTransition: clean up and simplify --- .../tasks/Transition/FlightTaskTransition.cpp | 149 +++++------------- .../tasks/Transition/FlightTaskTransition.hpp | 25 +-- 2 files changed, 42 insertions(+), 132 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index badf32fe7fcb..3dd2c6b6a2f2 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -41,21 +41,15 @@ using namespace matrix; FlightTaskTransition::FlightTaskTransition() { - _param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF"); - _param_handle_vt_b_dec_i = param_find("VT_B_DEC_I"); - _param_handle_vt_b_dec_mss = param_find("VT_B_DEC_MSS"); - - updateParametersFromStorage(); + param_get(param_find("FW_PSP_OFF"), &_param_fw_psp_off); + param_get(param_find("VT_B_DEC_I"), &_param_vt_b_dec_i); + param_get(param_find("VT_B_DEC_MSS"), &_param_vt_b_dec_mss); } bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); - _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); - - _decel_error_bt_int = 0.f; - if (PX4_ISFINITE(last_setpoint.velocity[2])) { _vel_z_filter.reset(last_setpoint.velocity[2]); @@ -63,9 +57,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _vel_z_filter.reset(_velocity(2)); } - _velocity_setpoint(2) = _vel_z_filter.getState(); - - if (isVtolFrontTransition()) { + if (_sub_vehicle_status.get().in_transition_to_fw) { _gear.landing_gear = landing_gear_s::GEAR_UP; } else { @@ -77,9 +69,10 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) bool FlightTaskTransition::updateInitialize() { - updateParameters(); - updateSubscribers(); - return FlightTask::updateInitialize(); + bool ret = FlightTask::updateInitialize(); + _sub_vehicle_status.update(); + _sub_position_sp_triplet.update(); + return ret; } bool FlightTaskTransition::update() @@ -88,122 +81,56 @@ bool FlightTaskTransition::update() // tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint bool ret = FlightTask::update(); - _position_setpoint.setAll(NAN); + // slowly move vertical velocity setpoint to zero + _velocity_setpoint(2) = _vel_z_filter.update(0.0f, _deltatime); - // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees + // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_fw_psp_off // and zero roll angle - const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); - float pitch_setpoint = math::radians(_param_pitch_cruise_degrees); + float pitch_setpoint = math::radians(_param_fw_psp_off); - if (isVtolBackTransition()) { + if (!_sub_vehicle_status.get().in_transition_to_fw) { pitch_setpoint = computeBackTranstionPitchSetpoint(); } - _acceleration_setpoint.xy() = tmp * tanf(pitch_setpoint) * CONSTANTS_ONE_G; - - // slowly move vertical velocity setpoint to zero - _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); - _velocity_setpoint(2) = _vel_z_filter.update(0.0f); + // Calculate horizontal acceleration components to follow a pitch setpoint with the current vehicle heading + const Vector2f horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); + _acceleration_setpoint.xy() = tanf(pitch_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction; _yaw_setpoint = NAN; return ret; } -void FlightTaskTransition::updateParameters() -{ - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - updateParametersFromStorage(); - } -} - -void FlightTaskTransition::updateParametersFromStorage() -{ - if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { - param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); - } - - if (_param_handle_vt_b_dec_i != PARAM_INVALID) { - param_get(_param_handle_vt_b_dec_i, &_param_vt_b_dec_i); - } - - if (_param_handle_vt_b_dec_mss != PARAM_INVALID) { - param_get(_param_handle_vt_b_dec_mss, &_param_vt_b_dec_mss); - } -} - -void FlightTaskTransition::updateSubscribers() -{ - _sub_vehicle_status.update(); - _sub_position_sp_triplet.update(); - _sub_vehicle_local_position.update(); -} - -bool FlightTaskTransition::isVtolFrontTransition() const -{ - return _sub_vehicle_status.get().in_transition_mode - && _sub_vehicle_status.get().in_transition_to_fw; - -} - -bool FlightTaskTransition::isVtolBackTransition() const -{ - return _sub_vehicle_status.get().in_transition_mode - && !_sub_vehicle_status.get().in_transition_to_fw; -} - float FlightTaskTransition::computeBackTranstionPitchSetpoint() { - const vehicle_local_position_s &local_pos = _sub_vehicle_local_position.get(); - const position_setpoint_s ¤t_pos_sp = _sub_position_sp_triplet.get().current; + const Vector2f position_xy{_position}; + const Vector2f velocity_xy{_velocity}; + const Vector2f velocity_xy_direction = velocity_xy.unit_or_zero(); - // Retrieve default decelaration setpoint from param - const float default_deceleration_sp = _param_vt_b_dec_mss; + float deceleration_setpoint = _param_vt_b_dec_mss; - float deceleration_sp = default_deceleration_sp; + if (_sub_position_sp_triplet.get().current.valid && _sub_vehicle_local_position.get().xy_global + && position_xy.isAllFinite() && velocity_xy.isAllFinite()) { + Vector2f position_setpoint_local; + _geo_projection.project(_sub_position_sp_triplet.get().current.lat, _sub_position_sp_triplet.get().current.lon, + position_setpoint_local(0), position_setpoint_local(1)); - const Vector2f position_local{local_pos.x, local_pos.y}; - const Vector2f velocity_local{local_pos.vx, local_pos.vy}; - const Vector2f acceleration_local{local_pos.ax, local_pos.ay}; + const Vector2f pos_to_target = position_setpoint_local - position_xy; // backtransition end-point w.r.t. vehicle + const float dist_to_target_in_moving_direction = pos_to_target.dot(velocity_xy_direction); - const float accel_in_flight_direction = acceleration_local.dot(velocity_local.unit_or_zero()); - - if (current_pos_sp.valid && local_pos.xy_valid && local_pos.xy_global) { - - Vector2f position_setpoint_local {0.f, 0.f}; - _geo_projection.project(current_pos_sp.lat, current_pos_sp.lon, position_setpoint_local(0), - position_setpoint_local(1)); - - const Vector2f pos_sp_dist = position_setpoint_local - position_local; // backtransition end-point w.r.t. vehicle - - const float dist_body_forward = pos_sp_dist.dot(velocity_local.unit_or_zero()); - - // Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle - if (dist_body_forward > FLT_EPSILON && PX4_ISFINITE(velocity_local.norm())) { - - // Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped - deceleration_sp = velocity_local.norm_squared() / (2.f * dist_body_forward); - // Limit the deceleration setpoint to maximal twice the default deceleration setpoint (param) - deceleration_sp = math::constrain(deceleration_sp, 0.f, 2.f * default_deceleration_sp); + if (dist_to_target_in_moving_direction > FLT_EPSILON) { + // Backtransition target point is ahead of the vehicle, compute the desired deceleration + deceleration_setpoint = velocity_xy.norm_squared() / (2.f * dist_to_target_in_moving_direction); + deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss); } } - // positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number) - const float deceleration_error = deceleration_sp - (-accel_in_flight_direction); - - updateBackTransitioDecelerationErrorIntegrator(deceleration_error); + // Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow + const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay}; + const float deceleration = -acceleration_xy.dot(velocity_xy_direction); + const float deceleration_error = deceleration_setpoint - deceleration; + // Update back-transition deceleration error integrator + _decel_error_bt_int += (_param_vt_b_dec_i * deceleration_error) * _deltatime; + _decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, DECELERATION_INTEGRATOR_LIMIT); return _decel_error_bt_int; } - -void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error) -{ - const float integrator_input = _param_vt_b_dec_i * deceleration_error; - - _decel_error_bt_int += integrator_input * math::constrain(_deltatime, 0.001f, 0.1f); - _decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, _pitch_limit_bt); -} diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp index 901da114cc8b..12687223b598 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -61,35 +61,18 @@ class FlightTaskTransition : public FlightTask bool update() override; private: - - static constexpr float _vel_z_filter_time_const = 2.0f; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + static constexpr float VERTICAL_VELOCITY_TIME_CONSTANT = 2.0f; + static constexpr float DECELERATION_INTEGRATOR_LIMIT = .3f; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; uORB::SubscriptionData _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)}; - param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID}; - float _param_pitch_cruise_degrees{0.f}; - param_t _param_handle_vt_b_dec_i{PARAM_INVALID}; + float _param_fw_psp_off{0.f}; float _param_vt_b_dec_i{0.f}; - param_t _param_handle_vt_b_dec_mss{PARAM_INVALID}; float _param_vt_b_dec_mss{0.f}; - AlphaFilter _vel_z_filter; + AlphaFilter _vel_z_filter{VERTICAL_VELOCITY_TIME_CONSTANT}; float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value - static constexpr float _pitch_limit_bt = .3f; ///< Bactransition pitch limit - - void updateParameters(); - void updateParametersFromStorage(); - - void updateSubscribers(); - - bool isVtolFrontTransition() const; - bool isVtolBackTransition() const; - float computeBackTranstionPitchSetpoint(); - void updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error); - }; From 34bcc277a564559495de77e2a52f8a7201cdad85 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Dec 2024 15:59:45 +0100 Subject: [PATCH 205/211] FlightTaskTransition: keep high decelration when overshooting the transition target --- .../tasks/Transition/FlightTaskTransition.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index 3dd2c6b6a2f2..06e69a7e6193 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -120,8 +120,12 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() if (dist_to_target_in_moving_direction > FLT_EPSILON) { // Backtransition target point is ahead of the vehicle, compute the desired deceleration deceleration_setpoint = velocity_xy.norm_squared() / (2.f * dist_to_target_in_moving_direction); - deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss); + + } else { + deceleration_setpoint = 2.f * _param_vt_b_dec_mss; } + + deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss); } // Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow From 1239f0aaed45c85714c29b16841f49a5e3e55db5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 19 Dec 2024 15:21:22 +0100 Subject: [PATCH 206/211] FlightTaskTransition: Comment why using invalid horizontal velocity works --- .../tasks/Transition/FlightTaskTransition.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index 06e69a7e6193..727d7cd52934 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -104,7 +104,8 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() { const Vector2f position_xy{_position}; const Vector2f velocity_xy{_velocity}; - const Vector2f velocity_xy_direction = velocity_xy.unit_or_zero(); + const Vector2f velocity_xy_direction = + velocity_xy.unit_or_zero(); // Zero when velocity invalid Vector2f(NAN, NAN).unit_or_zero() == Vector2f(0.f, 0.f) float deceleration_setpoint = _param_vt_b_dec_mss; @@ -130,7 +131,7 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() // Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay}; - const float deceleration = -acceleration_xy.dot(velocity_xy_direction); + const float deceleration = -acceleration_xy.dot(velocity_xy_direction); // Zero when velocity invalid const float deceleration_error = deceleration_setpoint - deceleration; // Update back-transition deceleration error integrator From ad799b64b71c8b729d8be5ed934eda8d56ed42d7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Dec 2024 19:54:23 +0100 Subject: [PATCH 207/211] mc_pos_control: shorten parameter descriptions to a readable size --- .../multicopter_altitude_mode_params.c | 16 +++++++--------- .../multicopter_position_mode_params.c | 11 +++++------ .../multicopter_stabilized_mode_params.c | 16 +++++++--------- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index 9174b1ef0d83..4f617d009296 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -73,15 +73,13 @@ PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); /** * Altitude reference mode * - * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in - * flight due to sensor drift. - * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down - * with terrain height variation. Requires a distance to ground sensor. The height controller will - * revert to using height above origin if the distance to ground estimate becomes invalid as indicated - * by the local_position.distance_bottom_valid message being false. - * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative - * to earth frame origin when moving horizontally. - * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. + * Control height + * 0: relative earth frame origin which may drift due to sensors + * 1: relative to ground (requires distance sensor) which changes with terrain variation. + * It will revert to relative earth frame if the distance to ground estimate becomes invalid. + * 2: relative to ground (requires distance sensor) when stationary + * and relative to earth frame when moving horizontally. + * The speed threshold is MPC_HOLD_MAX_XY * * @min 0 * @max 2 diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index 9fce344319c8..b70bb95a3e2a 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -35,12 +35,12 @@ * Position/Altitude mode variant * * The supported sub-modes are: - * - "Direct velocity": + * Direct velocity: * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * - "Acceleration based": - * Sticks map to acceleration and there's a virtual brake drag + * Acceleration based: + * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity * @value 4 Acceleration based @@ -114,9 +114,8 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); /** * Maximum horizontal and vertical jerk in Position/Altitude mode * - * Limit the maximum jerk of the vehicle (how fast the acceleration can change). - * A lower value leads to smoother motions but limits agility - * (how fast it can change directions or break). + * Limit the maximum jerk (acceleration change) of the vehicle. + * A lower value leads to smoother motions but limits agility. * * Setting this to the maximum value essentially disables the limit. * diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c index 481d9cce66f9..50cf5a653627 100644 --- a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c @@ -75,18 +75,16 @@ PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); /** * Thrust curve mapping in Stabilized Mode * - * This parameter defines how the throttle stick input is mapped to collective thrust - * in Stabilized mode. + * Defines how the throttle stick is mapped to collective thrust in Stabilized mode. * - * In case the default is used ('Rescale to hover thrust'), the stick input is linearly - * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * Rescale to hover thrust: + * Stick input is linearly rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). * - * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful - * in case the hover thrust is very low and the default would lead to too much distortion - * (e.g. if hover thrust is set to 20%, then 80% of the upper thrust range is squeezed into the - * upper half of the stick range). + * No Rescale: + * Directly map the stick 1:1 to the output. + * Can be useful with very low hover thrust which leads to much distortion and the upper half getting sensitive. * - * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * With MPC_THR_HOVER 0.5 both modes are the same. * * @value 0 Rescale to hover thrust * @value 1 No Rescale From 5cf85e320a7ee4b53b78b5fbe98b688068d5bce0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 4 Dec 2024 17:51:53 +0100 Subject: [PATCH 208/211] offboardCheck: acceleration setpoints only require vehicle attitude control --- .../commander/HealthAndArmingChecks/checks/offboardCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp index c2e2fb15988b..331b12b27282 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp @@ -56,7 +56,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) } else if (offboard_control_mode.velocity && reporter.failsafeFlags().local_velocity_invalid) { offboard_available = false; - } else if (offboard_control_mode.acceleration && reporter.failsafeFlags().local_velocity_invalid) { + } else if (offboard_control_mode.acceleration && reporter.failsafeFlags().attitude_invalid) { // OFFBOARD acceleration handled by position controller offboard_available = false; } From 61961350f9a7a6631bb0990293a29ea11ddfcd78 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 5 Sep 2024 18:36:23 +0200 Subject: [PATCH 209/211] mavlink_receiver: don't publish out of range joystick input Note that the MAVLink definition explicitly writes "A value of INT16_MAX indicates that this axis is invalid." which before this change was happily executed. --- src/modules/mavlink/mavlink_receiver.cpp | 31 ++++++++++++++++-------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b3237241c312..e698c851863a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2145,25 +2145,36 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } manual_control_setpoint_s manual_control_setpoint{}; - manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; - manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; + + if (math::isInRange((int)mavlink_manual_control.x, -1000, 1000)) { manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; } + + if (math::isInRange((int)mavlink_manual_control.y, -1000, 1000)) { manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; } + // For backwards compatibility at the moment interpret throttle in range [0,1000] - manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; - manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; + if (math::isInRange((int)mavlink_manual_control.z, 0, 1000)) { manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; } + + if (math::isInRange((int)mavlink_manual_control.r, -1000, 1000)) { manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; } + // Pass along the button states manual_control_setpoint.buttons = mavlink_manual_control.buttons; - if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 2) + && math::isInRange((int)mavlink_manual_control.aux1, -1000, 1000)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 3) + && math::isInRange((int)mavlink_manual_control.aux2, -1000, 1000)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 4) + && math::isInRange((int)mavlink_manual_control.aux3, -1000, 1000)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 5) + && math::isInRange((int)mavlink_manual_control.aux4, -1000, 1000)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 6) + && math::isInRange((int)mavlink_manual_control.aux5, -1000, 1000)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 7) + && math::isInRange((int)mavlink_manual_control.aux6, -1000, 1000)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); From b765769f50a0a830d96e3a416a55bd229b0b6bce Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 20 Dec 2024 11:21:32 +0300 Subject: [PATCH 210/211] Navigator: Set altitude acceptance radius to infinity when moving to land point after transition (#24115) * Navigator: set alt acceptance radius to infinity for land waypoint after backtransition -> avoid vehicle with depleted battery from not reaching the alt setpoint and getting stuck Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 10 ++++++++-- src/modules/navigator/navigator_main.cpp | 9 +++++++-- src/modules/navigator/rtl_direct.cpp | 1 + src/modules/navigator/rtl_direct_mission_land.cpp | 10 ++++++++-- src/modules/navigator/rtl_mission_fast_reverse.cpp | 7 +++++++ 5 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8c5078084749..08a200eda02f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -211,8 +211,14 @@ void Mission::setActiveMissionItems() // prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() && - _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT; + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (fw_on_mission_landing || mc_landing_after_transition) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cce3031ba1bf..201eeb6123f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1145,8 +1145,13 @@ float Navigator::get_altitude_acceptance_radius() const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { + const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; + + if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { + alt_acceptance_radius = curr_sp.alt_acceptance_radius; + + } else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) + && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 2c4c172f8b4d..c264c6fc88b4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -355,6 +355,7 @@ void RtlDirect::set_rtl_item() pos_yaw_sp.alt = loiter_altitude; pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled + altitude_acceptance_radius = FLT_MAX; setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); _navigator->reset_position_setpoint(pos_sp_triplet->previous); diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index d5256535c500..6b7f2f649a6d 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -215,8 +215,14 @@ void RtlDirectMissionLand::setActiveMissionItems() // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && MissionBase::isLanding() - && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT; + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (fw_on_mission_landing || mc_landing_after_transition) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; } } diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index eb5f3d102246..707219b1b37a 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -155,6 +155,13 @@ void RtlMissionFastReverse::setActiveMissionItems() } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (mc_landing_after_transition) { + pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; + } } issue_command(_mission_item); From defccfa99b01b74f106b84f7b778d46fc992a469 Mon Sep 17 00:00:00 2001 From: Mahima Yoga <61349849+mahimayoga@users.noreply.github.com> Date: Fri, 20 Dec 2024 14:47:22 +0100 Subject: [PATCH 211/211] Collision Prevention: Scale obstacle distance with vehicle attitude for varying sensor orientations (#24107) --- src/lib/collision_prevention/CollisionPrevention.cpp | 12 ++++++++---- src/modules/simulation/gz_bridge/GZBridge.cpp | 10 ++++++++++ 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 0733f1b289d4..b2c62d4906c6 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -400,11 +400,15 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f))); - // rotate vehicle attitude into the sensor body frame - Quatf attitude_sensor_frame = vehicle_attitude; - attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); - float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify + const Vector3f forward_vector(1.0f, 0.0f, 0.0f); + + const Quatf q_sensor_rotation = vehicle_attitude * q_sensor; + + const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector); + + const float sensor_dist_scale = rotated_sensor_vector.xy().norm(); if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index f69be21062db..2dc1ef13e4f4 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -800,7 +800,10 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) pose_orientation.y(), pose_orientation.z()); + const gz::math::Quaterniond q_left(0.7071068, 0, 0, -0.7071068); + const gz::math::Quaterniond q_front(0.7071068, 0.7071068, 0, 0); + const gz::math::Quaterniond q_down(0, 1, 0, 0); if (q_sensor.Equal(q_front, 0.03)) { @@ -809,8 +812,15 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) } else if (q_sensor.Equal(q_down, 0.03)) { distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + } else if (q_sensor.Equal(q_left, 0.03)) { + distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + } else { distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; + distance_sensor.q[0] = q_sensor.W(); + distance_sensor.q[1] = q_sensor.X(); + distance_sensor.q[2] = q_sensor.Y(); + distance_sensor.q[3] = q_sensor.Z(); } _distance_sensor_pub.publish(distance_sensor);