diff --git a/.gitignore b/.gitignore index d2d63ed98612b69e0bb748cd74a300734f41e1a2..9df3b454f410d9279fb0a25c11ee4fbb2d9e752c 100644 --- a/.gitignore +++ b/.gitignore @@ -23,7 +23,9 @@ build *.sublime-project .vscode/* - store.json scripts/event_header_generator/generated/ scripts/event_header_generator/venv/ + +**/generated +**/scxmls diff --git a/README.md b/README.md index 6ef93476980cd8cec5048709b66b0cc07167bdbf..feb2371aff0701eb275648c15053d092af94cd36 100644 --- a/README.md +++ b/README.md @@ -1,43 +1,39 @@ -# R2A-HERMES OBSW [](https://git.skywarder.eu/r2a/skyward-boardcore/commits/master) +# Lynx On-Board Software [](https://git.skywarder.eu/r2a/skyward-boardcore/commits/master) -*On Board software for R2A-Hermes* +*On Board software for Lynx* -To clone, use the `git clone --recurse-submodules` option. +To clone, use `git clone --recurse-submodules git@git.skywarder.eu:scs/hermes/r2a-obsw.git`. To build, use `sbs` (for more info, type `./sbs --help` on Linux or `sbs --help` on Windows). +## Folder structure - -## Folder Structure - -| Folder | Content | -| ---------------------- | --------------------------------------------------- | -| **src/** | **sources!** | -| boards/ | Classes and components, divided by board. | -| entrypoints/ | Each file here is a "*main*" to be built with SBS. | -| tests/ | Tests that can be built with SBS. | -| **skyward-boardcore/** | Provides the build system (SBS) and common drivers. | -| **bin/** | Compiled binaries, to be flashed on boards. | -| **build/** | MIOSIX Makefiles generated by SBS (*not important*) | -| **obj/** | Building stuff (*not important*) | +| Folder | Content | +| ---------------------- | ------------------------------------------------------------ | +| src/boards/ | Classes and components, divided by board. | +| src/entrypoints/ | Each file here is a "*main*" to be built with SBS. | +| src/tests/ | Tests that can be built with SBS. | +| skyward-boardcore/ | Provides the build system (SBS) and common drivers. | +| bin/ | Compiled binaries generated by SBS, to be flashed on boards. | +| build/ | MIOSIX Makefiles generated by SBS (*not important*) | +| obj/ | Building stuff (*not important*) | ## Useful entrypoints -### Death Stack Test Suite -`src/entrypoints/death-stack-testsuite.cpp` +### Death Stack X Test Suite `src/entrypoints/death-stack-x-testsuite.cpp` + Interactive entrypoint to test various aspects of the Death Stack hardware / software: -| Test | Description | -| ----- | ----- | -| Test All Sensors | Reads and displays values from all the sensors | -| Thermal Cutter Test | Test the cutters with user-configurable parameters | -| Nosecone motor | Test nosecone motor opening / closing (Hermes V0) | -| Sensors + TMTC | Sample all the sensor with flight parameters and sends high rate telemetry, while also listening for telecommands | -| XBee send/receive | Sends and receives data trough the XBee module | -| Logger | Tests the SDCARD / logger | +| Test | Description | +| ----------------- | -------------------------------------------------------- | +| Test power board | Allows to test battery voltage, cutters and servo motors | +| Test stm board | Allows to test leds and external oscillator | +| Test rf board | Allows to test the IMUs, GPS and SD Card | +| Test analog board | Allows to test the pressure sensors and detachment pins | + +### Death Stack X Entry `src/entrypoints/death-stack-x-entry.cpp` -### Death Stack Entry Main entrypoint to be used for flight ## Contributing diff --git a/bin_delivery/13_05_2019/README.md b/bin_delivery/13_05_2019/README.md deleted file mode 100644 index 73fbf28cfdeb8a6793a9380b2f6afd3bfaa428d3..0000000000000000000000000000000000000000 --- a/bin_delivery/13_05_2019/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# Tests 13/05/2019 - -**Working** -* ramtest (changed Makefile.inc) -* test-all-sensors (all a part from GPS) -* xbee-send-rcv -* test-actuators (thc1 and 2) -* test-logger - -**Not working** -* ADIS -* IMU soufiane -* Digital pressure sensor - -**Untested** -* GPS -* Nosecone motor -* Rogallo servos - diff --git a/bin_delivery/13_05_2019/test-all-sensors/test-all-sensors.bin b/bin_delivery/hermes/13_05_2019/test-all-sensors/test-all-sensors.bin similarity index 100% rename from bin_delivery/13_05_2019/test-all-sensors/test-all-sensors.bin rename to bin_delivery/hermes/13_05_2019/test-all-sensors/test-all-sensors.bin diff --git a/bin_delivery/13_05_2019/test-logger/test-logger.bin b/bin_delivery/hermes/13_05_2019/test-logger/test-logger.bin similarity index 100% rename from bin_delivery/13_05_2019/test-logger/test-logger.bin rename to bin_delivery/hermes/13_05_2019/test-logger/test-logger.bin diff --git a/bin_delivery/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin b/bin_delivery/hermes/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin similarity index 100% rename from bin_delivery/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin rename to bin_delivery/hermes/13_05_2019/xbee-send-rcv/xbee-send-rcv.bin diff --git a/bin_delivery/14_05_2019/README.md b/bin_delivery/hermes/14_05_2019/README.md similarity index 100% rename from bin_delivery/14_05_2019/README.md rename to bin_delivery/hermes/14_05_2019/README.md diff --git a/bin_delivery/14_05_2019/test-mavchannel/test-mavchannel.bin b/bin_delivery/hermes/14_05_2019/test-mavchannel/test-mavchannel.bin similarity index 100% rename from bin_delivery/14_05_2019/test-mavchannel/test-mavchannel.bin rename to bin_delivery/hermes/14_05_2019/test-mavchannel/test-mavchannel.bin diff --git a/bin_delivery/14_05_2019/test-mavlink/test-mavlink.bin b/bin_delivery/hermes/14_05_2019/test-mavlink/test-mavlink.bin similarity index 100% rename from bin_delivery/14_05_2019/test-mavlink/test-mavlink.bin rename to bin_delivery/hermes/14_05_2019/test-mavlink/test-mavlink.bin diff --git a/bin_delivery/15_05_2019/README.md b/bin_delivery/hermes/15_05_2019/README.md similarity index 100% rename from bin_delivery/15_05_2019/README.md rename to bin_delivery/hermes/15_05_2019/README.md diff --git a/bin_delivery/15_05_2019/test-motor/test-motor.bin b/bin_delivery/hermes/15_05_2019/test-motor/test-motor.bin similarity index 100% rename from bin_delivery/15_05_2019/test-motor/test-motor.bin rename to bin_delivery/hermes/15_05_2019/test-motor/test-motor.bin diff --git a/bin_delivery/15_05_2019/test-tmtc/test-tmtc.bin b/bin_delivery/hermes/15_05_2019/test-tmtc/test-tmtc.bin similarity index 100% rename from bin_delivery/15_05_2019/test-tmtc/test-tmtc.bin rename to bin_delivery/hermes/15_05_2019/test-tmtc/test-tmtc.bin diff --git a/bin_delivery/20_05_19/death-stack-entry.bin b/bin_delivery/hermes/20_05_19/death-stack-entry.bin similarity index 100% rename from bin_delivery/20_05_19/death-stack-entry.bin rename to bin_delivery/hermes/20_05_19/death-stack-entry.bin diff --git a/bin_delivery/28_05_19/test-all-sensors/test-all-sensors.bin b/bin_delivery/hermes/28_05_19/test-all-sensors/test-all-sensors.bin similarity index 100% rename from bin_delivery/28_05_19/test-all-sensors/test-all-sensors.bin rename to bin_delivery/hermes/28_05_19/test-all-sensors/test-all-sensors.bin diff --git a/bin_delivery/hermes/final/death-stack-entry/death-stack-entry.bin b/bin_delivery/hermes/final/death-stack-entry/death-stack-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..76a90770d2b2924f709d1516f26cb6a927156d04 Binary files /dev/null and b/bin_delivery/hermes/final/death-stack-entry/death-stack-entry.bin differ diff --git a/bin_delivery/13_05_2019/ramtest/ramtest.bin b/bin_delivery/hermes/final/ramtest/ramtest.bin similarity index 100% rename from bin_delivery/13_05_2019/ramtest/ramtest.bin rename to bin_delivery/hermes/final/ramtest/ramtest.bin diff --git a/bin_delivery/lynx/01_09_2021/death-stack-x-entry.bin b/bin_delivery/lynx/01_09_2021/death-stack-x-entry.bin new file mode 100644 index 0000000000000000000000000000000000000000..c2cabaefee395a0047aeb57f1251ff87c3ef0e81 Binary files /dev/null and b/bin_delivery/lynx/01_09_2021/death-stack-x-entry.bin differ diff --git a/bin_delivery/lynx/01_09_2021/death-stack-x-hil-entry.bin b/bin_delivery/lynx/01_09_2021/death-stack-x-hil-entry.bin new file mode 100644 index 0000000000000000000000000000000000000000..94f428f2556dc634a90d784d2a4040c43c595ff0 Binary files /dev/null and b/bin_delivery/lynx/01_09_2021/death-stack-x-hil-entry.bin differ diff --git a/bin_delivery/lynx/01_09_2021/ramtest.bin b/bin_delivery/lynx/01_09_2021/ramtest.bin new file mode 100644 index 0000000000000000000000000000000000000000..0e78678a46015a35e30eb1ab5f0cbdf3195a6944 Binary files /dev/null and b/bin_delivery/lynx/01_09_2021/ramtest.bin differ diff --git a/bin_delivery/lynx/09_09_2021/bmx160-calibration-entry.bin b/bin_delivery/lynx/09_09_2021/bmx160-calibration-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..ca69e32e04d4fc86c8d3f8b319d82cc546ca7701 Binary files /dev/null and b/bin_delivery/lynx/09_09_2021/bmx160-calibration-entry.bin differ diff --git a/bin_delivery/lynx/09_09_2021/death-stack-x-entry.bin b/bin_delivery/lynx/09_09_2021/death-stack-x-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..6c65d9f57303075f20d7a2236411967106fee976 Binary files /dev/null and b/bin_delivery/lynx/09_09_2021/death-stack-x-entry.bin differ diff --git a/bin_delivery/lynx/09_09_2021/death-stack-x-hil-entry.bin b/bin_delivery/lynx/09_09_2021/death-stack-x-hil-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..658d9a03c33fde897cc36d06083af31492257656 Binary files /dev/null and b/bin_delivery/lynx/09_09_2021/death-stack-x-hil-entry.bin differ diff --git a/bin_delivery/lynx/09_09_2021/death-stack-x-testsuite.bin b/bin_delivery/lynx/09_09_2021/death-stack-x-testsuite.bin new file mode 100755 index 0000000000000000000000000000000000000000..aad9d8a6378c8fbed073bfcf720b93f6eef2ef27 Binary files /dev/null and b/bin_delivery/lynx/09_09_2021/death-stack-x-testsuite.bin differ diff --git a/bin_delivery/lynx/09_09_2021/ramtest.bin b/bin_delivery/lynx/09_09_2021/ramtest.bin new file mode 100755 index 0000000000000000000000000000000000000000..7a288a98220a20c0e0a0aded228b1bef3d89c285 Binary files /dev/null and b/bin_delivery/lynx/09_09_2021/ramtest.bin differ diff --git a/bin_delivery/lynx/13_09_2021/bmx160-calibration-entry.bin b/bin_delivery/lynx/13_09_2021/bmx160-calibration-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..ca69e32e04d4fc86c8d3f8b319d82cc546ca7701 Binary files /dev/null and b/bin_delivery/lynx/13_09_2021/bmx160-calibration-entry.bin differ diff --git a/bin_delivery/lynx/13_09_2021/death-stack-x-entry.bin b/bin_delivery/lynx/13_09_2021/death-stack-x-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..f630049dd049bf4f8e7e599d3b266adbeccd48e5 Binary files /dev/null and b/bin_delivery/lynx/13_09_2021/death-stack-x-entry.bin differ diff --git a/bin_delivery/lynx/13_09_2021/death-stack-x-hil-entry.bin b/bin_delivery/lynx/13_09_2021/death-stack-x-hil-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..a434c51ee90bc2bd87add8fa215f1d455193e23e Binary files /dev/null and b/bin_delivery/lynx/13_09_2021/death-stack-x-hil-entry.bin differ diff --git a/bin_delivery/lynx/13_09_2021/death-stack-x-testsuite.bin b/bin_delivery/lynx/13_09_2021/death-stack-x-testsuite.bin new file mode 100755 index 0000000000000000000000000000000000000000..ddf65e21ce6a645fd1acc741b5b2a376baf8302e Binary files /dev/null and b/bin_delivery/lynx/13_09_2021/death-stack-x-testsuite.bin differ diff --git a/bin_delivery/lynx/13_09_2021/ramtest.bin b/bin_delivery/lynx/13_09_2021/ramtest.bin new file mode 100755 index 0000000000000000000000000000000000000000..7a288a98220a20c0e0a0aded228b1bef3d89c285 Binary files /dev/null and b/bin_delivery/lynx/13_09_2021/ramtest.bin differ diff --git a/bin_delivery/lynx/14_09_2021/bmx160-calibration-entry.bin b/bin_delivery/lynx/14_09_2021/bmx160-calibration-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..ca69e32e04d4fc86c8d3f8b319d82cc546ca7701 Binary files /dev/null and b/bin_delivery/lynx/14_09_2021/bmx160-calibration-entry.bin differ diff --git a/bin_delivery/lynx/14_09_2021/death-stack-x-entry.bin b/bin_delivery/lynx/14_09_2021/death-stack-x-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..b62fc3a4012b9f6844dc35bc611d3aadc29f4d76 Binary files /dev/null and b/bin_delivery/lynx/14_09_2021/death-stack-x-entry.bin differ diff --git a/bin_delivery/lynx/14_09_2021/death-stack-x-hil-entry.bin b/bin_delivery/lynx/14_09_2021/death-stack-x-hil-entry.bin new file mode 100755 index 0000000000000000000000000000000000000000..5ca4377d3ca7dc9b27b874047f42fea3d0641a15 Binary files /dev/null and b/bin_delivery/lynx/14_09_2021/death-stack-x-hil-entry.bin differ diff --git a/bin_delivery/lynx/14_09_2021/death-stack-x-testsuite.bin b/bin_delivery/lynx/14_09_2021/death-stack-x-testsuite.bin new file mode 100755 index 0000000000000000000000000000000000000000..09dcf600e834afa6237d5ff669cbf0db6c270ce7 Binary files /dev/null and b/bin_delivery/lynx/14_09_2021/death-stack-x-testsuite.bin differ diff --git a/bin_delivery/lynx/14_09_2021/ramtest.bin b/bin_delivery/lynx/14_09_2021/ramtest.bin new file mode 100755 index 0000000000000000000000000000000000000000..7a288a98220a20c0e0a0aded228b1bef3d89c285 Binary files /dev/null and b/bin_delivery/lynx/14_09_2021/ramtest.bin differ diff --git a/data/airbrakes/coeffs_euroc.mat b/data/airbrakes/coeffs_euroc.mat new file mode 100644 index 0000000000000000000000000000000000000000..12f2d7394288c52b041eeab0b754abfd0e0cc425 Binary files /dev/null and b/data/airbrakes/coeffs_euroc.mat differ diff --git a/data/airbrakes/coeffs_roccaraso.mat b/data/airbrakes/coeffs_roccaraso.mat new file mode 100644 index 0000000000000000000000000000000000000000..d8bacf3589f3ed901674e19398c072ca04ebbf03 Binary files /dev/null and b/data/airbrakes/coeffs_roccaraso.mat differ diff --git a/data/airbrakes/trajectories_euroc.mat b/data/airbrakes/trajectories_euroc.mat new file mode 100644 index 0000000000000000000000000000000000000000..6a2096e02c58c36f189f42c583d85bc289b8e435 Binary files /dev/null and b/data/airbrakes/trajectories_euroc.mat differ diff --git a/data/airbrakes/trajectories_roccaraso.mat b/data/airbrakes/trajectories_roccaraso.mat new file mode 100644 index 0000000000000000000000000000000000000000..103a4eea617760358f279a5eddf2e49a95ffce73 Binary files /dev/null and b/data/airbrakes/trajectories_roccaraso.mat differ diff --git a/ide/vscode/c_cpp_properties.json b/ide/vscode/c_cpp_properties.json index c6f1e64ce1542049dabc4cdc03caa6f704f42024..d86fc75194e21468497092214c3f4877d69d0193 100755 --- a/ide/vscode/c_cpp_properties.json +++ b/ide/vscode/c_cpp_properties.json @@ -1,127 +1,62 @@ { - "configurations": [ - { - "name": "stm32f429zi_skyward_homeone", - "includePath": [ - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix", - "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", - "${workspaceFolder}/skyward-boardcore/libs", - "${workspaceFolder}/skyward-boardcore/src/shared", - "${workspaceFolder}/skyward-boardcore", - "${workspaceFolder}/src/boards", - "${workspaceFolder}/src/tests", - "${workspaceFolder}/src", - "${workspaceFolder}" - ], - "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", - - "defines": [ - "DEBUG", - "_ARCH_CORTEXM4_STM32F4", - "_BOARD_STM32F429ZI_SKYWARD_HOMEONE", - "_MIOSIX_BOARDNAME=stm32f429zi_skyward_homeone", - "HSE_VALUE=8000000", - "SYSCLK_FREQ_168MHz=168000000", - "_MIOSIX", - "__cplusplus=201103L" - ], - - "cStandard": "c11", - "cppStandard": "c++11", - - "browse": { - "path" : [ - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone/interfaces-impl", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/interfaces", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/kernel", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/util", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/e20", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/filesystem", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/stdlib_integration", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/*", - "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", - "${workspaceFolder}/skyward-boardcore/libs/*", - "${workspaceFolder}/skyward-boardcore/src/shared", - "${workspaceFolder}/skyward-boardcore/*", - "${workspaceFolder}/src/boards", - "${workspaceFolder}/src/tests", - "${workspaceFolder}/src/*", - "${workspaceFolder}/*" - ], - "limitSymbolsToIncludedHeaders": true - } - }, - - { - "name": "stm32f429zi_skyward_death_stack", - "includePath": [ - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix", - "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", - "${workspaceFolder}/skyward-boardcore/libs", - "${workspaceFolder}/skyward-boardcore/src/shared", - "${workspaceFolder}/skyward-boardcore", - "${workspaceFolder}/src/boards", - "${workspaceFolder}/src/tests", - "${workspaceFolder}/src", - "${workspaceFolder}" - ], - "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", - - "defines": [ - "DEBUG", - "LOG_THREAD_STACK", - "_ARCH_CORTEXM4_STM32F4", - "_BOARD_STM32F429ZI_SKYWARD_DEATHST", - "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack", - "HSE_VALUE=8000000", - "SYSCLK_FREQ_168MHz=168000000", - "_MIOSIX", - "__cplusplus=201103L", - "DEATH_STACK_1" - - ], - - "cStandard": "c11", - "cppStandard": "c++11", - - "browse": { - "path" : [ - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack/interfaces-impl", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/interfaces", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/kernel", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/util", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/e20", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/filesystem", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/stdlib_integration", - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/*", - "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", - "${workspaceFolder}/skyward-boardcore/libs/*", - "${workspaceFolder}/skyward-boardcore/src/shared", - "${workspaceFolder}/skyward-boardcore/*", - "${workspaceFolder}/src/boards", - "${workspaceFolder}/src/tests", - "${workspaceFolder}/src/*", - "${workspaceFolder}/*" - ], - "limitSymbolsToIncludedHeaders": true - } - } - ], - "version": 4 -} \ No newline at end of file + "configurations": [ + { + "name": "stm32f429zi_skyward_death_stack_x", + "cStandard": "c11", + "cppStandard": "c++11", + "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", + "defines": [ + "DEBUG", + "_ARCH_CORTEXM4_STM32F4", + "_BOARD_stm32f429zi_skyward_death_stack", + "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack", + "HSE_VALUE=8000000", + "SYSCLK_FREQ_168MHz=168000000", + "_MIOSIX", + "__cplusplus=201103L" + ], + "includePath": [ + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix", + "${workspaceFolder}/skyward-boardcore/libs/simple-template-matrix", + "${workspaceFolder}/skyward-boardcore/libs/fmt/include", + "${workspaceFolder}/skyward-boardcore/libs/eigen", + "${workspaceFolder}/skyward-boardcore/libs", + "${workspaceFolder}/skyward-boardcore/src/shared", + "${workspaceFolder}/skyward-boardcore/src/tests", + "${workspaceFolder}/src/boards/DeathStack", + "${workspaceFolder}/src/boards" + ], + "browse": { + "path": [ + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/stdlib_integration", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/interfaces", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/filesystem", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/kernel", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/util", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/e20", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/*", + "${workspaceFolder}/skyward-boardcore/libs/simple-template-matrix", + "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", + "${workspaceFolder}/skyward-boardcore/libs/eigen", + "${workspaceFolder}/skyward-boardcore/libs/tscpp", + "${workspaceFolder}/skyward-boardcore/libs/mxgui", + "${workspaceFolder}/skyward-boardcore/libs/fmt", + "${workspaceFolder}/skyward-boardcore/src/shared", + "${workspaceFolder}/skyward-boardcore/src/tests", + "${workspaceFolder}/src/boards", + "${workspaceFolder}/src/tests" + ], + "limitSymbolsToIncludedHeaders": true + } + } + ], + "version": 4 +} diff --git a/ide/vscode/launch.json b/ide/vscode/launch.json index b70d9703c59e77e15457a584b9e0f539153aacd9..3e9272fde8f6474f08b9803f2c3e3cf547050d0f 100644 --- a/ide/vscode/launch.json +++ b/ide/vscode/launch.json @@ -1,13 +1,6 @@ { "version": "0.2.0", "configurations": [ - { - "name": "Python: Current File", - "type": "python", - "request": "launch", - "program": "${file}", - "console": "integratedTerminal" - }, { "cwd": "${workspaceRoot}", "executable": "${workspaceFolder}/bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.elf", @@ -16,8 +9,10 @@ "type": "cortex-debug", "servertype": "openocd", "device": "STM32F429ZI", + "armToolchainPath": "/opt/arm-miosix-eabi/bin", + "toolchainPrefix": "arm-miosix-eabi", "configFiles": [ - "/home/luca/test.cfg" + "${workspaceFolder}/data/gdb/stm32f4-stlinv2.cfg" ] }, { @@ -28,8 +23,10 @@ "type": "cortex-debug", "servertype": "openocd", "device": "STM32F429ZI", + "armToolchainPath": "/opt/arm-miosix-eabi/bin", + "toolchainPrefix": "arm-miosix-eabi", "configFiles": [ - "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack/death_stack.cfg" + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack/death_stack.cfg" ] } ] diff --git a/ide/vscode/tasks.json b/ide/vscode/tasks.json index e3f1ac8521afa1bea5be835a75cf2c4200c6f294..3afed1afbf7ea2f36e4c773f52487bdfe1f46f34 100755 --- a/ide/vscode/tasks.json +++ b/ide/vscode/tasks.json @@ -1,121 +1,111 @@ { - // See https://go.microsoft.com/fwlink/?LinkId=733558 - // for the documentation about the tasks.json format - "version": "2.0.0", - "tasks": [ - { - "label": "Workspace", - "type": "shell", - - "command": "echo ${workspaceFolder}", - - "problemMatcher": [] - }, - { - "label": "CLEAN", - "type": "shell", - "windows": { - "command": "python skyward-boardcore/sbs -n -c" - }, - "linux": { - "command": "./sbs -c" - }, - "problemMatcher": "$gcc" - }, - { - "label": "BUILD all", - "type": "shell", - "windows": { - "command": "python skyward-boardcore/sbs -n" - }, - "linux": { - "command": "./sbs" - }, - "problemMatcher": "$gcc" - }, - { - "label": "BUILD current-entrypoint", - "type": "shell", - "windows": { - "command": "python skyward-boardcore/sbs -v -n -b ${fileBasenameNoExtension}" - }, - "linux": { - "command": "./sbs -v -b ${fileBasenameNoExtension}" - }, - "problemMatcher": "$gcc" - }, - { - "label": "RUN current-entrypoint", - "type": "shell", - "windows": { - "command": "ST-LINK_CLI.exe -P bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000 -V" - }, - "linux": { - "command": "st-flash write bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000" - }, - "problemMatcher": [] - }, - { - "label": "BUILD+RUN current-entrypoint", - "type": "shell", - "windows": { - "command": "ST-LINK_CLI.exe -P bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000 -V -Rst" - }, - "linux": { - "command": "sleep 1;st-flash write bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000" - }, - "problemMatcher": [], - "dependsOn": [ - "BUILD current-entrypoint" - ], - "group": { - "kind": "build", - "isDefault": true - } - }, - { - "label": "BUILD tests-stm32f429zi-discovery", - "type": "shell", - "windows": { - "command": "python skyward-boardcore/sbs -v -n -b tests-stm32f429zi-discovery" - }, - "linux": { - "command": "./sbs -v -b tests-stm32f429zi-discovery" - }, - "problemMatcher": "$gcc" - }, - { - "label": "RUN tests-stm32f429zi-discovery", - "type": "shell", - "windows": { - "command": "ST-LINK_CLI.exe -P bin/tests-stm32f429zi-discovery/tests-stm32f429zi-discovery.bin 0x8000000 -V" - }, - "linux": { - "command": "st-flash write bin/tests-stm32f429zi-discovery/tests-stm32f429zi-discovery.bin 0x8000000" - }, - "problemMatcher": [] - }, - { - "label": "BUILD tests-catch", - "type": "shell", - "windows": { - "command": "python skyward-boardcore/sbs -v -n -b tests-catch" - }, - "linux": { - "command": "./sbs -v -b tests-catch" - }, - "problemMatcher": "$gcc" - }, - { - "label": "RUN tests-catch", - "type": "shell", - "windows": { - "command": "ST-LINK_CLI.exe -P bin/tests-catch/tests-catch.bin 0x8000000 -V" - }, - "linux": { - "command": "st-flash write bin/tests-catch/tests-catch.bin 0x8000000" - }, - "problemMatcher": [] - } - ] + "version": "2.0.0", + "tasks": [ + { + "label": "Show Workspace Folder", + "type": "shell", + "windows": { + "command": "echo ${workspaceFolder}" + }, + "problemMatcher": [] + }, + { + "label": "CLEAN", + "type": "shell", + "windows": { + "command": "python sbs -c" + }, + "linux": { + "command": "./sbs -c" + }, + "problemMatcher": "$gcc" + }, + { + "label": "BUILD all", + "type": "shell", + "windows": { + "command": "python sbs" + }, + "linux": { + "command": "./sbs" + }, + "problemMatcher": "$gcc" + }, + { + "label": "BUILD current-entrypoint", + "type": "shell", + "windows": { + "command": "python sbs -v -n -b ${fileBasenameNoExtension}" + }, + "linux": { + "command": "./sbs -v -b ${fileBasenameNoExtension}" + }, + "problemMatcher": "$gcc" + }, + { + "label": "RUN current-entrypoint", + "type": "shell", + "windows": { + "command": "ST-LINK_CLI.exe -P bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000 -V -Rst" + }, + "linux": { + "command": "st-flash write bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000" + }, + "problemMatcher": [] + }, + { + "label": "BUILD+RUN current-entrypoint", + "type": "shell", + "windows": { + "command": "ST-LINK_CLI.exe -P bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000 -V -Rst" + }, + "linux": { + "command": "sleep 1;st-flash write bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000" + }, + "problemMatcher": [], + "dependsOn": [ + "BUILD current-entrypoint" + ], + "group": { + "kind": "build", + "isDefault": true + } + }, + { + "label": "BUILD tests-catch", + "type": "shell", + "windows": { + "command": "python sbs -v -n -b tests-catch" + }, + "linux": { + "command": "./sbs -v -b tests-catch" + }, + "problemMatcher": "$gcc" + }, + { + "label": "RUN tests-catch", + "type": "shell", + "windows": { + "command": "ST-LINK_CLI.exe -P bin/tests-catch/tests-catch.bin 0x8000000 -V" + }, + "linux": { + "command": "st-flash write bin/tests-catch/tests-catch.bin 0x8000000" + }, + "problemMatcher": [] + }, + { + "type": "shell", + "label": "arm-miosix-eabi-g++ build active file", + "command": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", + "args": [ + "-g", + "${file}", + "-o", + "${fileDirname}/${fileBasenameNoExtension}" + ], + "options": { + "cwd": "/opt/arm-miosix-eabi/bin" + } + } + ] } \ No newline at end of file diff --git a/sbs.conf b/sbs.conf index 85c4485a6af80f83fdd24596a77312241b566705..81e0ad3a92adce32c3d6ddabf37964ed8dac6f6c 100644 --- a/sbs.conf +++ b/sbs.conf @@ -49,6 +49,7 @@ #stm32f100c8_microboard #stm32f469ni_stm32f469i-disco #stm32f429zi_skyward_death_stack +#stm32f429zi_skyward_death_stack_x #stm32f401re_nucleo #stm32f103c8_skyward_aldeeran @@ -76,10 +77,12 @@ ENTRY_PATH: src/entrypoints TESTS_PATH: src/tests SRC_PATH: src SBS_BASE: skyward-boardcore -PROJECT_INCLUDES: -Isrc/boards +PROJECT_INCLUDES: -Isrc + -Isrc/boards/DeathStack -Iskyward-boardcore/src/shared - -Iskyward-boardcore -Iskyward-boardcore/libs/mavlink_skyward_lib + -Iskyward-boardcore/libs/eigen + -Iskyward-boardcore/libs/fmt PROJECT_SUBDIRS: PROJECT_LIBS: @@ -90,50 +93,71 @@ PROJECT_LIBS: # Type: srcfiles # Files: a '\n'-separated list of f +[deathstack-new] +Type: srcfiles +Files: src/boards/DeathStack/events/EventStrings.cpp + src/boards/DeathStack/PinHandler/PinHandler.cpp + src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp + src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp + src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp + src/boards/DeathStack/Main/Radio.cpp + src/boards/DeathStack/Main/Sensors.cpp + src/boards/DeathStack/Main/StateMachines.cpp + src/boards/DeathStack/Deployment/DeploymentController.cpp + src/boards/DeathStack/FlightModeManager/FMMController.cpp + src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp + src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp + src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp + src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp + src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp + src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp + [deathstack] Type: srcfiles Files: src/boards/DeathStack/LoggerService/LoggerService.cpp src/boards/DeathStack/events/EventStrings.cpp - src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp + src/boards/DeathStack/FlightModeManager/FMMController.cpp src/boards/DeathStack/SensorManager/SensorManager.cpp - src/boards/DeathStack/DeploymentController/DeploymentController.cpp - src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp + src/boards/DeathStack/Deployment/DeploymentController.cpp src/boards/DeathStack/PinHandler/PinHandler.cpp src/boards/DeathStack/TMTCManager/TMTCManager.cpp src/boards/DeathStack/LoggerService/TmRepository.cpp - src/boards/DeathStack/ADA/ADAController.cpp - src/boards/DeathStack/ADA/ADA.cpp - src/boards/DeathStack/ADA/ADACalibrator.cpp - src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp - src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp - src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp + src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp + src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp + src/boards/DeathStack/LoggerService/FSRController.cpp src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp + src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp [ada] Type: srcfiles -Files: src/boards/DeathStack/ADA/ADAController.cpp - src/boards/DeathStack/ADA/ADA.cpp - src/boards/DeathStack/ADA/ADACalibrator.cpp - src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp - src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp +Files: src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp + src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp [deployment] Type: srcfiles -Files: src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp - src/boards/DeathStack/DeploymentController/DeploymentController.cpp +Files: src/boards/DeathStack/Deployment/DeploymentController.cpp + +[airbrakes] +Type: srcfiles +Files: src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp + +[pinhandler] +Type: srcfiles +Files: src/boards/DeathStack/PinHandler/PinHandler.cpp [fmm] Type: srcfiles -Files: src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp +Files: src/boards/DeathStack/FlightModeManager/FMMController.cpp [sensors] Type: srcfiles -Files: src/boards/DeathStack/SensorManager/SensorManager.cpp +Files: src/boards/DeathStack/Sensors/BMX160Calibrator.cpp + skyward-boardcore/src/shared/sensors/calibration/SensorDataExtra.cpp [tmtc] Type: srcfiles -Files: src/boards/DeathStack/TMTCManager/TMTCManager.cpp - src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp +Files: src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp + src/boards/DeathStack/TelemetriesTelecommands/TMTCManager.cpp [aldeeran] Type: srcfiles @@ -141,21 +165,16 @@ Files: src/boards/Ignition/IgnitionManager.cpp [logservice] Type: srcfiles -Files: src/boards/DeathStack/LoggerService/LoggerService.cpp - src/boards/DeathStack/LoggerService/TmRepository.cpp - -[logger-hermes] -Type: srcfiles -Files: src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp +Files: src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp + src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp [ada-test-sources] Type: srcfiles Files: src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp - src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.cpp [kalman-test-sources] Type: srcfiles -Files: src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp +Files: src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp [mock-sensors-data] Type: srcfiles @@ -173,6 +192,23 @@ Files: src/tests/ram_test/sha1.cpp Type: srcfiles Files: src/boards/DeathStack/events/EventStrings.cpp +[navigation-system] +Type: srcfiles +Files: src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp + src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp + +[tests-obsw] +Type: srcfiles +Files: src/tests/catch/fsm/test-fmm.cpp + src/tests/catch/fsm/test-tmtc.cpp + src/tests/catch/fsm/test-ada.cpp + src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp + src/tests/catch/fsm/test-deployment.cpp + src/tests/catch/fsm/test-flightstatsrecorder.cpp + src/tests/catch/fsm/test-airbrakes.cpp + #src/tests/catch/fsm/test-nas.cpp + #src/tests/catch/nas/test-nas-simulation.cpp + #--------------------------# # Boards # #--------------------------# @@ -183,21 +219,55 @@ Files: src/boards/DeathStack/events/EventStrings.cpp # with the corresponding 'srcfiles' # Main: name of the main file (e.g. 'foo' -> src/entrypoints/foo.cpp) -[death-stack-entry] +[death-stack-x-entry] Type: board -BoardId: stm32f429zi_skyward_death_stack -BinName: death-stack-entry -Include: %shared %deathstack %logger %pwm %xbee %piksi %servo -Defines: -DDEATH_STACK_2 -Main: death-stack-entry - -[death-stack-testsuite] +BoardId: stm32f429zi_skyward_death_stack_x +BinName: death-stack-x-entry +Libs: fmt eigen +Include: %shared %deathstack-new %math %ubloxgps %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration +Defines: -DFLIGHT -DROCCARASO +Main: death-stack-x-entry + +[death-stack-x-testsuite] Type: board -BoardId: stm32f429zi_skyward_death_stack -BinName: death-stack-testsuite -Include: %shared %deathstack %logger %pwm %xbee %piksi %servo -Defines: -DDEATH_STACK_1 -Main: death-stack-testsuite +BoardId: stm32f429zi_skyward_death_stack_x +BinName: death-stack-x-testsuite +Libs: fmt eigen +Include: %shared %deathstack-new %internal-adc %pwm %hbridge %test-utils %servo %spi %ubloxgps %ads1118 %ms5803 %bmx160 %math %xbee %bmx160withcorrection %sensormanager %calibration +Defines: -DDEBUG +Main: death-stack-x-testsuite + +# [death-stack-entry] +# Type: board +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: death-stack-entry +# Include: %shared %deathstack %pwm %hbridge %spi %xbee %piksi %servo +# Defines: -DDEATH_STACK_1 +# Main: death-stack-entry + +# [death-stack-testsuite] +# Type: board +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: death-stack-testsuite +# Include: %shared %deathstack %pwm %xbee %piksi %servo +# Defines: -DDEATH_STACK_1 +# Main: death-stack-testsuite + +# [windtunnel-entry] +# Type: board +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: windtunnel-entry +# Include: %shared %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 +# Defines: +# Main: windtunnel-entry + +# [hil-entry] +# Type: board +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: hil-entry +# Include: %shared %sensormanager %fmm %pinhandler %pwm %servo %ada %navigation-system %math %logservice %deployment %hbridge +# Defines: -DHARDWARE_IN_THE_LOOP -DDEBUG +# Main: hardware_in_the_loop/hil-entry #--------------------------# # Tests # @@ -209,211 +279,399 @@ Main: death-stack-testsuite # with the corresponding 'srcfiles' # Main: name of the main file (e.g. 'foo' -> src/tests/foo.cpp) -[catch-tests-entry] +# [catch-tests-entry] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: catch-tests-entry +# Include: %shared %deathstack %pwm %xbee %piksi %servo %hermes-tests %ada-test-sources %mock-sensors-data +# Defines: -DDEATH_STACK_1 +# Main: catch/catch-tests-entry + +[tests-catch] Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: catch-tests-entry -Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %hermes-tests -Defines: -DDEATH_STACK_1 +BoardId: stm32f429zi_skyward_death_stack_x +BinName: tests-catch +Include: %shared %deathstack-new %tests-obsw %ada-test-sources %mock-sensors-data %math %ubloxgps %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration +Defines: +Libs: fmt eigen Main: catch/catch-tests-entry ## Driver tests -[test-imus] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-imus -Include: %shared %sensors %logger %logger-hermes -Defines: -DDEATH_STACK_1 -Main: drivers/test-imus +# [test-imus] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-imus +# Include: %shared %sensors +# Defines: -DDEATH_STACK_1 +# Main: drivers/test-imus -[test-cutter] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-cutter -Include: %pwm -Defines: -DDEBUG -DDEATH_STACK_1 -Main: drivers/test-cutter +# [test-cutter] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-cutter +# Include: %pwm %hbridge %internal-adc %shared +# Defines: -DDEBUG +# Libs: fmt +# Main: drivers/test-cutter -[test-mavlink] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-mavlink -Include: %shared %tmtc %logger %logger-hermes %xbee %logservice -Defines: -DDEBUG -DTRACE_EVENTS -DDEATH_STACK_1 -Main: drivers/test-mavlink +# [test-mavlink] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-mavlink +# Include: %shared %spi %tmtc %xbee %logservice +# Defines: -DDEBUG -DTRACE_EVENTS -DDEATH_STACK_1 +# Main: drivers/test-mavlink -[ledwave] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: ledwave -Include: %shared -Defines: -DDEBUG -Main: ledwave +# [ledwave] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: ledwave +# Include: %shared +# Defines: -DDEBUG +# Libs: fmt +# Main: ledwave [ramtest] Type: test -BoardId: stm32f429zi_skyward_death_stack +BoardId: stm32f429zi_skyward_death_stack_x BinName: ramtest Include: %ram-test -Defines: +Defines: -D__ENABLE_XRAM Main: ram_test/ramtest -[test-motor] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-motor -Include: %shared %deployment %pwm %servo -Defines: -Main: drivers/test-motor - -[test-servo] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-servo -Include: %shared %servo %pwm -Defines: -Main: drivers/test-servo +# [test-motor] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-motor +# Include: %shared %deployment %pwm %servo +# Defines: +# Main: drivers/test-motor -[test-all-sensors] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-all-sensors -Include: %shared %logger %piksi -Defines: -DDEATH_STACK_1 -Main: drivers/test-all-sensors +# [test-servo] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-servo +# Include: %shared %servo %pwm %logservice %airbrakes +# Defines: +# Libs: fmt +# Main: drivers/test-servo + +# [test-hse] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-hse +# Include: +# Defines: +# Main: test-hse -[test-pressure-calib] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-pressure-calib -Include: %shared %logger %piksi -Defines: -DDEATH_STACK_1 -Main: drivers/test-pressure-calib +# [test-all-sensors] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-all-sensors +# Include: %shared %piksi +# Defines: -DDEATH_STACK_1 +# Main: drivers/test-all-sensors -[test-power-board] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-power-board -Include: %shared -Defines: -DDEBUG -Main: drivers/test-power-board +# [test-pressure-calib] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-pressure-calib +# Include: %shared %piksi +# Defines: -DDEATH_STACK_1 +# Main: drivers/test-pressure-calib ## Component tests -# Cant use this as it is since the DeathStack singleton is required for handling messages in the TCHandler class -# [test-tmtc] -# Type: test -# BoardId: stm32f429zi_skyward_death_stack -# BinName: test-tmtc -# Include: %shared %tmtc %logger %logger-hermes %xbee %evt-functions %logproxy -# Defines: -DDEBUG -DTRACE_EVENTS -# Main: test-tmtc - # [test-canproxy] # Type: test -# BoardId: stm32f429zi_skyward_death_stack +# BoardId: stm32f429zi_skyward_death_stack_x # BinName: test-canproxy -# Include: %shared %canproxy %logger %logger-hermes %canbus +# Include: %shared %canproxy %canbus # Defines: -DDEBUG # Main: test-canproxy -[test-logproxy] +[test-pinhandler] Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-logproxy -Include: %shared %logger %logger-hermes %logservice -Defines: -DDEBUG -DDEATH_STACK_1 -Main: test-logproxy +BoardId: stm32f429zi_skyward_death_stack_x +BinName: test-pinhandler +Include: %shared %pinhandler %logservice +Defines: -DDEBUG +Libs: fmt +Main: test-pinhandler -[test-sensormanager] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-sensormanager -Include: %shared %sensors %logger %logger-hermes %ada %piksi %logservice %ada-test-sources -Defines: -DDEBUG -USE_MOCK_SENSORS -DDEATH_STACK_1 -Main: test-sensormanager +# [test-logproxy] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-logproxy +# Include: %shared %logservice +# Defines: -DDEBUG -DDEATH_STACK_1 +# Main: test-logproxy + +# [test-sensormanager] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-sensormanager +# Include: %shared %sensors %piksi %logservice %ada-test-sources +# Defines: -DDEBUG -USE_MOCK_SENSORS -DDEATH_STACK_1 +# Main: test-sensormanager #Cant use this as it is since the DeathStack singleton is required for handling messages in the TCHandler class # [test-sm+tmtc] # Type: test -# BoardId: stm32f429zi_skyward_death_stack +# BoardId: st %adam32f429zi_skyward_death_stack # BinName: test-sm+tmtc -# Include: %shared %sensors %logger %logservice %logger-hermes %ada %piksi %tmtc %xbee %evt-functions %ada-test-sources +# Include: %shared %sensors %logservice %ada %piksi %tmtc %xbee %evt-functions %ada-test-sources # Defines: -DDEBUG -DDEATH_STACK_1 # Main: test-sm+tmtc -[test-dpl] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-dpl -Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %test-utils -Defines: -DDEBUG -DTRACE_EVENTS -DDEATH_STACK_1 -Main: test-dpl +# [test-fmm] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-fmm +# Include: %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration +# Defines: -DSTANDALONE_CATCH1_TEST +# Libs: fmt eigen +# Main: catch/fsm/test-fmm -[test-fmm] -Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-fmm -Include: %deployment %shared %logger %logger-hermes %logservice %evt-functions %fmm %pwm %servo -Defines: -DDEBUG -DDEATH_STACK_1 -Main: test-fmm - -#[test-homeone] -#Type: test -#BoardId: stm32f429zi_skyward_death_stack -#BinName: test-homeone -#Include: %shared -#Defines: -DDEBUG -#Main: test-homeone +# [test-fmm-interactive] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-fmm-interactive +# Include: %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration +# Defines: -DDEBUG +# Libs: fmt eigen +# Main: test-fmm-interactive ## FSM tests - -[test-ada-simulation] +# [test-ada] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-ada +# Include: %shared %ada %test-utils %ada-test-sources %mock-sensors-data %logservice +# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG +# Libs: fmt +# Main: catch/fsm/test-ada + +# [test-ada-simulation] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-ada-simulation +# Include: %shared %ada-test-sources %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration +# Defines: -DDEBUG -DSTANDALONE_CATCH1_TEST +# Libs: fmt eigen +# Main: catch/ada/ada_kalman_p/test-ada-simulation + +[test-ada-dpl-simulation] Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-ada-simulation -Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %ada-test-sources -Defines: -DSTANDALONE_CATCH1_TEST -DSDRAM_ISSI -DDEATH_STACK_2 -Main: catch/ada/ada_kalman_p/test-ada-simulation +BoardId: stm32f429zi_skyward_death_stack_x +BinName: test-ada-dpl-simulation +Include: %shared %ada-test-sources %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration +Defines: -DDEBUG +Libs: fmt eigen +Main: test-ada-dpl-simulation -[test-kalman-acc] +[test-nas] +Type: test +BoardId: stm32f429zi_skyward_death_stack_x +BinName: test-nas +Include: %shared %navigation-system %test-utils %mock-sensors-data %math %logservice +Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG +Libs: fmt +Main: catch/fsm/test-nas + +[test-nas-simulation] Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-kalman-acc -Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %kalman-test-sources -Defines: -DSTANDALONE_CATCH1_TEST -DSDRAM_ISSI -DDEATH_STACK_2 -DDEBUG -Main: catch/ada/kalman_acc/test-kalman-acc +BoardId: stm32f429zi_skyward_death_stack_x +BinName: test-nas-simulation +Include: %shared %navigation-system %mock-sensors-data %math %sensormanager %logservice +Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG +Libs: fmt +Main: catch/nas/test-nas-simulation -# [test-deployment] +# [test-tmtc] # Type: test -# BoardId: stm32f429zi_skyward_death_stack -# BinName: test-deployment -# Include: %deployment %shared %pwm %logger %logger-hermes %test-utils -# Defines: -DSTANDALONE_CATCH1_TEST -# Main: catch/fsm/test-deployment -# +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-tmtc +# Include: %shared %test-utils %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration +# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG -DEIGEN_MAX_ALIGN_BYTES=0 +# Libs: fmt +# Main: catch/fsm/test-tmtc + +# [test-flightstatsrecorder] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-flightstatsrecorder +# Include: %shared %test-utils %logservice +# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG +# Libs: fmt +# Main: catch/fsm/test-flightstatsrecorder + # [test-ignition] -# Type: test -# BoardId: stm32f429zi_skyward_death_stack +# Type: testada_controller +# BoardId: stm32f429zi_skyward_death_stack_x # BinName: test-ignition -# Include: %shared %ignition %logger %logger-hermes %canbus %canproxy %test-utils +# Include: %shared %ignition %canbus %canproxy %test-utils # Defines: -DSTANDALONE_CATCH1_TEST # Main: catch/fsm/test-ignition -# -# [test-fmm] + +# HIL testing algorithms + +# [test-SerialInterface] # Type: test -# BoardId: stm32f429zi_skyward_death_stack -# BinName: test-fmm -# Include: %shared %logger %logger-hermes %test-utils %fmm +# BoardId: stm32f407vg_stm32f4discovery +# BinName: test-SerialInterface +# Include: %shared +# Defines: -DHIL_SERIALINTERFACE -DDEBUG +# Libs: fmt +# Main: hardware_in_the_loop/test-SerialInterface/test-SerialInterface + +# [test-HIL] +# Type: test +# BoardId: stm32f407vg_stm32f4discovery +# BinName: test-HIL +# Include: %shared %sensormanager %servo %pwm +# Defines: -DHIL -DDEBUG +# Main: hardware_in_the_loop/test-HIL/test-HIL + +# [test-HIL+Aerobrake] +# Type: test +# BoardId: stm32f407vg_stm32f4discovery +# BinName: test-HIL+Aerobrake +# Include: %shared %sensormanager +# Defines: -DHIL_AEROBRAKE -DDEBUG +# Main: hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake + +# [test-HIL+AerobrakeController] +# Type: test +# BoardId: stm32f407vg_stm32f4discovery +# BinName: test-HIL+AerobrakeController +# Include: %shared %sensormanager +# Defines: -DHIL_AEROBRAKE -DDEBUG +# Main: hardware_in_the_loop/test-HIL+AerobrakeController/test-HIL+AerobrakeController + +# [test-HIL+ADA] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-HIL+ADA +# Include: %shared %sensormanager %ada %ada-test-sources %logservice +# Defines: -DHIL_ADA -DDEBUG +# Main: hardware_in_the_loop/test-HIL+ApogeeDetectionAlgorithm/test-HIL+ADA + +# [test-HIL+ADA+Aerobrake] +# Type: test +# BoardId: stm32f407vg_stm32f4discovery +# BinName: test-HIL+ADA+Aerobrake +# Include: %shared %sensormanager %ada %ada-test-sources %logservice +# Defines: -DHIL_ADA_AEROBRAKE -DDEBUG +# Main: hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake + +# [test-HIL+ADA+AerobrakeController+nas] +# Type: test +# BoardId: stm32f429zi_stm32f4discovery +# BinName: test-HIL+ADA+AerobrakeController+nas +# Include: %shared %sensormanager %ada %ada-test-sources %logservice %navigation-system %math +# Defines: -DHIL_ADA_AEROBRAKECONTROLLER_NAS -DDEBUG +# Main: hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas + +# [eigen-test] +# Type: test +# BoardId: stm32f429zi_stm32f4discovery +# BinName: eigen-test +# Include: %shared +# Defines: -DDEBUG +# Main: eigen-test + +# [test-mock-sensors] +# Type: test +# BoardId: stm32f429zi_stm32f4discovery +# BinName: test-mock-sensors +# Include: %shared %mock-sensors-data +# Defines: -DDEBUG +# Libs: fmt +# Main: mock_sensors/test-mock-sensors + +# [test-deployment] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-deployment +# Include: %shared %test-utils %pwm %servo %deployment %hbridge %logservice # Defines: -DSTANDALONE_CATCH1_TEST -# Main: catch/fsm/test-fmm -# -[test-ada] +# Libs: fmt +# Main: catch/fsm/test-deployment + +# [test-deployment-interactive] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-deployment-interactive +# Include: %shared %test-utils %pwm %servo %deployment %hbridge %logservice +# Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG +# Libs: fmt +# Main: deployment/test-deployment-interactive + +# [test-airbrakes] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-airbrakes +# Include: %shared %test-utils %pwm %servo %logservice %airbrakes +# Defines: -DSTANDALONE_CATCH1_TEST +# Libs: fmt +# Main: catch/fsm/test-airbrakes + +[test-airbrakes-interactive] Type: test -BoardId: stm32f429zi_skyward_death_stack -BinName: test-ada -Include: %shared %logger %test-utils %deathstack %xbee -Defines: -DSTANDALONE_CATCH1_TEST -DDEATH_STACK_2 -DSDRAM_ISSI -DDEBUG -Main: catch/fsm/test-ada +BoardId: stm32f429zi_skyward_death_stack_x +BinName: test-airbrakes-interactive +Include: %shared %test-utils %pwm %servo %logservice %airbrakes +Defines: -DDEBUG +Libs: fmt +Main: airbrakes/test-airbrakes-interactive + +# [test-airbrakes-algorithm] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-airbrakes-algorithm +# Include: %shared %servo %pwm %logservice %airbrakes +# Defines: -DDEBUG +# Libs: fmt +# Main: airbrakes/test-airbrakes-algorithm + +# Comprehensive DeathStackX tests +# (included in death-stack-x-testsuite) + +# [test-power-board] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-power-board +# Include: %shared %internal-adc %pwm %hbridge %test-utils %servo %logservice %airbrakes +# Defines: -DDEBUG +# Libs: fmt +# Main: deathstack-boards/test-power-board + +# [test-stm-board] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-stm-board +# Include: %shared +# Defines: -DDEBUG +# Libs: fmt +# Main: deathstack-boards/test-stm-board + +# [test-rf-board] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-rf-board +# Include: %shared %spi %ubloxgps %bmx160 +# Defines: -DDEBUG +# Libs: fmt +# Main: deathstack-boards/test-rf-board + +# [test-analog-board] +# Type: test +# BoardId: stm32f429zi_skyward_death_stack_x +# BinName: test-analog-board +# Include: %shared %spi %ads1118 %pinhandler %logservice +# Defines: -DDEBUG +# Libs: fmt +# Main: deathstack-boards/test-analog-board diff --git a/scripts/airbrakes/coeffs.py b/scripts/airbrakes/coeffs.py new file mode 100644 index 0000000000000000000000000000000000000000..e9a2dfb1c98239e37423789a81e9c68867483d7c --- /dev/null +++ b/scripts/airbrakes/coeffs.py @@ -0,0 +1,24 @@ +import scipy.io as sio +import sys + +#filename = "coeffs.mat" +fieldname = "coeffs" + +if len(sys.argv) < 2: + print("\nError, missing path to file") + print("Usage : python3 coeffs.py <path_to_mat_file>\n") + +mat = sio.loadmat(sys.argv[1]) + +data = mat[fieldname][0][0] +names = data.dtype.fields.keys() + +coeffs = {} + +for coeff, name in zip(data, names): + coeffs[name] = coeff.item() + +print("struct Coefficients\n{") +for name in coeffs: # coeff name = dict key + print("\tfloat " + name + " = " + str(coeffs[name]) + ";") +print("};") diff --git a/scripts/airbrakes/trajectories.py b/scripts/airbrakes/trajectories.py new file mode 100644 index 0000000000000000000000000000000000000000..2a67e171e33e08d3af0b142f9cf183d4101b26fa --- /dev/null +++ b/scripts/airbrakes/trajectories.py @@ -0,0 +1,63 @@ +import scipy.io as sio +import sys + +#filename = "Trajectories.mat" +outfilename = "Trajectories.h" +fieldname = "trajectories_saving" + +if len(sys.argv) < 2: + print("\nError, missing path to file") + print("Usage : python3 coeffs.py <path_to_mat_file>\n") + +outfilename = sys.argv[1].replace(".mat", "") + ".h" # use same name as input file + +mat = sio.loadmat(sys.argv[1]) +trajectories = mat[fieldname][0] + +with open(outfilename, "w") as f: + + num_trajectories = len(trajectories) + max_trajectory_len = 0 + trajectory_index = 0 + output_string = "" + + for trajectory in trajectories: + zs, vzs, xs, vxs, ys, vys = trajectory + + for i in range(0, len(zs)): + if zs[i] >= 0: # find first non negative altitude + break + + # exctract only non negative altitude elements + zs = zs[i:] + vzs = vzs[i:] + + if (len(zs) != len(vzs)): + print("ERROR : z and vz don't have the same number of elements in trajectory " + str(trajectory_index)) + + # check length of first column for each trajectory + if len(zs) > max_trajectory_len: + max_trajectory_len = len(zs) + + # output trajectory to file + output_string += "\t{\n" + output_string += "\t\t%d, \n\t\t{\n" % (len(zs)) + for z, vz in zip(zs, vzs): + output_string += "\t\t\t{%f, %f},\n" % (z, vz) + output_string += "\t\t}\n\t},\n" + + trajectory_index += 1 + + output_string += "};" + + # after the preprocessing, output max trajectories length + s = "static const unsigned int TOT_TRAJECTORIES = " + str(num_trajectories) + ";\n" + s += "static const unsigned int TRAJECTORY_MAX_LENGTH = " + str(max_trajectory_len) + ";\n\n" + s += "const trajectory_t TRAJECTORIES_DATA[TOT_TRAJECTORIES] = {\n" + + # final string to be output to file + output_string = s + output_string + + f.write(output_string) + +print("\nFile " + outfilename + " written") diff --git a/scripts/cppcheck_suppressions_list.txt b/scripts/cppcheck_suppressions_list.txt deleted file mode 100644 index 84fbc0df253990712eeec40ac270a53a17228317..0000000000000000000000000000000000000000 --- a/scripts/cppcheck_suppressions_list.txt +++ /dev/null @@ -1,2 +0,0 @@ -*:src/tests/* - diff --git a/scripts/event_header_generator/EventStrings.cpp.template b/scripts/event_header_generator/EventStrings.cpp.template deleted file mode 100644 index d11c60bbcd8c6f9896aa2e36f18cd173e3f4eb31..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/EventStrings.cpp.template +++ /dev/null @@ -1,60 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: {sheet_link} -// Autogen date: {date} - - -#include "Events.h" -#include "Topics.h" - -#include <map> -using std::map; - -namespace DeathStackBoard -{{ - -string getEventString(uint8_t event) -{{ - static const map<uint8_t, string> event_string_map {{ -{event_map_data} - }}; - auto it = event_string_map.find(event); - return it == event_string_map.end() ? "EV_UNKNOWN" : it->second; -}} - -string getTopicString(uint8_t topic) -{{ - static const map<uint8_t, string> topic_string_map{{ -{topic_map_data} - }}; - auto it = topic_string_map.find(topic); - return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second; -}} - -}} \ No newline at end of file diff --git a/scripts/event_header_generator/Events.h.template b/scripts/event_header_generator/Events.h.template deleted file mode 100644 index 3d79b58591da02b142395484b2d2865983d57a7f..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/Events.h.template +++ /dev/null @@ -1,65 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: {sheet_link} -// Autogen date: {date} - -#pragma once - -#include <cstdint> -#include <string> -#include <vector> - -#include "events/Event.h" -#include "events/EventBroker.h" -#include "Topics.h" - -using std::string; - -namespace DeathStackBoard -{{ -/** - * Definition of all events in the Homeone Board software - * Refer to section 5.1.1 of the Software Design Document. - */ -enum Events : uint8_t -{{ -{enum_data} -}}; - -const std::vector<uint8_t> EVENT_LIST {{{event_list}}}; - -/** - * @brief Returns the name of the provided event - * - * @param event - * @return string - */ -string getEventString(uint8_t event); - -}} diff --git a/scripts/event_header_generator/README.md b/scripts/event_header_generator/README.md deleted file mode 100644 index f87e46c67ebcec406b02c58e10dbe91b00a0c8d3..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# Events Generator Script - -This script generates the Events.h and Topics.h heander file from a GoogleSheets document on Google Drive. - -To execute the script: - -``` -pip install --upgrade google-api-python-client -pip install oauth2client -python scripts/homeone/event_header_generator/event_generator.py -``` - -The first time a browser window should open asking for your credentials (use the Skyward ones). -If it doesn't and only gives you an error, try executing it by clicking on the icon. \ No newline at end of file diff --git a/scripts/event_header_generator/Topics.h.template b/scripts/event_header_generator/Topics.h.template deleted file mode 100644 index 87986edbad2dfd6f72a2377389d43da214e85816..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/Topics.h.template +++ /dev/null @@ -1,61 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: {sheet_link} -// Autogen date: {date} - -#pragma once - -#include <stdint.h> -#include <string> -#include <vector> - -using std::string; - -namespace DeathStackBoard -{{ -/** - * Definition of various event topics to use in the EventBroker - */ -enum Topics : uint8_t -{{ -{enum_data} -}}; - -const std::vector<uint8_t> TOPIC_LIST {{{topic_list}}}; - -/** - * @brief Returns the name of the provided event - * - * @param event - * @return string - */ -string getTopicString(uint8_t topic); - -}} // namespace DeathStackBoard - diff --git a/scripts/event_header_generator/credentials.json b/scripts/event_header_generator/credentials.json deleted file mode 100644 index dae318afc5661116b82fe75f80f4316e538e670a..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/credentials.json +++ /dev/null @@ -1 +0,0 @@ -{"installed":{"client_id":"1025168905991-tv31etsgm3lecodc5c798shqciekad40.apps.googleusercontent.com","project_id":"homeone-event-ge-1540834105298","auth_uri":"https://accounts.google.com/o/oauth2/auth","token_uri":"https://www.googleapis.com/oauth2/v3/token","auth_provider_x509_cert_url":"https://www.googleapis.com/oauth2/v1/certs","client_secret":"Yhaf67HuHR4DyXZKNXWu2lre","redirect_uris":["urn:ietf:wg:oauth:2.0:oob","http://localhost"]}} \ No newline at end of file diff --git a/scripts/event_header_generator/event_generator.py b/scripts/event_header_generator/event_generator.py deleted file mode 100644 index 048a2ba7f8fe2725e46533b6877e974bf75e8cfc..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/event_generator.py +++ /dev/null @@ -1,209 +0,0 @@ -#!/usr/bin/python3 - -# Copyright (c) 2018 Skyward Experimental Rocketry -# Authors: Luca Erbetta -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -# - -from googleapiclient.discovery import build -from httplib2 import Http -from oauth2client import file, client, tools - -import re -import datetime -import sys -from os.path import join -import os - -OUTPUT_FOLDER = "generated" -SCOPES = 'https://www.googleapis.com/auth/spreadsheets.readonly' - -service = None - -# Spreadsheet file used to generate the events. The can be found in the link -# to the spreadsheet, for example: -# https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0/ -# --> -# 184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0 -SPREADSHEET_ID = '184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0' -EVENTS_RANGE_NAME = 'EventList!A2:A' -TOPICS_RANGE_NAME = 'Topics!B3:B' - - -def auth(): - try: - store = file.Storage('store.json') - creds = store.get() - if not creds or creds.invalid: - flow = client.flow_from_clientsecrets('credentials.json', SCOPES) - creds = tools.run_flow(flow, store) - - global service - service = build('sheets', 'v4', http=creds.authorize(Http())) - return True - except: - print("Authentication error:", sys.exc_info()[0]) - return False - - -def load_events(): - result = service.spreadsheets().values().get(spreadsheetId=SPREADSHEET_ID, - range=EVENTS_RANGE_NAME).execute() - lines = result.get('values', []) - - # Event names start with EV_ and contains only uppercase letters or underscores - re_event = re.compile(r'(?P<ev>^EV_([A-Z_]+)+)') - - # Only return lines with valid events, remove additional data - events = [] - for l in lines: - m = re_event.match(l[0]) - if m is not None: - events.append(m.group('ev')) - else: - print("Skipped line containing invalid event: {}".format(l)) - - return events - - -def load_topics(): - result = service.spreadsheets().values().get(spreadsheetId=SPREADSHEET_ID, - range=TOPICS_RANGE_NAME).execute() - lines = result.get('values', []) - re_topics = re.compile(r'(?P<topic>^TOPIC_([A-Z_]+)+)') - - # Only return lines with valid topics, remove additional data - topics = [] - for l in lines: - m = re_topics.match(l[0]) - if m is not None: - topics.append(m.group('topic')) - else: - print("Skipped line containing invalid topics: {}".format(l)) - - return topics - - -def has_duplicates(lst): - if len(lst) != len(set(lst)): - return True - return False - - - -print("Skyward on-board software event header generator v0.2") -print("Google sheets API auth in progress...") - -if auth(): - print("Auth successfull.") -else: - exit() - -#directory = os.path.dirname(OUTPUT_FOLDER) -if not os.path.exists(OUTPUT_FOLDER): - os.mkdir(OUTPUT_FOLDER) - -print("Reading from: https://docs.google.com/spreadsheets/d/" + SPREADSHEET_ID) - -events = load_events() -topics = load_topics() - -# Check duplicates -if has_duplicates(events): - print("Duplicate events found! Terminating.") - exit() - -if has_duplicates(topics): - print("Duplicate topics found! Terminating.") - exit() - -events.sort() -topics.sort() - -print("{} events loaded.".format(len(events))) -print("{} topics loaded.".format(len(topics))) - -enum_str = "" -event_map_str = "" -event_list_str = "" - -date = datetime.datetime.now() -link = "https://docs.google.com/spreadsheets/d/{id}".format(id=SPREADSHEET_ID) - - -# Events.h generation - -print("Generating Events.h...") - -for e in events: - endl = ",\n" if e != events[-1] else "" - enum_str += " " + e + \ - (" = EV_FIRST_SIGNAL" if e == events[0] else "") + endl - event_map_str += " {{ {event}, {string} }}{endl}".format( - event=e, string='"' + e + '"', endl=endl) - event_list_str += e + (", " if e != events[-1] else "") - -with open('Events.h.template', 'r') as template_file: - template = template_file.read() - -template = template.format(sheet_link=link, date=date, - enum_data=enum_str,event_list=event_list_str) - -with open(join(OUTPUT_FOLDER, 'Events.h'), 'w') as header_file: - header_file.write(template) - -print("Events.h successfully generated.") -# Topics.h generation - -print("Generating Topics.h...") -enum_str = "" -topic_map_str = "" -topic_list_str = "" - -for t in topics: - endl = ",\n" if t != topics[-1] else "" - enum_str += " " + t + endl - topic_map_str += " {{ {topics}, {string} }}{endl}".format( - topics=t, string='"' + t + '"', endl=endl) - topic_list_str += t + (", " if t != topics[-1] else "") - -with open('Topics.h.template', 'r') as template_file: - template = template_file.read() - -template = template.format(sheet_link=link, date=date, - enum_data=enum_str, topic_list=topic_list_str) - -with open(join(OUTPUT_FOLDER, 'Topics.h'), 'w') as header_file: - header_file.write(template) - -with open('EventStrings.cpp.template', 'r') as cpp_template_file: - cpp = cpp_template_file.read() - -cpp = cpp.format(sheet_link=link, date=date, - event_map_data=event_map_str, topic_map_data=topic_map_str) - -with open(join(OUTPUT_FOLDER, 'EventStrings.cpp'), 'w') as cpp_file: - cpp_file.write(cpp) - -print("Topics.h successfully generated.") - -print() -print("All files successfully generated. Please move Events.h, Topics.h, EventStrings.cpp into your project sources.") -print(".... Done.") diff --git a/scripts/event_header_generator/requirements.txt b/scripts/event_header_generator/requirements.txt deleted file mode 100644 index b4e01569861fa5001ba712bfb61688ed45a3f451..0000000000000000000000000000000000000000 --- a/scripts/event_header_generator/requirements.txt +++ /dev/null @@ -1,20 +0,0 @@ -cachetools==3.1.0 -certifi==2019.3.9 -chardet==3.0.4 -google-api-python-client==1.7.8 -google-auth==1.6.3 -google-auth-httplib2==0.0.3 -google-auth-oauthlib==0.3.0 -httplib2==0.12.3 -idna==2.8 -oauth2client==4.1.3 -oauthlib==3.0.1 -pkg-resources==0.0.0 -pyasn1==0.4.5 -pyasn1-modules==0.2.5 -requests==2.21.0 -requests-oauthlib==1.2.0 -rsa==4.0 -six==1.12.0 -uritemplate==3.0.0 -urllib3==1.24.2 diff --git a/scripts/eventgen.sh b/scripts/eventgen.sh new file mode 100755 index 0000000000000000000000000000000000000000..6763addd5072948bc8e191ff4e61e4c31c20494a --- /dev/null +++ b/scripts/eventgen.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright (c) 2021 Skyward Experimental Rocketry +# Authors: Alberto Nidasio +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +DIRNAME="$(dirname $0)" +$DIRNAME/../skyward-boardcore/scripts/generators/eventgen.py $@ diff --git a/scripts/flash-calibration-entry.sh b/scripts/flash-calibration-entry.sh new file mode 100755 index 0000000000000000000000000000000000000000..f6f85e49db62848c9fd145e6890b3e2ebd2e682c --- /dev/null +++ b/scripts/flash-calibration-entry.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +st-flash write bin_delivery/lynx/final/calibration-entry.bin 0x8000000 diff --git a/scripts/flash-entry.sh b/scripts/flash-entry.sh new file mode 100755 index 0000000000000000000000000000000000000000..9c62109a6d5169bbe50df11ea2fc96827740cb78 --- /dev/null +++ b/scripts/flash-entry.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +st-flash write bin_delivery/lynx/final/death-stack-x-entry.bin 0x8000000 diff --git a/scripts/flash-ramtest.sh b/scripts/flash-ramtest.sh new file mode 100755 index 0000000000000000000000000000000000000000..ed26dd8b4542171e7135a119052f07229b538af8 --- /dev/null +++ b/scripts/flash-ramtest.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +st-flash write bin_delivery/lynx/final/ramtest.bin 0x8000000 diff --git a/scripts/flashentry.sh b/scripts/flashentry.sh deleted file mode 100755 index 7e9ed530b586a41268390ca12ed520520b527ffb..0000000000000000000000000000000000000000 --- a/scripts/flashentry.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/sh - -st-flash write bin_delivery/final/death-stack-entry/death-stack-entry.bin 0x8000000 diff --git a/scripts/flashram.sh b/scripts/flashram.sh deleted file mode 100755 index 8c1e4e111e0943de10e775f2963430063cbdc598..0000000000000000000000000000000000000000 --- a/scripts/flashram.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/sh - -st-flash write bin_delivery/final/ramtest/ramtest.bin 0x8000000 diff --git a/scripts/fsmgen.sh b/scripts/fsmgen.sh new file mode 100755 index 0000000000000000000000000000000000000000..2bae83d7f938ec78624784bfbb93aaa0f17e4431 --- /dev/null +++ b/scripts/fsmgen.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright (c) 2021 Skyward Experimental Rocketry +# Authors: Alberto Nidasio +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +DIRNAME="$(dirname $0)" +$DIRNAME/../skyward-boardcore/scripts/generators/fsmgen.py $@ diff --git a/scripts/linter.sh b/scripts/linter.sh index 79792ccfa247a75ddccfc2bb96a2db7dccbe98ce..f0b2eac4f3a6e8628c1abe05506886fe2b7fc572 100755 --- a/scripts/linter.sh +++ b/scripts/linter.sh @@ -1,38 +1,4 @@ #!/bin/bash -declare -i RESULT=0 - -function check { - B=$(eval "$3") - A=$? - echo -n "[LINTER] $2... " - - if [ "$1" == "OUT" ]; then - if [ ${#B} -gt 0 ]; then - A=$1 - fi; - fi; - if [ "$A" == "$1" ]; then - echo -e "FAIL\n------------ OUTPUT ------------" - echo "$B" - echo "--------------------------------" - return 1 - else - echo "OK" - return 0 - fi; -} - -#check 0 "Checking for files with lines longer than 80 characters" 'egrep --include="*.cpp" --include="*.h" -nr ".{81}" src/' -check 0 "Checking for files with lines longer than 120 characters" 'egrep --include="*.cpp" --include="*.h" -nr ".{121}" src/' -RESULT+=$? -check OUT "Checking for files having \n\n\n" "grep -HPcrz '(\\r?\\n){4,}' src | egrep -v ':0$' | cut -d ':' -f 1" -check OUT "Checking for files not having the copyright" "grep -rL '* Permission is hereby granted, free of charge' src/" -check OUT "Checking for tabulations instead of spaces" "grep -Pr '\t' src" -check OUT "MMP wants his full name" "grep -rl 'Matteo Piazzolla' src/" -check OUT "Launching cppcheck" "cppcheck --suppressions-list=./scripts/cppcheck_suppressions_list.txt --template=gcc -q --suppress=unusedFunction --suppress=missingInclude --std=c++11 --enable=all src/ 2>&1" -RESULT+=$? -check OUT "Checking for using namespace in header files" "grep -rl 'using namespace' src | egrep '.h(pp)?$'" -RESULT+=$? - -exit 0 +DIRNAME="$(dirname $0)" +$DIRNAME/../skyward-boardcore/scripts/linter.sh $DIRNAME/../src/boards \ No newline at end of file diff --git a/scripts/scxml2plant.sh b/scripts/scxml2plant.sh new file mode 100755 index 0000000000000000000000000000000000000000..7a4d5a9cd20a8837ae09f940ff8b382d171ffee0 --- /dev/null +++ b/scripts/scxml2plant.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +DIRNAME="$(dirname $0)" +$DIRNAME/../skyward-boardcore/scripts/scxml2plant/scxml2plant.sh $DIRNAME/../src/boards \ No newline at end of file diff --git a/scripts/vscodesetup.sh b/scripts/vscodesetup.sh new file mode 100755 index 0000000000000000000000000000000000000000..6b515971bef104982d79dc960fd3cd756ae265bb --- /dev/null +++ b/scripts/vscodesetup.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Copyright (c) 2021 Skyward Experimental Rocketry +# Authors: Alberto Nidasio +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +# Copies the directory ide/vscode to .vscode + +cp -r ide/vscode .vscode \ No newline at end of file diff --git a/skyward-boardcore b/skyward-boardcore index 3fe40d6d9f4c035c3524f60e1c896353653c8379..2f03d61094091f38d82c76d559ccf378ba1fc6ec 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit 3fe40d6d9f4c035c3524f60e1c896353653c8379 +Subproject commit 2f03d61094091f38d82c76d559ccf378ba1fc6ec diff --git a/src/boards/CanInterfaces.h b/src/boards/CanInterfaces.h index fe93d6119f07a4b394051b05cff3cb285202165e..b994b38e62afdea325e083b32846baa945eaa4a9 100644 --- a/src/boards/CanInterfaces.h +++ b/src/boards/CanInterfaces.h @@ -1,24 +1,24 @@ /* Copyright (c) 2015-2018 Skyward Experimental Rocketry -* Authors: Alvise de' Faveri Tron -* -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: -* -* The above copyright notice and this permission notice shall be included in -* all copies or substantial portions of the Software. -* -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -* THE SOFTWARE. -*/ + * Authors: Alvise de' Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ #pragma once #include <stdint.h> @@ -27,9 +27,10 @@ namespace CanInterfaces { /** -* CanTopics = Canbus FilterIds = Source of the Canbus message -* Pay attention to the ORDER: lower number => higher priority -*/ + * @brief CanTopics = Canbus FilterIds = Source of the Canbus message + * + * Pay attention to the ORDER: lower number => higher priority + */ enum CanTopic : uint16_t { CAN_TOPIC_HOMEONE = 0, @@ -49,28 +50,28 @@ enum CanCommandID : uint8_t }; /** -* Boards Status structs. -*/ + * @brief Boards Status structs. + */ struct __attribute__((packed)) IgnitionBoardStatus { - uint8_t avr_abortCmd: 1; // abort after explicit command (LSB) - uint8_t avr_abortTimeout: 1; // abort after timeout - uint8_t avr_abortWrongCode: 1; // abort after wrong launch code received - uint8_t avr_launchDone: 1; - uint8_t avr_powerOnReset: 1; // last AVR reset cause was power on - uint8_t avr_externalReset: 1; // last AVR reset cause was reset pin - uint8_t avr_brownoutReset: 1; // last AVR reset cause was brownout - uint8_t avr_watchdogReset: 1; // last AVR reset cause was watchdog (MSB) - - uint8_t stm32_abortCmd: 1; // abort after explicit command (LSB) - uint8_t stm32_abortTimeout: 1; // abort after timeout - uint8_t stm32_abortWrongCode: 1; // abort after wrong launch code received - uint8_t stm32_launchDone: 1; - uint8_t stm32_powerOnReset: 1; // last STM32 reset cause was power on - uint8_t stm32_externalReset: 1; // last STM32 reset cause was reset pin - uint8_t stm32_brownoutReset: 1; // last STM32 reset cause was brownout - uint8_t stm32_watchdogReset: 1; // last STM32 reset cause was watchdog (MSB) + uint8_t avr_abortCmd : 1; // abort after explicit command (LSB) + uint8_t avr_abortTimeout : 1; // abort after timeout + uint8_t avr_abortWrongCode : 1; // abort after wrong launch code received + uint8_t avr_launchDone : 1; + uint8_t avr_powerOnReset : 1; // last AVR reset cause was power on + uint8_t avr_externalReset : 1; // last AVR reset cause was reset pin + uint8_t avr_brownoutReset : 1; // last AVR reset cause was brownout + uint8_t avr_watchdogReset : 1; // last AVR reset cause was watchdog (MSB) + uint8_t stm32_abortCmd : 1; // abort after explicit command (LSB) + uint8_t stm32_abortTimeout : 1; // abort after timeout + uint8_t stm32_abortWrongCode : 1; // abort after wrong launch code received + uint8_t stm32_launchDone : 1; + uint8_t stm32_powerOnReset : 1; // last STM32 reset cause was power on + uint8_t stm32_externalReset : 1; // last STM32 reset cause was reset pin + uint8_t stm32_brownoutReset : 1; // last STM32 reset cause was brownout + uint8_t + stm32_watchdogReset : 1; // last STM32 reset cause was watchdog (MSB) }; } /* namespace CanInterfaces */ diff --git a/src/boards/DeathStack/ADA/ADA.cpp b/src/boards/DeathStack/ADA/ADA.cpp deleted file mode 100644 index f5d34414b3803ae578eaa055abfb5456ebb54a63..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/ADA.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#include "ADA.h" -#include <DeathStack/events/Events.h> -#include <Debug.h> -#include <boards/DeathStack/configs/ADA_config.h> -#include <events/EventBroker.h> -#include <libs/simple-template-matrix/matrix.h> -#include <utils/aero/AeroUtils.h> -#include "DeploymentUtils/elevation_map.h" - -namespace DeathStackBoard -{ -ADA::ADA(ReferenceValues ref_values) - : filter(A_INIT, C_INIT, V1_INIT, V2_INIT, P_INIT), - filter_acc(A_INIT, C_INIT_ACC, V1_INIT_ACC, V2_INIT_ACC, P_INIT_ACC), - ref_values(ref_values) -{ - // Initialize Kalman filter - filter.X(0, 0) = ref_values.ref_pressure; - filter.X(1, 0) = 0; - filter.X(2, 0) = KALMAN_INITIAL_ACCELERATION; - - filter_acc.X(0, 0) = ref_values.ref_altitude; - filter_acc.X(1, 0) = 0; - filter_acc.X(2, 0) = 0; - - TRACE("[ADA] Finalized calibration. p_ref: %.3f, p0: %.3f, t0: %.3f\n", - ref_values.ref_pressure, ref_values.msl_pressure, - ref_values.msl_temperature); -} - -ADA::~ADA() {} - -void ADA::updateBaro(float pressure) -{ - // First kalman (pressure only) - MatrixBase<float, 1, 1> y{pressure}; - filter.update(y); - - // Second kalman (pressure and acceleration) - float z = pressureToAltitude(pressure); - float ax = last_acc_average; - - MatrixBase<float, 2, 1> y_acc{z, ax}; - filter_acc.update(y_acc); - - // Convert filter data to altitudes & speeds - ada_data.timestamp = miosix::getTick(); - ada_data.msl_altitude = pressureToAltitude(filter.X(0, 0)); - - AltitudeDPL ad = altitudeMSLtoDPL(ada_data.msl_altitude); - ada_data.dpl_altitude = ad.altitude; - ada_data.is_dpl_altitude_agl = ad.is_agl; - - ada_data.vert_speed = aeroutils::verticalSpeed( - filter.X(0, 0), filter.X(1, 0), ref_values.msl_pressure, - ref_values.msl_temperature); - - // Filter with accelerometer - ada_data.acc_msl_altitude = filter_acc.X(0, 0); - ada_data.acc_vert_speed = filter_acc.X(1, 0); -} - -void ADA::updateAcc(float ax) -{ - acc_stats.add(ax - 9.81f); - if (acc_stats.n_samples >= ACCELERATION_AVERAGE_N_SAMPLES) - { - last_acc_average = acc_stats.getAverage(); - acc_stats.reset(); - } -} - -void ADA::updateGPS(double lat, double lon, bool has_fix) -{ - last_lat = lat; - last_lon = lon; - last_fix = has_fix; -} - -float ADA::getAltitudeMsl() const { return ada_data.msl_altitude; } - -ADA::AltitudeDPL ADA::getAltitudeForDeployment() const -{ - return AltitudeDPL{ada_data.dpl_altitude, ada_data.is_dpl_altitude_agl}; -} - -float ADA::getVerticalSpeed() const { return ada_data.vert_speed; } - -float ADA::pressureToAltitude(float pressure) const -{ - return aeroutils::relAltitude(pressure, ref_values.msl_pressure, - ref_values.msl_temperature); -} - -ADA::AltitudeDPL ADA::altitudeMSLtoDPL(float altitude_msl) const -{ - float elev = elevationmap::getElevation(last_lat, last_lon); - - if (last_fix && elev >= 0) - { - return {altitude_msl - elev, true}; - } - else - { - return {altitude_msl - ref_values.ref_altitude, false}; - } -} - -KalmanState ADA::getKalmanState() const -{ - KalmanState state; - state.timestamp = miosix::getTick(); - - state.x0 = filter.X(0, 0); - state.x1 = filter.X(1, 0); - state.x2 = filter.X(2, 0); - - state.x0_acc = filter_acc.X(0, 0); - state.x1_acc = filter_acc.X(1, 0); - state.x2_acc = filter_acc.X(2, 0); - - return state; -} -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/ADA.h b/src/boards/DeathStack/ADA/ADA.h deleted file mode 100644 index da7368924231c53f0377e993086d2ac92cd4d4ea..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/ADA.h +++ /dev/null @@ -1,127 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <kalman/Kalman.h> -#include <math/Stats.h> -#include "ADAStatus.h" - -namespace DeathStackBoard -{ -class ADA -{ -public: - struct AltitudeDPL - { - float altitude; - bool is_agl; - }; - - ADA(ReferenceValues setup_data); - ~ADA(); - - void updateBaro(float pressure); - void updateAcc(float ax); - void updateGPS(double lat, double lon, bool has_fix); - - KalmanState getKalmanState() const; - - ADAData getADAData() const { return ada_data; } - - /** - * @brief Current altitude above mean sea level - * - * @return Altitude in meters (MSL) - */ - float getAltitudeMsl() const; - - /** - * @brief Returns altitude for main chute deployment altitude check: - * if last gps sample has fix, returns altitude above ground level - * if not, returns QFE altitude relative to the elevation of the launch - * site - * - * @return Altitude in meters - */ - AltitudeDPL getAltitudeForDeployment() const; - - /** - * @brief Current vertical speed in m/s, positive upwards - */ - float getVerticalSpeed() const; - - /** - * @brief Converts an atmospheric pressure to altitude based on the provided - * reference values. - * - * @param pressure Atmospheric pressure in Pa - * @return Corresponding altitude above mean sea level (m) - */ - float pressureToAltitude(float pressure) const; - - /** - * @brief Converts an altitude above mean sea level to altitude for chute - * deployment (see getAltitudeForDeployment()) - * - */ - AltitudeDPL altitudeMSLtoDPL(float altitude_msl) const; - - ReferenceValues getReferenceValues() const { return ref_values; } - -private: - Kalman<3, 1> filter; // Filter object - Kalman<3, 2> filter_acc; // Filter with accelerometer - - // References for pressure to altitude conversion - ReferenceValues ref_values; - - ADAData ada_data; - - struct AccAverage - { - float accumulator = 0; - unsigned int n_samples = 0; - - void add(float acc) - { - accumulator += acc; - n_samples++; - } - - float getAverage() { return accumulator / n_samples; } - - void reset() - { - n_samples = 0; - accumulator = 0; - } - }; - - float last_acc_average = 0; - AccAverage acc_stats; - - double last_lat = 0; - double last_lon = 0; - bool last_fix = false; -}; -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/ADAController.cpp b/src/boards/DeathStack/ADA/ADAController.cpp deleted file mode 100644 index 6b3dceaaffe948f7b45ba1e2c1ab2d26b832e851..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/ADAController.cpp +++ /dev/null @@ -1,617 +0,0 @@ -/* Copyright (c) 2018,2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli, Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <DeathStack/ADA/ADAController.h> -#include <events/EventBroker.h> -#include <utils/aero/AeroUtils.h> -#include "DeathStack/System/StackLogger.h" -#include "Debug.h" - -using miosix::Lock; - -namespace DeathStackBoard -{ - -/* --- LIFE CYCLE --- */ - -ADAController::ADAController() - : FSM(&ADAController::stateIdle, 4096, 2), ada(ReferenceValues{}) -{ - // Subscribe to topics - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_TC); - sEventBroker->subscribe(this, TOPIC_ADA); - - status.state = ADAState::IDLE; -} - -/* --- SENSOR UPDATE METHODS --- */ - -void ADAController::updateGPS(double lat, double lon, bool hasFix) -{ - // Update gps regardless of the current state - ada.updateGPS(lat, lon, hasFix); -} - -void ADAController::updateBaro(float pressure) -{ - ADAState state = status.state; - - switch (state) - { - case ADAState::IDLE: - { - break; - } - case ADAState::CALIBRATING: - { - bool end_calib = false; - { - Lock<FastMutex> l(calibrator_mutex); - - // Add samples to the calibration - calibrator.addBaroSample(pressure); - - // Save the state of calibration to release mutex - end_calib = calibrator.calibIsComplete(); - } - - if (end_calib) - { - // If samples are enough and dpl altitude has been set init ada - finalizeCalibration(); - } - break; - } - - case ADAState::READY: - { - // Log the altitude & vertical speed but don't use kalman pressure - // while we are on the ramp - ADAData d; - d.timestamp = miosix::getTick(); - - d.msl_altitude = ada.pressureToAltitude(pressure); - d.vert_speed = 0; - - d.acc_msl_altitude = 0; - d.acc_vert_speed = 0; - - ADA::AltitudeDPL ad = ada.altitudeMSLtoDPL(d.msl_altitude); - d.dpl_altitude = ad.altitude; - d.is_dpl_altitude_agl = ad.is_agl; - - logger.log(d); - break; - } - - case ADAState::SHADOW_MODE: - { - // Shadow mode state: update kalman, DO NOT send events - ada.updateBaro(pressure); - - // Check if the vertical speed smaller than the target apogee speed - if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) - { - // Log - ApogeeDetected apogee{status.state, miosix::getTick()}; - logger.log(apogee); - } - - logData(ada.getKalmanState(), ada.getADAData()); - - break; - } - - case ADAState::ACTIVE: - { - ada.updateBaro(pressure); - // Check if we reached apogee - if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) - { - if (++n_samples_apogee_detected >= APOGEE_N_SAMPLES) - { - // Active state send notifications for apogee - sEventBroker->post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA); - status.apogee_reached = true; - } - - // Log - ApogeeDetected apogee{status.state, miosix::getTick()}; - logger.log(apogee); - } - else if (n_samples_apogee_detected != 0) - { - n_samples_apogee_detected = 0; - } - - logData(ada.getKalmanState(), ada.getADAData()); - break; - } - - case ADAState::PRESSURE_STABILIZATION: - { - // Stabilization state: do not send notifications for target - // altitude reached, log it - ada.updateBaro(pressure); - - if (ada.getAltitudeForDeployment().altitude <= - deployment_altitude && - ada.getAltitudeMsl() <= MAX_DEPLOYMENT_ALTITUDE_MSL) - { - if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) - { - logger.log(DplAltitudeReached{miosix::getTick()}); - } - } - else if (n_samples_deployment_detected != 0) - { - n_samples_deployment_detected = 0; - } - - logData(ada.getKalmanState(), ada.getADAData()); - break; - } - - case ADAState::FIRST_DESCENT_PHASE: - { - // Descent state: send notifications for target altitude reached - ada.updateBaro(pressure); - - if (ada.getAltitudeForDeployment().altitude <= - deployment_altitude && - ada.getAltitudeMsl() <= MAX_DEPLOYMENT_ALTITUDE_MSL) - { - if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) - { - logger.log(DplAltitudeReached{miosix::getTick()}); - - sEventBroker->post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA); - } - } - else if (n_samples_deployment_detected != 0) - { - n_samples_deployment_detected = 0; - } - - logData(ada.getKalmanState(), ada.getADAData()); - break; - } - case ADAState::END: - { - // Continue updating the filter for logging & telemetry purposes - ada.updateBaro(pressure); - - logData(ada.getKalmanState(), ada.getADAData()); - break; - } - case ADAState::UNDEFINED: - { - TRACE("[ADA] Update Baro: Undefined state value \n"); - break; - } - - default: - { - TRACE("[ADA] Update Baro: Unexpected state value \n"); - break; - } - } -} - -void ADAController::updateAcc(float ax) -{ - // Update acceleration unconditionally - ada.updateAcc(ax); -} - -/* --- TC --- */ -void ADAController::setReferenceTemperature(float ref_temp) -{ - if (status.state == ADAState::CALIBRATING || - status.state == ADAState::READY) - { - { - Lock<FastMutex> l(calibrator_mutex); - calibrator.setReferenceTemperature(ref_temp); - logger.log(calibrator.getReferenceValues()); - } - - finalizeCalibration(); - } -} - -void ADAController::setReferenceAltitude(float ref_alt) -{ - if (status.state == ADAState::CALIBRATING || - status.state == ADAState::READY) - { - { - Lock<FastMutex> l(calibrator_mutex); - calibrator.setReferenceAltitude(ref_alt); - logger.log(calibrator.getReferenceValues()); - } - - finalizeCalibration(); - } -} - -void ADAController::setDeploymentAltitude(float dpl_alt) -{ - if (status.state == ADAState::CALIBRATING || - status.state == ADAState::READY) - { - { - Lock<FastMutex> l(calibrator_mutex); - - deployment_altitude = dpl_alt; - deployment_altitude_set = true; - } - logger.log(TargetDeploymentAltitude{dpl_alt}); - - TRACE("[ADA] Deployment altitude set to %.3f m\n", dpl_alt); - - finalizeCalibration(); - } -} - -/* --- CALIBRATION --- */ -void ADAController::finalizeCalibration() -{ - Lock<FastMutex> l(calibrator_mutex); - - if (calibrator.calibIsComplete() && deployment_altitude_set && - ada.getReferenceValues() != calibrator.getReferenceValues()) - { - // If samples are enough and dpl altitude has been set init ada - ada = ADA{calibrator.getReferenceValues()}; - - // ADA READY! - sEventBroker->post({EV_ADA_READY}, TOPIC_ADA); - - logger.log(calibrator.getReferenceValues()); - logger.log(ada.getKalmanState()); - } -} - -/* --- STATES --- */ -/** - * \brief Idle state: the ADA waits for a command to start calibration. This is - * the initial state. - */ -void ADAController::stateIdle(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - TRACE("[ADA] Entering stateIdle\n"); - logStatus(ADAState::IDLE); - break; - } - case EV_EXIT: - { - TRACE("[ADA] Exiting stateIdle\n"); - break; - } - case EV_CALIBRATE_ADA: - { - transition(&ADAController::stateCalibrating); - break; - } - default: - { - // TRACE("[ADA] stateIdle: %d event not handled\n", ev.sig); - break; - } - } -} - -/** - * \brief Calibrating state: the ADA calibrates the initial Kalman state. - * - * In this state a call to update() will result in a altitude sample being added - * to the average. - * The exiting transition to the idle state is triggered at the first sample - * update after having set the deployment altitude and having reached the - * minimum number of calibration samples. - */ -void ADAController::stateCalibrating(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - { - Lock<FastMutex> l(calibrator_mutex); - calibrator.resetBaro(); - } - logStatus(ADAState::CALIBRATING); - TRACE("[ADA] Entering stateCalibrating\n"); - break; - } - case EV_EXIT: - { - TRACE("[ADA] Exiting stateCalibrating\n"); - break; - } - case EV_ADA_READY: - { - transition(&ADAController::stateReady); - break; - } - case EV_TC_CALIBRATE_ADA: - { - transition(&ADAController::stateCalibrating); - break; - } - default: - { - // TRACE("ADA stateCalibrating: %d event not handled\n", ev.sig); - break; - } - } -} - -/** - * \brief Ready state: ADA is ready and waiting for liftoff - * - * In this state a call to update() will have no effect. - * The exiting transition to the shadow mode state is triggered by the liftoff - * event. - */ -void ADAController::stateReady(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - logStatus(ADAState::READY); - TRACE("[ADA] Entering stateReady\n"); - break; - } - case EV_EXIT: - { - TRACE("[ADA] Exiting stateReady\n"); - break; - } - case EV_LIFTOFF: - { - transition(&ADAController::stateShadowMode); - break; - } - case EV_TC_CALIBRATE_ADA: - { - transition(&ADAController::stateCalibrating); - break; - } - default: - { - // TRACE("ADA stateIdle: %d event not handled\n", ev.sig); - break; - } - } -} - -/** - * \brief Shadow mode state: ADA is running and logging apogees detected, but - * is not generating events - * - * In this state a call to update() will trigger a one step update of the kalman - * filter followed by a check of vertical speed sign. - * The exiting transition to the active state is triggered by a timeout event. - */ -void ADAController::stateShadowMode(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - shadow_delayed_event_id = - sEventBroker->postDelayed<TIMEOUT_ADA_SHADOW_MODE>( - {EV_TIMEOUT_SHADOW_MODE}, TOPIC_ADA); - logStatus(ADAState::SHADOW_MODE); - TRACE("[ADA] Entering stateShadowMode\n"); - break; - } - case EV_EXIT: - { - sEventBroker->removeDelayed(shadow_delayed_event_id); - TRACE("[ADA] Exiting stateShadowMode\n"); - break; - } - case EV_TIMEOUT_SHADOW_MODE: - { - transition(&ADAController::stateActive); - break; - } - default: - { - // TRACE("ADA stateShadowMode: %d event not handled\n", ev.sig); - break; - } - } -} - -/** - * \brief Active state: ADA is running and it generates an event whe apogee is - * detected - * - * In this state a call to update() will trigger a one step update of the kalman - * filter followed by a check of vertical speed sign. - * The exiting transition to the descent state is triggered by the apogee - * reached event (NOT self generated!) - */ -void ADAController::stateActive(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - logStatus(ADAState::ACTIVE); - TRACE("[ADA] Entering stateActive\n"); - break; - } - case EV_EXIT: - { - TRACE("[ADA] Exiting stateActive\n"); - break; - } - case EV_ADA_APOGEE_DETECTED: - { - transition(&ADAController::statePressureStabilization); - break; - } - default: - { - // TRACE("ADA stateActive: %d event not handled\n", ev.sig); - break; - } - } -} - -void ADAController::statePressureStabilization(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - pressure_delayed_event_id = - sEventBroker->postDelayed<TIMEOUT_ADA_P_STABILIZATION>( - {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA); - logStatus(ADAState::PRESSURE_STABILIZATION); - TRACE("[ADA] Entering statePressureStabilization\n"); - break; - } - case EV_EXIT: - { - sEventBroker->removeDelayed(pressure_delayed_event_id); - TRACE("[ADA] Exiting statePressureStabilization\n"); - break; - } - case EV_TIMEOUT_PRESS_STABILIZATION: - { - transition(&ADAController::stateFirstDescentPhase); - break; - } - default: - { - // TRACE("ADA statePressureStabilization: %d event not handled\n", - // ev.sig); - break; - } - } -} - -/** - * \brief First descent phase state: ADA is running and it generates an event - * when a set altitude is reached - * - * In this state a call to update() will trigger a one step update of the kalman - * filter followed by a check of the altitude. - * The exiting transition to the stop state is triggered by the parachute - * deployment altitude reached event (NOT self generated!) - */ -void ADAController::stateFirstDescentPhase(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - logStatus(ADAState::FIRST_DESCENT_PHASE); - TRACE("[ADA] Entering stateFirstDescentPhase\n"); - n_samples_deployment_detected = 0; - break; - } - case EV_EXIT: - { - TRACE("[ADA] Exiting stateFirstDescentPhase\n"); - break; - } - case EV_ADA_DPL_ALT_DETECTED: - { - status.dpl_altitude_reached = true; - logStatus(); - - transition(&ADAController::stateEnd); - break; - } - default: - { - // TRACE("ADA stateFirstDescentPhase: %d event not handled\n", - // ev.sig); - break; - } - } -} - -/** - * \brief End state: ADA is stopped - * - * In this state a call to update() will have no effect. - * This is the final state - */ -void ADAController::stateEnd(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - { - TRACE("[ADA] Entering stateEnd\n"); - logStatus(ADAState::END); - break; - } - case EV_EXIT: - { - TRACE("[ADA] Exiting stateEnd\n"); - break; - } - default: - { - // TRACE("ADA stateEnd: %d event not handled\n", ev.sig); - break; - } - } -} - -/* --- LOGGER --- */ -void ADAController::logStatus(ADAState state) -{ - status.state = state; - logStatus(); -} - -void ADAController::logStatus() -{ - status.timestamp = miosix::getTick(); - logger.log(status); - - StackLogger::getInstance()->updateStack(THID_ADA_FSM); -} - -void ADAController::logData(const KalmanState& s, const ADAData& d) -{ - logger.log(s); - logger.log(d); -} - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ADA/ADAController.h b/src/boards/DeathStack/ADA/ADAController.h deleted file mode 100644 index 019b83a4dea11ef47af3bd84c111b88839950ea0..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/ADAController.h +++ /dev/null @@ -1,134 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <DeathStack/ADA/ADAStatus.h> -#include <events/FSM.h> -#include "ADA.h" -#include "ADACalibrator.h" - -#include <DeathStack/configs/ADA_config.h> -#include <DeathStack/events/Events.h> -#include <kalman/Kalman.h> -#include "DeathStack/LoggerService/LoggerService.h" - -#include <miosix.h> - -using miosix::FastMutex; -using miosix::Lock; - -namespace DeathStackBoard -{ - -class ADAController : public FSM<ADAController> -{ - -public: - ADAController(); - ~ADAController() {} - - /* --- SENSOR UPDATE METHODS --- */ - /* - * It's critical that this methods are called at regualar intervals during - * the flight. Call frequency is defined in ADA_config.h The behavior of - * this functions changes depending on the ADA state - */ - void updateBaro(float pressure); - void updateGPS(double lat, double lon, bool hasFix); - void updateAcc(float ax); - - /* --- TC --- */ - /** - * Sets the reference temperature to be used to calibrate the altimeter - * @param ref_temp Reference temperature in degrees Celsisus - */ - void setReferenceTemperature(float ref_temp); - - /** - * Sets the reference altitude to be used to calibrate the altimeter - * @param ref_alt Reference altitude in meters above mean sea level - */ - void setReferenceAltitude(float ref_alt); - - /** - * Sets the deployment altitude - * @param dpl_alt Deployment altitude in meters above GROUND level - */ - void setDeploymentAltitude(float dpl_alt); - - /** - * ADA status - * @returns A struct containing the time stamp, the ADA FSM state and - * several flags - */ - ADAControllerStatus getStatus() { return status; } - -private: - /* --- FSM STATES --- */ - void stateIdle(const Event& ev); - void stateCalibrating(const Event& ev); - void stateReady(const Event& ev); - void stateShadowMode(const Event& ev); - void stateActive(const Event& ev); - void statePressureStabilization(const Event& ev); - void stateFirstDescentPhase(const Event& ev); - void stateEnd(const Event& ev); - - void finalizeCalibration(); - - void logStatus(ADAState state); // Update and log ADA FSM state - void logStatus(); // Log the ADA FSM state without updating it - - void logData(const KalmanState& s, const ADAData& d); - - uint16_t shadow_delayed_event_id = - 0; // Event id to store shadow mode timeout - - uint16_t pressure_delayed_event_id = - 0; // Event id to store pressure stabilization timeout - - ADAControllerStatus status; // ADA status: timestamp + state - - /* --- CALIBRATION --- */ - FastMutex calibrator_mutex; - ADACalibrator calibrator; - - /* --- ALGORITHM --- */ - ADA ada; - - float deployment_altitude = DEFAULT_DEPLOYMENT_ALTITUDE; - bool deployment_altitude_set = false; - - // Number of consecutive samples in which apogee is detected - unsigned int n_samples_apogee_detected = 0; - - // Number of consecutive samples in which dpl altitude is detected - unsigned int n_samples_deployment_detected = 0; - - /* --- LOGGER --- */ - LoggerService& logger = *(LoggerService::getInstance()); // Logger - - -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/RogalloDTS.cpp b/src/boards/DeathStack/ADA/DeploymentUtils/RogalloDTS.cpp deleted file mode 100644 index 050f6b2ba2be7d9535cc23cc9ddf384844eec103..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/RogalloDTS.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "RogalloDTS.h" -#include <events/EventBroker.h> -#include <cstring> -#include "DeathStack/events/Events.h" -#include "LHCircles.h" - -namespace DeathStackBoard -{ - -RogalloDTS::RogalloDTS() {} - -RogalloDTS::~RogalloDTS() {} - -void RogalloDTS::setDeploymentAltitudeAgl(float dpl_altitude) -{ - deployment_altitude_agl = dpl_altitude; - deployment_altitude_set = true; -} - -float RogalloDTS::getDeploymentAltitudeAgl() { return deployment_altitude_agl; } - -void RogalloDTS::updateGPS(double lat, double lon, bool has_fix) -{ - last_fix = has_fix; - - last_lat = lat; - last_lon = lon; - - last_terran_elev = elevationmap::getElevation(lat, lon); - - has_gps_sample = true; - - update(); -} - -void RogalloDTS::updateAltitude(float altitude_msl) -{ - last_altitude_msl = altitude_msl; - has_altitude_sample = true; - update(); -} - -void RogalloDTS::update() -{ - // Do things only if we have at least 1 sample from each sensor and the dpl - // altitude has been set. - if (has_gps_sample && has_altitude_sample && deployment_altitude_set && - deployment_altitude_agl > 0) - { - // We deploy only if we have fix and we are inside the launch hazard - // area - bool can_deploy = last_fix && isInsideLHA(last_lat, last_lon); - - if (!deployed && can_deploy) - { - float altitude_agl = last_altitude_msl - last_terran_elev; - - if (altitude_agl <= deployment_altitude_agl) - { - deployed = true; - - sEventBroker->post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA); - } - } - } -} - -bool RogalloDTS::isInsideLHA(double lat, double lon) -{ - using namespace launchhazard; - - // Assume we are always inside if no circles are specified - if (NUM_CIRCLES == 0) - { - return true; - } - - for (int i = 0; i < NUM_CIRCLES; i++) - { - if (circles[i].isInside(lat, lon)) - { - return true; - } - } - - return false; -} - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/RogalloDTS.h b/src/boards/DeathStack/ADA/DeploymentUtils/RogalloDTS.h deleted file mode 100644 index 383f3c7c181b52ce52d9d416d337b131b7c5444e..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/RogalloDTS.h +++ /dev/null @@ -1,96 +0,0 @@ -/** - * Rogallo Deployment and Termination System - * This class is used to determine when it is safe to deploy the Rogallo wing, - * and to terminate the flight in case safety can no longer be assured. - * - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once -#include "elevation_map.h" -#include "DeathStack/configs/ADA_config.h" - -namespace DeathStackBoard -{ - -/** - * Rogallo Deployment and Termination System - * This class receives samples from GPS and Kalman (altitude) in order to deploy - * the rogallo only when it is safe. If the rogallo is deployed and safety - * conditions are not met, this class sends an abort event. - * - * It is safe to deploy the rogallo if the following conditions apply: - * - GPS has fix - * - GPS says we are inside the LHA - * - The altitude is below the target deployment altitude - * - * The rogallo is aborted if: - * - The rogallo was deployed - * - GPS says we are outside the LHA or has no fix for LHA_EGRESS_THRESHOLD - * consecutive samples - */ -class RogalloDTS -{ -public: - RogalloDTS(); - ~RogalloDTS(); - - void updateGPS(double lat, double lon, bool hasFix); - void updateAltitude(float altitudeMSL); - - void setDeploymentAltitudeAgl(float dpl_altitude); - float getDeploymentAltitudeAgl(); - - bool dtsIsReady() {return deployment_altitude_set; } - -private: - void update(); - - /** - * Are the provided coordinates inside the Launch Hazard Area? - * @param lat Latitude [degrees] - * @param lon Longitude [degrees] - * @return Whether the coordinates are inside the LHA - */ - static bool isInsideLHA(double lat, double lon); - - // States - bool deployed = false; - bool terminated = false; - - // Deployment altitude - bool deployment_altitude_set = false; - float deployment_altitude_agl = -1000; - - // Last available data - bool has_gps_sample = false; // Do we have at least one sample? - - double last_lat = 0; - double last_lon = 0; - bool last_fix = false; - int last_terran_elev = elevationmap::INVALID_ELEVATION; - - bool has_altitude_sample = false; // Do we have at least one sample? - float last_altitude_msl = 0; -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp b/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp deleted file mode 100644 index bfee5bb11ed874431198e592b524aef2ae6d0aa8..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "elevation_map.h" - -#include <cmath> -#include <cstddef> - -namespace elevationmap -{ - -int getElevation(double lat, double lon) -{ - if (lat < SOUTH || lat >= NORTH || lon < WEST || lon >= EAST) - { - return INVALID_ELEVATION; - } - - int x = (int)round((lon - WEST) * (RESOLUTION - 1.0) / LON_DELTA); - int y = (int)round((lat - SOUTH) * (RESOLUTION - 1.0) / LAT_DELTA); - - // Check bounds - if (x < 0 || x >= RESOLUTION || y < 0 || y >= RESOLUTION) - { - return INVALID_ELEVATION; - } - - int index = y * RESOLUTION + x; - return elevations[index]; -} -} // namespace elevationmap \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp b/src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp deleted file mode 100644 index b1c640c52467394c8d15a81159d68b5f2cd7a2b7..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.801470 - -#include "elevation_map_data.h" - -namespace elevationmap -{ - -extern const int elevations[] = { 1755, 1757, 1757, 1757, 1758, 1758, 1759, 1761, 1765, 1765, 1764, 1764, 1765, 1763, 1762, 1759, 1747, 1744, 1735, 1731, 1730, 1726, 1721, 1715, 1709, 1702, 1696, 1684, 1675, 1669, 1667, 1667, 1671, 1674, 1679, 1679, 1682, 1682, 1679, 1671, 1667, 1662, 1657, 1647, 1642, 1638, 1639, 1640, 1644, 1652, 1662, 1673, 1682, 1691, 1698, 1703, 1702, 1696, 1690, 1681, 1672, 1669, 1666, 1659, 1652, 1643, 1632, 1622, 1615, 1609, 1602, 1594, 1586, 1580, 1573, 1567, 1566, 1568, 1572, 1570, 1571, 1574, 1576, 1580, 1584, 1587, 1590, 1595, 1599, 1603, 1607, 1610, 1612, 1615, 1617, 1624, 1628, 1631, 1631, 1632, 1635, 1634, 1631, 1623, 1618, 1608, 1593, 1586, 1581, 1582, 1587, 1595, 1595, 1596, 1602, 1614, 1626, 1631, 1639, 1645, 1651, 1653, 1653, 1658, 1666, 1672, 1673, 1675, 1671, 1670, 1666, 1660, 1651, 1643, 1632, 1620, 1606, 1591, 1573, 1565, 1557, 1549, 1545, 1542, 1535, 1534, 1533, 1538, 1543, 1546, 1550, 1551, 1548, 1547, 1546, 1541, 1532, 1521, 1510, 1496, 1482, 1470, 1458, 1446, 1435, 1426, 1418, 1406, 1390, 1373, 1360, 1349, 1337, 1328, 1321, 1311, 1299, 1285, 1270, 1256, 1241, 1761, 1764, 1764, 1764, 1764, 1766, 1765, 1768, 1771, 1772, 1771, 1770, 1770, 1767, 1766, 1761, 1751, 1750, 1741, 1738, 1735, 1733, 1726, 1719, 1713, 1706, 1700, 1688, 1679, 1671, 1669, 1670, 1674, 1679, 1681, 1683, 1689, 1688, 1688, 1682, 1677, 1673, 1666, 1658, 1653, 1649, 1647, 1644, 1642, 1648, 1657, 1666, 1675, 1686, 1694, 1698, 1699, 1695, 1688, 1680, 1673, 1673, 1670, 1663, 1657, 1650, 1641, 1630, 1621, 1612, 1602, 1594, 1585, 1577, 1571, 1567, 1567, 1562, 1559, 1559, 1562, 1567, 1572, 1573, 1573, 1576, 1580, 1584, 1588, 1594, 1599, 1600, 1603, 1605, 1606, 1613, 1617, 1621, 1624, 1627, 1627, 1623, 1617, 1607, 1603, 1597, 1589, 1578, 1570, 1571, 1575, 1578, 1572, 1574, 1583, 1595, 1607, 1613, 1626, 1631, 1632, 1632, 1634, 1639, 1646, 1652, 1657, 1662, 1664, 1663, 1660, 1657, 1652, 1645, 1633, 1624, 1617, 1604, 1590, 1576, 1564, 1558, 1556, 1557, 1553, 1552, 1547, 1547, 1551, 1554, 1556, 1558, 1557, 1557, 1553, 1546, 1536, 1522, 1508, 1494, 1479, 1463, 1454, 1439, 1426, 1419, 1414, 1403, 1389, 1369, 1356, 1346, 1337, 1327, 1320, 1314, 1304, 1296, 1284, 1270, 1254, 1763, 1767, 1770, 1772, 1770, 1772, 1771, 1771, 1772, 1774, 1774, 1773, 1774, 1773, 1772, 1767, 1760, 1758, 1748, 1744, 1740, 1738, 1732, 1725, 1718, 1710, 1703, 1694, 1684, 1679, 1673, 1672, 1678, 1685, 1686, 1689, 1694, 1693, 1691, 1688, 1682, 1678, 1671, 1668, 1662, 1658, 1655, 1650, 1647, 1649, 1658, 1662, 1670, 1677, 1686, 1693, 1695, 1691, 1685, 1678, 1675, 1675, 1673, 1667, 1660, 1653, 1645, 1633, 1620, 1609, 1600, 1593, 1585, 1576, 1569, 1566, 1564, 1556, 1552, 1554, 1557, 1561, 1565, 1562, 1560, 1562, 1568, 1574, 1579, 1585, 1590, 1591, 1594, 1596, 1600, 1608, 1611, 1613, 1616, 1618, 1618, 1607, 1603, 1590, 1586, 1583, 1578, 1568, 1561, 1559, 1562, 1560, 1563, 1565, 1576, 1585, 1590, 1600, 1608, 1618, 1619, 1618, 1621, 1627, 1634, 1638, 1644, 1650, 1655, 1656, 1657, 1655, 1654, 1646, 1635, 1630, 1626, 1617, 1607, 1598, 1581, 1572, 1571, 1572, 1570, 1569, 1564, 1559, 1558, 1560, 1562, 1564, 1565, 1563, 1558, 1552, 1541, 1524, 1505, 1490, 1474, 1456, 1444, 1428, 1414, 1407, 1403, 1395, 1387, 1365, 1350, 1342, 1335, 1326, 1318, 1312, 1304, 1299, 1289, 1278, 1263, 1761, 1764, 1769, 1775, 1775, 1775, 1773, 1772, 1772, 1774, 1773, 1774, 1776, 1779, 1777, 1774, 1769, 1763, 1755, 1748, 1744, 1740, 1736, 1729, 1724, 1716, 1709, 1699, 1689, 1684, 1677, 1675, 1680, 1688, 1691, 1694, 1699, 1699, 1697, 1695, 1690, 1686, 1679, 1677, 1671, 1666, 1662, 1658, 1654, 1651, 1657, 1661, 1668, 1674, 1683, 1690, 1691, 1688, 1683, 1679, 1677, 1676, 1676, 1670, 1662, 1653, 1645, 1633, 1619, 1607, 1599, 1594, 1588, 1577, 1570, 1565, 1558, 1552, 1552, 1554, 1555, 1556, 1556, 1553, 1550, 1551, 1558, 1564, 1570, 1574, 1578, 1580, 1585, 1590, 1598, 1602, 1603, 1604, 1607, 1608, 1605, 1593, 1591, 1580, 1576, 1573, 1566, 1558, 1554, 1551, 1551, 1549, 1557, 1559, 1566, 1575, 1581, 1590, 1592, 1607, 1612, 1612, 1611, 1617, 1624, 1632, 1636, 1642, 1650, 1654, 1660, 1658, 1653, 1646, 1638, 1635, 1634, 1628, 1621, 1612, 1602, 1591, 1588, 1584, 1582, 1583, 1577, 1571, 1567, 1565, 1568, 1571, 1573, 1570, 1563, 1555, 1545, 1525, 1503, 1486, 1471, 1454, 1437, 1422, 1410, 1401, 1396, 1390, 1385, 1359, 1342, 1335, 1331, 1325, 1316, 1308, 1302, 1297, 1289, 1280, 1266, 1760, 1762, 1766, 1773, 1779, 1779, 1775, 1774, 1774, 1775, 1774, 1775, 1778, 1783, 1781, 1778, 1772, 1766, 1760, 1753, 1749, 1743, 1739, 1733, 1728, 1722, 1715, 1702, 1692, 1684, 1679, 1679, 1682, 1688, 1692, 1697, 1704, 1705, 1704, 1703, 1699, 1695, 1688, 1684, 1680, 1674, 1668, 1663, 1657, 1652, 1654, 1658, 1666, 1675, 1683, 1688, 1688, 1686, 1681, 1679, 1676, 1675, 1675, 1670, 1661, 1652, 1642, 1630, 1618, 1606, 1599, 1595, 1588, 1576, 1567, 1560, 1554, 1552, 1552, 1553, 1553, 1552, 1550, 1548, 1546, 1547, 1550, 1555, 1559, 1563, 1567, 1572, 1578, 1585, 1594, 1595, 1595, 1595, 1596, 1595, 1590, 1583, 1581, 1573, 1569, 1564, 1556, 1551, 1547, 1543, 1535, 1537, 1545, 1550, 1554, 1561, 1568, 1578, 1580, 1595, 1606, 1607, 1606, 1610, 1615, 1626, 1630, 1636, 1645, 1652, 1659, 1659, 1652, 1646, 1641, 1638, 1640, 1637, 1633, 1624, 1620, 1611, 1606, 1599, 1595, 1595, 1587, 1582, 1577, 1573, 1575, 1579, 1579, 1575, 1567, 1556, 1544, 1525, 1503, 1485, 1473, 1457, 1439, 1424, 1413, 1403, 1397, 1393, 1386, 1358, 1338, 1331, 1327, 1321, 1313, 1305, 1300, 1295, 1289, 1281, 1268, 1760, 1761, 1765, 1773, 1781, 1783, 1779, 1777, 1778, 1777, 1778, 1775, 1778, 1784, 1782, 1778, 1771, 1766, 1763, 1758, 1753, 1747, 1743, 1738, 1732, 1727, 1719, 1703, 1693, 1684, 1681, 1683, 1685, 1688, 1693, 1700, 1708, 1710, 1710, 1709, 1705, 1703, 1696, 1691, 1688, 1681, 1674, 1665, 1658, 1652, 1650, 1653, 1664, 1675, 1683, 1687, 1686, 1684, 1681, 1679, 1675, 1673, 1672, 1667, 1658, 1651, 1640, 1627, 1616, 1606, 1599, 1593, 1584, 1573, 1562, 1556, 1553, 1552, 1553, 1553, 1552, 1550, 1545, 1543, 1542, 1543, 1544, 1546, 1548, 1552, 1558, 1564, 1572, 1580, 1587, 1589, 1588, 1586, 1585, 1582, 1578, 1574, 1571, 1566, 1563, 1555, 1547, 1541, 1537, 1531, 1521, 1523, 1530, 1537, 1541, 1546, 1554, 1564, 1567, 1582, 1593, 1601, 1600, 1604, 1607, 1616, 1623, 1628, 1636, 1645, 1652, 1657, 1654, 1649, 1645, 1643, 1645, 1644, 1645, 1639, 1634, 1629, 1623, 1615, 1610, 1607, 1599, 1593, 1588, 1585, 1586, 1587, 1582, 1576, 1567, 1556, 1543, 1526, 1506, 1487, 1477, 1463, 1447, 1433, 1421, 1411, 1403, 1394, 1384, 1358, 1338, 1331, 1325, 1317, 1307, 1301, 1296, 1291, 1287, 1280, 1270, 1761, 1762, 1769, 1778, 1785, 1788, 1786, 1783, 1784, 1781, 1782, 1774, 1777, 1783, 1780, 1775, 1769, 1767, 1766, 1761, 1755, 1750, 1746, 1742, 1735, 1730, 1720, 1703, 1694, 1685, 1683, 1685, 1687, 1689, 1696, 1704, 1710, 1714, 1714, 1712, 1709, 1707, 1703, 1698, 1695, 1688, 1679, 1667, 1657, 1650, 1646, 1649, 1662, 1675, 1683, 1685, 1684, 1683, 1681, 1679, 1673, 1670, 1666, 1661, 1654, 1647, 1637, 1624, 1614, 1606, 1597, 1589, 1577, 1568, 1559, 1555, 1553, 1554, 1554, 1553, 1551, 1547, 1542, 1537, 1536, 1536, 1536, 1537, 1539, 1543, 1548, 1555, 1565, 1571, 1577, 1581, 1580, 1576, 1573, 1569, 1565, 1563, 1560, 1555, 1554, 1546, 1536, 1527, 1523, 1518, 1514, 1513, 1521, 1526, 1531, 1535, 1543, 1552, 1553, 1568, 1578, 1592, 1590, 1597, 1602, 1609, 1616, 1622, 1629, 1637, 1644, 1653, 1657, 1656, 1653, 1651, 1652, 1654, 1657, 1653, 1646, 1641, 1637, 1631, 1625, 1618, 1612, 1605, 1600, 1598, 1597, 1595, 1586, 1577, 1566, 1556, 1544, 1530, 1512, 1496, 1483, 1470, 1457, 1445, 1434, 1422, 1411, 1393, 1377, 1357, 1340, 1330, 1322, 1311, 1300, 1294, 1291, 1287, 1284, 1279, 1270, 1762, 1766, 1776, 1786, 1792, 1792, 1792, 1788, 1789, 1787, 1784, 1778, 1777, 1779, 1776, 1771, 1770, 1768, 1766, 1761, 1754, 1749, 1747, 1742, 1738, 1732, 1719, 1703, 1693, 1686, 1684, 1685, 1687, 1691, 1700, 1707, 1712, 1717, 1716, 1713, 1711, 1709, 1708, 1705, 1700, 1692, 1681, 1666, 1653, 1645, 1644, 1648, 1658, 1672, 1681, 1684, 1685, 1685, 1684, 1680, 1672, 1668, 1662, 1655, 1648, 1640, 1630, 1619, 1611, 1602, 1593, 1583, 1575, 1567, 1560, 1557, 1555, 1556, 1555, 1552, 1549, 1546, 1539, 1532, 1531, 1529, 1528, 1529, 1531, 1534, 1538, 1544, 1554, 1558, 1564, 1568, 1568, 1564, 1561, 1557, 1552, 1547, 1543, 1539, 1540, 1535, 1523, 1515, 1510, 1511, 1510, 1511, 1517, 1520, 1524, 1530, 1535, 1540, 1545, 1555, 1567, 1574, 1580, 1592, 1600, 1608, 1614, 1622, 1629, 1635, 1642, 1653, 1661, 1662, 1661, 1663, 1665, 1668, 1670, 1665, 1655, 1649, 1646, 1642, 1635, 1627, 1622, 1617, 1612, 1609, 1607, 1602, 1593, 1581, 1568, 1557, 1546, 1535, 1522, 1509, 1492, 1476, 1465, 1454, 1446, 1433, 1421, 1395, 1372, 1355, 1340, 1327, 1316, 1305, 1298, 1292, 1291, 1289, 1284, 1278, 1270, 1762, 1770, 1782, 1793, 1798, 1798, 1796, 1795, 1791, 1789, 1785, 1781, 1776, 1773, 1771, 1768, 1769, 1768, 1764, 1761, 1753, 1749, 1747, 1740, 1737, 1731, 1720, 1707, 1695, 1688, 1684, 1684, 1686, 1693, 1703, 1708, 1714, 1717, 1715, 1711, 1709, 1709, 1710, 1707, 1701, 1689, 1675, 1660, 1648, 1642, 1642, 1647, 1654, 1665, 1679, 1686, 1688, 1687, 1685, 1682, 1674, 1667, 1659, 1651, 1643, 1634, 1624, 1616, 1608, 1600, 1591, 1583, 1577, 1569, 1563, 1560, 1558, 1558, 1557, 1552, 1547, 1545, 1537, 1531, 1529, 1525, 1523, 1524, 1525, 1527, 1530, 1535, 1541, 1544, 1550, 1555, 1556, 1552, 1550, 1543, 1536, 1531, 1525, 1527, 1527, 1523, 1511, 1507, 1504, 1504, 1502, 1503, 1509, 1514, 1516, 1522, 1525, 1527, 1535, 1545, 1555, 1561, 1576, 1593, 1600, 1606, 1615, 1624, 1631, 1640, 1649, 1657, 1664, 1670, 1673, 1676, 1678, 1680, 1682, 1681, 1668, 1657, 1654, 1650, 1642, 1635, 1630, 1626, 1621, 1617, 1615, 1610, 1599, 1583, 1570, 1562, 1552, 1542, 1533, 1520, 1501, 1483, 1470, 1458, 1448, 1439, 1428, 1402, 1380, 1358, 1339, 1327, 1317, 1308, 1301, 1295, 1293, 1291, 1285, 1276, 1267, 1764, 1773, 1786, 1799, 1803, 1803, 1799, 1797, 1790, 1788, 1786, 1781, 1774, 1768, 1765, 1768, 1767, 1767, 1764, 1759, 1757, 1755, 1746, 1740, 1737, 1725, 1717, 1708, 1698, 1691, 1687, 1688, 1691, 1698, 1706, 1712, 1715, 1716, 1713, 1707, 1705, 1707, 1707, 1702, 1694, 1682, 1668, 1655, 1644, 1641, 1640, 1646, 1654, 1666, 1680, 1687, 1690, 1690, 1687, 1682, 1676, 1668, 1659, 1651, 1642, 1631, 1625, 1619, 1612, 1606, 1597, 1589, 1582, 1575, 1571, 1568, 1564, 1562, 1559, 1552, 1549, 1544, 1537, 1532, 1528, 1524, 1522, 1523, 1521, 1522, 1524, 1528, 1532, 1537, 1542, 1546, 1546, 1542, 1542, 1534, 1528, 1524, 1517, 1514, 1509, 1505, 1500, 1496, 1494, 1491, 1490, 1488, 1496, 1502, 1507, 1511, 1514, 1518, 1526, 1536, 1546, 1556, 1570, 1586, 1596, 1603, 1610, 1623, 1635, 1647, 1656, 1664, 1671, 1677, 1682, 1685, 1686, 1689, 1692, 1691, 1687, 1676, 1664, 1657, 1649, 1641, 1636, 1632, 1628, 1624, 1622, 1617, 1604, 1586, 1577, 1570, 1561, 1548, 1534, 1521, 1509, 1493, 1476, 1463, 1449, 1444, 1433, 1406, 1389, 1369, 1347, 1335, 1327, 1317, 1307, 1302, 1298, 1294, 1287, 1275, 1266, 1764, 1775, 1790, 1801, 1804, 1801, 1797, 1797, 1792, 1789, 1789, 1785, 1777, 1769, 1768, 1770, 1768, 1766, 1763, 1758, 1756, 1755, 1748, 1738, 1734, 1720, 1714, 1706, 1700, 1698, 1695, 1697, 1699, 1702, 1707, 1711, 1713, 1718, 1714, 1708, 1707, 1708, 1705, 1697, 1687, 1675, 1664, 1653, 1644, 1642, 1643, 1651, 1661, 1671, 1681, 1689, 1692, 1692, 1687, 1680, 1673, 1666, 1658, 1649, 1640, 1634, 1627, 1618, 1612, 1605, 1599, 1592, 1585, 1578, 1571, 1566, 1565, 1561, 1557, 1552, 1550, 1546, 1538, 1532, 1528, 1526, 1525, 1522, 1519, 1519, 1520, 1522, 1526, 1530, 1536, 1537, 1535, 1531, 1530, 1527, 1522, 1514, 1505, 1496, 1489, 1487, 1484, 1482, 1481, 1481, 1481, 1480, 1484, 1491, 1497, 1501, 1505, 1512, 1523, 1534, 1542, 1554, 1563, 1574, 1587, 1599, 1606, 1623, 1637, 1648, 1659, 1667, 1676, 1683, 1690, 1693, 1695, 1697, 1701, 1701, 1703, 1695, 1679, 1666, 1655, 1649, 1644, 1642, 1638, 1632, 1628, 1620, 1610, 1597, 1584, 1570, 1558, 1543, 1531, 1520, 1512, 1500, 1484, 1468, 1451, 1442, 1428, 1404, 1386, 1375, 1356, 1341, 1332, 1326, 1315, 1309, 1303, 1296, 1290, 1278, 1272, 1769, 1775, 1789, 1799, 1802, 1798, 1796, 1797, 1793, 1792, 1792, 1786, 1781, 1775, 1772, 1771, 1769, 1767, 1763, 1758, 1755, 1754, 1748, 1737, 1731, 1720, 1717, 1711, 1708, 1708, 1706, 1706, 1706, 1707, 1709, 1710, 1711, 1714, 1714, 1711, 1709, 1709, 1703, 1690, 1677, 1669, 1661, 1654, 1648, 1645, 1648, 1657, 1669, 1676, 1683, 1690, 1694, 1693, 1689, 1681, 1674, 1665, 1657, 1648, 1641, 1635, 1626, 1616, 1609, 1604, 1600, 1595, 1586, 1578, 1570, 1564, 1563, 1559, 1556, 1551, 1546, 1545, 1540, 1535, 1530, 1526, 1525, 1524, 1518, 1515, 1515, 1517, 1519, 1521, 1526, 1527, 1525, 1520, 1518, 1517, 1513, 1505, 1493, 1484, 1477, 1474, 1472, 1472, 1473, 1473, 1472, 1473, 1480, 1484, 1491, 1497, 1502, 1510, 1521, 1533, 1540, 1549, 1556, 1569, 1584, 1599, 1604, 1620, 1635, 1649, 1660, 1670, 1680, 1689, 1698, 1705, 1707, 1706, 1708, 1711, 1712, 1703, 1693, 1680, 1670, 1663, 1656, 1651, 1649, 1642, 1634, 1626, 1615, 1605, 1587, 1569, 1556, 1542, 1531, 1521, 1516, 1502, 1484, 1468, 1451, 1438, 1423, 1402, 1386, 1380, 1366, 1351, 1344, 1336, 1325, 1316, 1308, 1299, 1291, 1285, 1280, 1770, 1776, 1785, 1794, 1798, 1795, 1795, 1799, 1798, 1798, 1797, 1796, 1789, 1783, 1778, 1776, 1774, 1771, 1766, 1760, 1758, 1757, 1751, 1739, 1731, 1726, 1724, 1721, 1718, 1718, 1715, 1713, 1711, 1711, 1711, 1712, 1711, 1713, 1713, 1713, 1711, 1709, 1699, 1683, 1670, 1664, 1660, 1656, 1651, 1649, 1655, 1665, 1675, 1682, 1687, 1693, 1697, 1697, 1693, 1685, 1677, 1666, 1657, 1649, 1644, 1638, 1627, 1616, 1609, 1605, 1603, 1601, 1591, 1578, 1567, 1562, 1561, 1557, 1555, 1551, 1547, 1544, 1540, 1536, 1532, 1528, 1526, 1524, 1516, 1512, 1510, 1511, 1511, 1513, 1515, 1516, 1512, 1509, 1507, 1506, 1504, 1498, 1486, 1479, 1475, 1472, 1471, 1470, 1470, 1467, 1465, 1466, 1474, 1478, 1487, 1494, 1500, 1508, 1521, 1532, 1540, 1547, 1554, 1568, 1586, 1598, 1606, 1621, 1636, 1655, 1667, 1675, 1685, 1695, 1705, 1714, 1718, 1716, 1720, 1724, 1723, 1711, 1701, 1692, 1684, 1673, 1664, 1658, 1654, 1650, 1641, 1631, 1620, 1608, 1590, 1572, 1560, 1547, 1533, 1522, 1518, 1501, 1481, 1464, 1447, 1435, 1421, 1407, 1393, 1384, 1376, 1365, 1358, 1347, 1333, 1324, 1314, 1305, 1298, 1294, 1286, 1770, 1776, 1783, 1791, 1796, 1796, 1798, 1803, 1804, 1806, 1804, 1808, 1800, 1793, 1785, 1783, 1780, 1778, 1772, 1767, 1763, 1762, 1756, 1745, 1736, 1732, 1731, 1730, 1726, 1725, 1721, 1715, 1715, 1714, 1713, 1715, 1713, 1713, 1713, 1712, 1708, 1704, 1694, 1679, 1669, 1664, 1662, 1660, 1655, 1656, 1665, 1675, 1681, 1687, 1692, 1697, 1701, 1701, 1697, 1689, 1680, 1669, 1659, 1651, 1646, 1640, 1629, 1619, 1611, 1606, 1605, 1604, 1595, 1579, 1567, 1561, 1560, 1557, 1555, 1552, 1548, 1544, 1539, 1535, 1530, 1528, 1526, 1523, 1516, 1510, 1509, 1507, 1506, 1508, 1507, 1505, 1501, 1498, 1498, 1499, 1498, 1495, 1486, 1481, 1479, 1475, 1474, 1472, 1470, 1465, 1461, 1461, 1466, 1472, 1482, 1490, 1496, 1504, 1518, 1529, 1537, 1544, 1553, 1565, 1582, 1596, 1607, 1623, 1638, 1659, 1673, 1681, 1693, 1702, 1713, 1723, 1728, 1726, 1731, 1735, 1732, 1721, 1709, 1701, 1693, 1680, 1670, 1663, 1656, 1652, 1645, 1636, 1628, 1613, 1592, 1576, 1564, 1551, 1535, 1524, 1518, 1500, 1482, 1466, 1449, 1436, 1423, 1413, 1400, 1389, 1383, 1376, 1369, 1356, 1341, 1332, 1323, 1315, 1309, 1303, 1292, 1773, 1778, 1784, 1789, 1797, 1802, 1805, 1809, 1808, 1813, 1813, 1815, 1811, 1800, 1794, 1791, 1787, 1785, 1780, 1775, 1770, 1766, 1761, 1754, 1745, 1739, 1739, 1737, 1734, 1732, 1726, 1717, 1717, 1716, 1715, 1717, 1716, 1714, 1713, 1710, 1704, 1698, 1690, 1677, 1671, 1668, 1666, 1665, 1662, 1665, 1674, 1682, 1686, 1690, 1697, 1701, 1704, 1705, 1701, 1692, 1683, 1674, 1664, 1656, 1649, 1641, 1631, 1622, 1613, 1608, 1605, 1603, 1595, 1582, 1571, 1563, 1562, 1559, 1557, 1553, 1548, 1543, 1537, 1532, 1527, 1525, 1523, 1522, 1516, 1513, 1512, 1509, 1506, 1505, 1502, 1499, 1495, 1494, 1494, 1497, 1497, 1494, 1491, 1488, 1486, 1481, 1478, 1474, 1471, 1467, 1460, 1461, 1463, 1469, 1477, 1485, 1491, 1499, 1513, 1522, 1531, 1540, 1552, 1563, 1576, 1594, 1608, 1623, 1637, 1658, 1676, 1688, 1701, 1710, 1722, 1732, 1737, 1736, 1741, 1743, 1738, 1730, 1719, 1708, 1698, 1687, 1678, 1669, 1661, 1654, 1645, 1640, 1636, 1619, 1594, 1578, 1567, 1553, 1539, 1526, 1514, 1498, 1484, 1470, 1456, 1443, 1428, 1418, 1407, 1397, 1389, 1386, 1380, 1368, 1353, 1343, 1335, 1328, 1321, 1312, 1301, 1778, 1783, 1787, 1790, 1799, 1808, 1809, 1811, 1812, 1816, 1819, 1819, 1818, 1805, 1801, 1798, 1795, 1792, 1788, 1781, 1776, 1770, 1764, 1760, 1754, 1750, 1748, 1745, 1742, 1740, 1732, 1724, 1721, 1718, 1718, 1719, 1719, 1717, 1716, 1713, 1706, 1697, 1690, 1679, 1673, 1672, 1671, 1670, 1670, 1673, 1678, 1683, 1687, 1692, 1701, 1706, 1708, 1708, 1702, 1692, 1685, 1678, 1670, 1662, 1653, 1645, 1635, 1623, 1616, 1610, 1604, 1599, 1593, 1584, 1575, 1567, 1564, 1562, 1560, 1555, 1550, 1544, 1536, 1531, 1526, 1523, 1521, 1521, 1519, 1517, 1515, 1513, 1507, 1501, 1499, 1499, 1498, 1498, 1499, 1501, 1502, 1499, 1498, 1495, 1492, 1486, 1482, 1476, 1471, 1465, 1460, 1465, 1464, 1469, 1474, 1483, 1489, 1499, 1510, 1517, 1527, 1538, 1553, 1566, 1576, 1595, 1610, 1621, 1636, 1655, 1676, 1694, 1706, 1717, 1732, 1740, 1744, 1744, 1749, 1748, 1743, 1736, 1725, 1711, 1701, 1692, 1683, 1674, 1667, 1658, 1648, 1642, 1636, 1617, 1592, 1578, 1566, 1556, 1545, 1526, 1510, 1498, 1486, 1472, 1459, 1448, 1435, 1425, 1415, 1405, 1398, 1395, 1393, 1383, 1370, 1358, 1350, 1341, 1332, 1323, 1314, 1786, 1788, 1788, 1791, 1798, 1806, 1808, 1809, 1814, 1817, 1822, 1821, 1819, 1811, 1807, 1805, 1801, 1797, 1793, 1785, 1782, 1775, 1769, 1766, 1765, 1761, 1756, 1753, 1749, 1747, 1741, 1737, 1729, 1728, 1726, 1722, 1723, 1724, 1723, 1718, 1710, 1703, 1695, 1683, 1678, 1677, 1677, 1675, 1675, 1677, 1681, 1685, 1689, 1695, 1702, 1708, 1709, 1708, 1701, 1693, 1686, 1681, 1674, 1666, 1656, 1650, 1639, 1624, 1617, 1611, 1604, 1598, 1593, 1585, 1577, 1572, 1569, 1566, 1564, 1560, 1551, 1545, 1539, 1534, 1529, 1526, 1525, 1524, 1520, 1518, 1514, 1514, 1505, 1499, 1498, 1503, 1505, 1506, 1507, 1508, 1507, 1505, 1503, 1498, 1492, 1486, 1482, 1477, 1468, 1461, 1456, 1462, 1464, 1470, 1477, 1484, 1491, 1501, 1513, 1517, 1526, 1536, 1554, 1568, 1579, 1594, 1612, 1626, 1635, 1654, 1676, 1696, 1709, 1725, 1739, 1744, 1747, 1748, 1753, 1752, 1748, 1737, 1726, 1711, 1699, 1691, 1683, 1675, 1666, 1656, 1648, 1639, 1628, 1607, 1586, 1572, 1562, 1553, 1546, 1529, 1512, 1500, 1488, 1476, 1462, 1453, 1445, 1434, 1424, 1415, 1408, 1408, 1407, 1398, 1388, 1378, 1368, 1355, 1342, 1334, 1327, 1791, 1793, 1787, 1790, 1799, 1804, 1806, 1810, 1815, 1815, 1821, 1824, 1818, 1813, 1810, 1812, 1809, 1801, 1798, 1787, 1782, 1781, 1778, 1776, 1775, 1771, 1763, 1759, 1755, 1750, 1744, 1739, 1736, 1732, 1733, 1728, 1728, 1733, 1727, 1722, 1710, 1703, 1694, 1686, 1686, 1684, 1685, 1683, 1678, 1679, 1683, 1689, 1692, 1698, 1705, 1711, 1711, 1709, 1704, 1695, 1687, 1682, 1677, 1667, 1657, 1652, 1641, 1627, 1619, 1612, 1607, 1602, 1596, 1591, 1584, 1577, 1572, 1568, 1566, 1564, 1554, 1548, 1542, 1537, 1533, 1532, 1529, 1526, 1521, 1516, 1515, 1512, 1504, 1499, 1498, 1502, 1505, 1509, 1510, 1510, 1508, 1504, 1499, 1495, 1489, 1483, 1476, 1471, 1464, 1455, 1455, 1457, 1456, 1468, 1477, 1484, 1490, 1498, 1509, 1521, 1529, 1539, 1553, 1570, 1586, 1599, 1619, 1630, 1637, 1654, 1686, 1694, 1711, 1730, 1736, 1739, 1744, 1751, 1756, 1754, 1745, 1737, 1725, 1710, 1698, 1687, 1678, 1669, 1660, 1652, 1645, 1635, 1621, 1602, 1582, 1569, 1559, 1549, 1544, 1536, 1519, 1503, 1492, 1482, 1469, 1462, 1458, 1447, 1437, 1429, 1421, 1418, 1415, 1410, 1400, 1392, 1384, 1372, 1358, 1347, 1337, 1795, 1796, 1792, 1794, 1800, 1804, 1807, 1810, 1811, 1812, 1815, 1822, 1820, 1819, 1821, 1823, 1817, 1808, 1801, 1794, 1789, 1788, 1787, 1786, 1783, 1777, 1770, 1766, 1763, 1755, 1749, 1741, 1736, 1732, 1733, 1732, 1732, 1737, 1736, 1724, 1715, 1702, 1696, 1691, 1692, 1691, 1691, 1687, 1677, 1676, 1687, 1693, 1695, 1701, 1708, 1713, 1713, 1711, 1707, 1698, 1690, 1685, 1678, 1670, 1659, 1650, 1644, 1632, 1620, 1613, 1610, 1606, 1600, 1598, 1592, 1583, 1578, 1573, 1570, 1567, 1559, 1551, 1545, 1539, 1536, 1533, 1529, 1527, 1523, 1523, 1519, 1513, 1508, 1500, 1497, 1503, 1505, 1509, 1508, 1508, 1505, 1500, 1498, 1489, 1482, 1476, 1469, 1463, 1458, 1449, 1449, 1455, 1454, 1464, 1473, 1478, 1485, 1494, 1503, 1519, 1532, 1544, 1558, 1573, 1595, 1607, 1625, 1632, 1638, 1658, 1680, 1698, 1712, 1725, 1731, 1736, 1746, 1753, 1755, 1750, 1741, 1734, 1725, 1714, 1702, 1687, 1677, 1667, 1656, 1649, 1643, 1634, 1620, 1602, 1586, 1573, 1559, 1550, 1545, 1541, 1536, 1521, 1505, 1491, 1479, 1476, 1477, 1464, 1452, 1445, 1437, 1431, 1425, 1419, 1414, 1403, 1396, 1388, 1374, 1356, 1346, 1801, 1799, 1800, 1801, 1799, 1801, 1805, 1810, 1812, 1814, 1814, 1823, 1824, 1825, 1828, 1826, 1821, 1817, 1813, 1803, 1799, 1796, 1795, 1792, 1788, 1783, 1778, 1772, 1767, 1759, 1752, 1741, 1734, 1731, 1731, 1733, 1734, 1735, 1732, 1723, 1715, 1704, 1696, 1695, 1696, 1697, 1696, 1690, 1679, 1676, 1685, 1690, 1695, 1699, 1705, 1711, 1713, 1711, 1707, 1701, 1696, 1688, 1679, 1672, 1661, 1652, 1646, 1634, 1620, 1615, 1612, 1609, 1604, 1599, 1593, 1588, 1585, 1581, 1577, 1573, 1564, 1558, 1550, 1541, 1535, 1533, 1531, 1529, 1526, 1526, 1520, 1513, 1506, 1501, 1498, 1502, 1505, 1509, 1510, 1510, 1507, 1498, 1493, 1483, 1476, 1470, 1464, 1457, 1455, 1454, 1450, 1453, 1457, 1461, 1469, 1472, 1481, 1497, 1505, 1519, 1533, 1547, 1557, 1579, 1598, 1610, 1625, 1634, 1644, 1661, 1678, 1701, 1716, 1726, 1732, 1740, 1750, 1752, 1751, 1748, 1741, 1736, 1730, 1719, 1704, 1690, 1679, 1669, 1659, 1650, 1639, 1631, 1617, 1603, 1589, 1577, 1568, 1556, 1546, 1544, 1544, 1534, 1520, 1505, 1493, 1489, 1488, 1479, 1468, 1462, 1457, 1451, 1440, 1429, 1417, 1408, 1403, 1394, 1377, 1363, 1357, 1810, 1805, 1806, 1807, 1804, 1803, 1804, 1809, 1812, 1816, 1816, 1823, 1824, 1826, 1828, 1827, 1823, 1822, 1821, 1812, 1804, 1801, 1798, 1794, 1791, 1788, 1783, 1775, 1769, 1760, 1749, 1738, 1734, 1732, 1731, 1733, 1733, 1729, 1726, 1721, 1715, 1706, 1701, 1700, 1700, 1701, 1700, 1694, 1687, 1683, 1687, 1690, 1696, 1699, 1705, 1711, 1714, 1710, 1706, 1701, 1697, 1689, 1681, 1675, 1666, 1657, 1650, 1637, 1624, 1621, 1617, 1613, 1608, 1602, 1594, 1590, 1588, 1585, 1582, 1577, 1569, 1563, 1556, 1546, 1539, 1536, 1533, 1531, 1529, 1525, 1519, 1512, 1506, 1502, 1500, 1502, 1506, 1512, 1511, 1512, 1505, 1493, 1485, 1478, 1474, 1462, 1461, 1459, 1453, 1453, 1452, 1452, 1458, 1459, 1467, 1468, 1478, 1497, 1509, 1520, 1536, 1553, 1563, 1582, 1601, 1609, 1624, 1641, 1653, 1669, 1686, 1706, 1723, 1727, 1734, 1742, 1750, 1752, 1747, 1745, 1741, 1736, 1731, 1720, 1705, 1693, 1685, 1675, 1661, 1649, 1638, 1631, 1621, 1609, 1598, 1587, 1581, 1571, 1558, 1555, 1554, 1548, 1537, 1526, 1521, 1511, 1503, 1495, 1484, 1475, 1469, 1463, 1452, 1438, 1426, 1414, 1408, 1396, 1380, 1368, 1364, 1817, 1811, 1812, 1812, 1808, 1807, 1805, 1807, 1810, 1816, 1819, 1824, 1826, 1827, 1829, 1829, 1824, 1822, 1820, 1817, 1808, 1803, 1800, 1797, 1794, 1793, 1786, 1777, 1768, 1757, 1744, 1734, 1733, 1731, 1729, 1731, 1731, 1726, 1721, 1718, 1713, 1708, 1707, 1706, 1706, 1706, 1703, 1698, 1692, 1687, 1690, 1690, 1695, 1700, 1706, 1713, 1714, 1711, 1706, 1702, 1696, 1690, 1683, 1676, 1667, 1657, 1649, 1639, 1631, 1628, 1624, 1619, 1612, 1604, 1595, 1590, 1588, 1587, 1585, 1582, 1575, 1568, 1561, 1552, 1547, 1542, 1536, 1533, 1531, 1525, 1519, 1515, 1509, 1504, 1502, 1503, 1505, 1509, 1508, 1507, 1498, 1485, 1478, 1473, 1467, 1458, 1457, 1456, 1449, 1449, 1450, 1451, 1455, 1457, 1462, 1470, 1476, 1493, 1507, 1523, 1537, 1553, 1569, 1586, 1599, 1611, 1627, 1646, 1655, 1674, 1692, 1710, 1724, 1729, 1734, 1741, 1748, 1750, 1744, 1743, 1740, 1735, 1727, 1714, 1704, 1693, 1687, 1678, 1666, 1654, 1643, 1637, 1630, 1622, 1614, 1604, 1599, 1590, 1579, 1573, 1567, 1564, 1555, 1546, 1541, 1532, 1519, 1509, 1499, 1489, 1479, 1469, 1459, 1449, 1434, 1419, 1412, 1400, 1387, 1376, 1371, 1824, 1818, 1819, 1817, 1813, 1809, 1807, 1809, 1810, 1817, 1823, 1826, 1828, 1828, 1831, 1831, 1827, 1825, 1823, 1821, 1813, 1809, 1806, 1803, 1800, 1797, 1787, 1777, 1767, 1756, 1743, 1733, 1731, 1729, 1728, 1731, 1732, 1729, 1722, 1719, 1713, 1712, 1712, 1713, 1713, 1712, 1708, 1703, 1697, 1691, 1690, 1690, 1694, 1700, 1707, 1714, 1717, 1714, 1710, 1704, 1698, 1693, 1685, 1675, 1664, 1655, 1647, 1641, 1638, 1635, 1631, 1624, 1615, 1606, 1596, 1592, 1590, 1589, 1588, 1586, 1580, 1572, 1565, 1556, 1552, 1546, 1542, 1539, 1532, 1528, 1522, 1520, 1514, 1506, 1503, 1504, 1504, 1504, 1503, 1500, 1491, 1477, 1473, 1468, 1459, 1455, 1452, 1450, 1446, 1448, 1448, 1451, 1452, 1456, 1461, 1471, 1478, 1489, 1505, 1522, 1536, 1550, 1569, 1590, 1600, 1616, 1627, 1644, 1654, 1672, 1691, 1709, 1719, 1725, 1730, 1740, 1746, 1748, 1743, 1742, 1740, 1733, 1721, 1707, 1701, 1694, 1688, 1680, 1672, 1664, 1655, 1648, 1643, 1637, 1630, 1622, 1616, 1608, 1600, 1592, 1584, 1579, 1572, 1563, 1553, 1546, 1533, 1518, 1510, 1501, 1486, 1476, 1464, 1456, 1439, 1421, 1413, 1405, 1394, 1383, 1376, 1832, 1827, 1826, 1823, 1818, 1811, 1810, 1814, 1814, 1821, 1829, 1832, 1830, 1830, 1834, 1834, 1832, 1831, 1829, 1826, 1822, 1820, 1817, 1814, 1808, 1798, 1785, 1775, 1765, 1756, 1746, 1736, 1731, 1730, 1732, 1735, 1736, 1735, 1730, 1725, 1719, 1719, 1717, 1718, 1720, 1717, 1713, 1706, 1700, 1695, 1689, 1690, 1694, 1701, 1707, 1714, 1720, 1718, 1716, 1709, 1701, 1695, 1685, 1674, 1663, 1656, 1652, 1646, 1644, 1641, 1635, 1626, 1617, 1611, 1601, 1597, 1596, 1593, 1590, 1587, 1582, 1575, 1567, 1558, 1553, 1548, 1546, 1544, 1536, 1533, 1529, 1526, 1519, 1510, 1504, 1504, 1502, 1500, 1498, 1495, 1485, 1474, 1468, 1464, 1457, 1452, 1449, 1446, 1446, 1446, 1447, 1450, 1452, 1457, 1464, 1471, 1482, 1490, 1505, 1520, 1536, 1548, 1566, 1590, 1605, 1619, 1622, 1636, 1651, 1668, 1683, 1699, 1710, 1715, 1723, 1737, 1744, 1746, 1743, 1742, 1739, 1728, 1715, 1706, 1700, 1698, 1694, 1686, 1679, 1675, 1672, 1665, 1661, 1653, 1646, 1640, 1632, 1624, 1615, 1607, 1599, 1592, 1585, 1577, 1564, 1555, 1542, 1527, 1518, 1509, 1494, 1485, 1473, 1460, 1444, 1425, 1412, 1407, 1399, 1388, 1380, 1842, 1837, 1834, 1829, 1822, 1817, 1815, 1814, 1818, 1826, 1833, 1836, 1834, 1833, 1837, 1838, 1834, 1833, 1830, 1827, 1831, 1834, 1830, 1828, 1818, 1799, 1784, 1771, 1761, 1755, 1747, 1741, 1736, 1735, 1739, 1742, 1742, 1741, 1739, 1735, 1731, 1729, 1724, 1722, 1724, 1720, 1716, 1708, 1700, 1697, 1689, 1687, 1691, 1699, 1706, 1713, 1719, 1720, 1719, 1712, 1704, 1692, 1680, 1674, 1666, 1662, 1658, 1653, 1649, 1646, 1639, 1629, 1622, 1616, 1608, 1603, 1599, 1594, 1592, 1588, 1582, 1576, 1568, 1561, 1552, 1549, 1546, 1544, 1540, 1536, 1533, 1528, 1519, 1513, 1505, 1504, 1501, 1496, 1494, 1490, 1482, 1473, 1464, 1461, 1458, 1451, 1448, 1442, 1442, 1440, 1444, 1446, 1453, 1461, 1466, 1474, 1484, 1492, 1505, 1519, 1535, 1548, 1562, 1585, 1603, 1614, 1618, 1627, 1643, 1660, 1674, 1684, 1697, 1706, 1718, 1730, 1741, 1744, 1741, 1738, 1734, 1724, 1713, 1709, 1703, 1702, 1702, 1695, 1688, 1686, 1688, 1684, 1679, 1671, 1665, 1658, 1650, 1640, 1630, 1618, 1611, 1605, 1600, 1591, 1576, 1562, 1549, 1535, 1525, 1516, 1500, 1492, 1483, 1467, 1448, 1438, 1417, 1411, 1403, 1392, 1385, 1850, 1844, 1839, 1834, 1828, 1824, 1818, 1815, 1820, 1828, 1830, 1834, 1835, 1837, 1839, 1840, 1834, 1832, 1830, 1830, 1834, 1841, 1836, 1834, 1825, 1804, 1789, 1770, 1759, 1753, 1749, 1747, 1744, 1743, 1746, 1750, 1751, 1748, 1746, 1742, 1738, 1737, 1732, 1728, 1725, 1721, 1717, 1710, 1701, 1695, 1691, 1686, 1688, 1695, 1705, 1711, 1718, 1720, 1719, 1714, 1707, 1696, 1683, 1675, 1668, 1665, 1662, 1661, 1659, 1655, 1646, 1637, 1629, 1622, 1614, 1606, 1599, 1596, 1594, 1590, 1584, 1578, 1568, 1560, 1553, 1549, 1548, 1546, 1543, 1539, 1535, 1528, 1516, 1509, 1503, 1500, 1498, 1494, 1493, 1489, 1484, 1472, 1470, 1461, 1457, 1452, 1439, 1436, 1437, 1438, 1438, 1443, 1450, 1457, 1463, 1475, 1484, 1497, 1506, 1519, 1534, 1549, 1563, 1581, 1599, 1607, 1612, 1621, 1636, 1650, 1655, 1665, 1682, 1696, 1712, 1720, 1734, 1744, 1737, 1734, 1732, 1726, 1718, 1715, 1713, 1709, 1707, 1705, 1702, 1700, 1699, 1695, 1688, 1680, 1675, 1671, 1665, 1659, 1649, 1634, 1625, 1620, 1613, 1602, 1589, 1571, 1556, 1546, 1536, 1522, 1507, 1495, 1486, 1479, 1454, 1448, 1428, 1418, 1407, 1399, 1396, 1851, 1849, 1846, 1840, 1838, 1833, 1823, 1822, 1825, 1827, 1829, 1833, 1842, 1850, 1845, 1845, 1839, 1839, 1841, 1837, 1836, 1838, 1838, 1833, 1828, 1817, 1798, 1780, 1763, 1756, 1753, 1749, 1749, 1753, 1755, 1760, 1762, 1759, 1751, 1748, 1745, 1744, 1741, 1735, 1732, 1724, 1717, 1709, 1702, 1690, 1690, 1688, 1687, 1691, 1700, 1709, 1714, 1716, 1716, 1712, 1705, 1698, 1689, 1680, 1673, 1669, 1667, 1667, 1667, 1661, 1651, 1642, 1634, 1628, 1623, 1611, 1602, 1601, 1598, 1592, 1584, 1577, 1570, 1563, 1559, 1555, 1551, 1548, 1543, 1539, 1533, 1524, 1511, 1504, 1498, 1494, 1493, 1489, 1488, 1485, 1479, 1473, 1471, 1461, 1452, 1443, 1433, 1431, 1435, 1434, 1437, 1441, 1445, 1451, 1457, 1470, 1484, 1493, 1509, 1522, 1535, 1539, 1561, 1576, 1585, 1602, 1609, 1621, 1636, 1645, 1647, 1655, 1668, 1686, 1707, 1724, 1731, 1736, 1736, 1736, 1734, 1728, 1721, 1717, 1714, 1714, 1714, 1715, 1715, 1712, 1708, 1703, 1694, 1686, 1679, 1676, 1675, 1669, 1663, 1653, 1640, 1632, 1624, 1612, 1600, 1584, 1570, 1557, 1547, 1533, 1517, 1506, 1498, 1484, 1471, 1455, 1435, 1424, 1417, 1411, 1407, 1855, 1852, 1849, 1845, 1845, 1842, 1830, 1827, 1829, 1833, 1832, 1839, 1844, 1849, 1848, 1848, 1846, 1849, 1852, 1845, 1839, 1838, 1838, 1832, 1827, 1820, 1805, 1789, 1774, 1767, 1763, 1757, 1754, 1757, 1765, 1773, 1775, 1771, 1760, 1756, 1754, 1751, 1748, 1744, 1739, 1732, 1726, 1714, 1703, 1696, 1693, 1691, 1689, 1691, 1697, 1704, 1707, 1711, 1713, 1713, 1708, 1700, 1691, 1684, 1678, 1674, 1671, 1672, 1670, 1664, 1655, 1643, 1634, 1627, 1623, 1615, 1607, 1606, 1603, 1595, 1587, 1580, 1575, 1569, 1565, 1562, 1559, 1551, 1543, 1536, 1526, 1518, 1511, 1504, 1494, 1487, 1488, 1485, 1484, 1481, 1477, 1472, 1464, 1460, 1449, 1439, 1430, 1429, 1432, 1433, 1437, 1440, 1442, 1450, 1455, 1469, 1483, 1496, 1512, 1523, 1525, 1539, 1552, 1567, 1573, 1598, 1607, 1624, 1630, 1644, 1647, 1651, 1660, 1683, 1705, 1719, 1724, 1732, 1738, 1738, 1737, 1732, 1726, 1721, 1720, 1720, 1722, 1724, 1724, 1719, 1717, 1713, 1704, 1695, 1687, 1682, 1678, 1676, 1672, 1663, 1651, 1642, 1632, 1619, 1602, 1586, 1577, 1566, 1552, 1541, 1528, 1516, 1502, 1489, 1478, 1458, 1450, 1436, 1426, 1419, 1415, 1859, 1854, 1852, 1850, 1850, 1848, 1840, 1836, 1836, 1837, 1837, 1842, 1847, 1852, 1853, 1853, 1854, 1858, 1858, 1852, 1847, 1842, 1834, 1828, 1824, 1817, 1810, 1797, 1782, 1773, 1771, 1766, 1765, 1769, 1777, 1783, 1784, 1782, 1774, 1767, 1765, 1759, 1757, 1754, 1750, 1742, 1734, 1719, 1707, 1702, 1700, 1697, 1691, 1691, 1695, 1703, 1706, 1709, 1712, 1714, 1713, 1708, 1698, 1687, 1682, 1677, 1676, 1678, 1673, 1665, 1659, 1650, 1641, 1631, 1627, 1620, 1613, 1608, 1605, 1598, 1591, 1586, 1581, 1574, 1569, 1564, 1561, 1553, 1544, 1534, 1522, 1513, 1505, 1497, 1490, 1485, 1485, 1483, 1482, 1477, 1471, 1465, 1459, 1450, 1444, 1437, 1431, 1433, 1432, 1432, 1436, 1440, 1444, 1451, 1458, 1468, 1479, 1495, 1506, 1518, 1524, 1535, 1539, 1552, 1567, 1585, 1599, 1620, 1630, 1643, 1650, 1649, 1667, 1683, 1704, 1714, 1719, 1727, 1736, 1741, 1738, 1735, 1732, 1729, 1728, 1731, 1736, 1735, 1735, 1730, 1728, 1726, 1719, 1711, 1703, 1694, 1688, 1687, 1684, 1674, 1660, 1648, 1635, 1622, 1606, 1593, 1585, 1575, 1560, 1548, 1536, 1525, 1509, 1494, 1483, 1472, 1460, 1445, 1439, 1429, 1420, 1861, 1858, 1855, 1854, 1853, 1852, 1850, 1845, 1842, 1841, 1840, 1845, 1851, 1857, 1859, 1858, 1861, 1867, 1865, 1858, 1853, 1846, 1837, 1828, 1821, 1817, 1811, 1801, 1791, 1782, 1777, 1776, 1778, 1783, 1790, 1795, 1794, 1789, 1783, 1777, 1774, 1770, 1766, 1763, 1759, 1751, 1740, 1728, 1715, 1709, 1705, 1701, 1694, 1693, 1694, 1699, 1704, 1709, 1714, 1719, 1719, 1715, 1704, 1692, 1686, 1683, 1681, 1678, 1673, 1669, 1665, 1658, 1650, 1639, 1632, 1626, 1619, 1612, 1608, 1603, 1598, 1595, 1586, 1576, 1569, 1563, 1560, 1555, 1548, 1537, 1521, 1510, 1501, 1494, 1491, 1486, 1485, 1480, 1478, 1475, 1469, 1462, 1451, 1442, 1437, 1434, 1432, 1433, 1431, 1432, 1435, 1439, 1442, 1448, 1459, 1467, 1476, 1489, 1500, 1515, 1523, 1529, 1533, 1540, 1557, 1577, 1595, 1611, 1623, 1636, 1649, 1648, 1671, 1682, 1698, 1706, 1713, 1721, 1733, 1742, 1740, 1738, 1738, 1738, 1740, 1744, 1748, 1745, 1743, 1740, 1740, 1739, 1735, 1728, 1720, 1710, 1702, 1698, 1695, 1687, 1670, 1656, 1642, 1627, 1612, 1599, 1589, 1580, 1570, 1557, 1540, 1527, 1510, 1494, 1484, 1479, 1464, 1450, 1443, 1434, 1424, 1866, 1864, 1861, 1860, 1859, 1857, 1855, 1851, 1846, 1844, 1844, 1849, 1855, 1861, 1863, 1864, 1867, 1873, 1869, 1869, 1860, 1853, 1845, 1834, 1825, 1818, 1810, 1804, 1797, 1791, 1785, 1785, 1787, 1795, 1801, 1803, 1802, 1796, 1790, 1786, 1784, 1781, 1776, 1771, 1766, 1757, 1746, 1736, 1725, 1716, 1712, 1707, 1698, 1696, 1697, 1699, 1704, 1711, 1717, 1722, 1722, 1721, 1714, 1701, 1691, 1688, 1686, 1681, 1676, 1672, 1668, 1663, 1658, 1649, 1640, 1632, 1625, 1617, 1611, 1609, 1605, 1601, 1590, 1579, 1571, 1565, 1561, 1557, 1551, 1541, 1525, 1515, 1503, 1497, 1495, 1490, 1485, 1479, 1479, 1478, 1473, 1459, 1448, 1438, 1432, 1430, 1431, 1430, 1433, 1435, 1438, 1440, 1442, 1451, 1460, 1466, 1474, 1483, 1495, 1511, 1519, 1525, 1528, 1533, 1546, 1565, 1588, 1602, 1617, 1627, 1643, 1651, 1665, 1677, 1690, 1700, 1706, 1712, 1726, 1738, 1738, 1737, 1742, 1746, 1750, 1754, 1757, 1754, 1752, 1751, 1751, 1750, 1746, 1741, 1732, 1720, 1711, 1705, 1699, 1691, 1678, 1664, 1649, 1634, 1618, 1604, 1591, 1580, 1573, 1562, 1545, 1524, 1505, 1492, 1481, 1477, 1465, 1451, 1442, 1433, 1425, 1874, 1870, 1868, 1868, 1867, 1863, 1857, 1853, 1849, 1848, 1851, 1854, 1859, 1865, 1866, 1870, 1871, 1873, 1874, 1876, 1868, 1859, 1853, 1839, 1831, 1816, 1807, 1805, 1799, 1797, 1793, 1793, 1796, 1804, 1810, 1809, 1809, 1804, 1797, 1797, 1794, 1791, 1786, 1779, 1769, 1760, 1750, 1742, 1733, 1725, 1722, 1717, 1704, 1700, 1701, 1705, 1708, 1713, 1718, 1720, 1721, 1724, 1722, 1712, 1699, 1692, 1689, 1684, 1677, 1671, 1668, 1665, 1663, 1657, 1647, 1639, 1632, 1623, 1615, 1612, 1609, 1602, 1592, 1582, 1575, 1570, 1565, 1559, 1551, 1542, 1531, 1523, 1511, 1505, 1501, 1495, 1488, 1481, 1481, 1480, 1474, 1455, 1446, 1437, 1428, 1428, 1429, 1429, 1433, 1435, 1440, 1440, 1444, 1456, 1460, 1465, 1471, 1480, 1494, 1505, 1515, 1523, 1525, 1531, 1539, 1554, 1579, 1597, 1612, 1620, 1631, 1645, 1657, 1670, 1682, 1692, 1698, 1704, 1718, 1731, 1733, 1735, 1744, 1752, 1756, 1761, 1765, 1765, 1763, 1762, 1760, 1757, 1750, 1746, 1738, 1724, 1712, 1704, 1696, 1686, 1678, 1668, 1654, 1639, 1623, 1606, 1590, 1578, 1571, 1563, 1546, 1522, 1501, 1490, 1479, 1472, 1462, 1448, 1436, 1428, 1422, 1882, 1876, 1874, 1874, 1875, 1868, 1861, 1856, 1853, 1854, 1859, 1859, 1863, 1870, 1873, 1879, 1877, 1875, 1882, 1877, 1874, 1863, 1858, 1844, 1834, 1814, 1807, 1805, 1804, 1802, 1802, 1803, 1806, 1813, 1818, 1817, 1818, 1812, 1806, 1806, 1802, 1801, 1796, 1790, 1775, 1762, 1753, 1746, 1742, 1736, 1735, 1729, 1714, 1706, 1706, 1711, 1714, 1716, 1719, 1718, 1720, 1723, 1726, 1722, 1708, 1698, 1691, 1681, 1674, 1670, 1667, 1666, 1665, 1660, 1652, 1648, 1640, 1631, 1622, 1617, 1613, 1603, 1594, 1585, 1578, 1576, 1571, 1563, 1552, 1543, 1535, 1527, 1520, 1514, 1508, 1501, 1493, 1485, 1480, 1478, 1471, 1454, 1443, 1436, 1427, 1430, 1431, 1430, 1429, 1431, 1437, 1438, 1445, 1455, 1459, 1462, 1469, 1479, 1495, 1498, 1510, 1518, 1525, 1528, 1536, 1550, 1572, 1592, 1603, 1613, 1617, 1631, 1653, 1663, 1671, 1682, 1691, 1699, 1712, 1724, 1729, 1732, 1745, 1758, 1761, 1768, 1775, 1776, 1771, 1769, 1765, 1759, 1751, 1746, 1738, 1724, 1710, 1700, 1689, 1679, 1672, 1666, 1657, 1644, 1627, 1606, 1588, 1576, 1566, 1559, 1543, 1522, 1501, 1488, 1477, 1465, 1455, 1440, 1428, 1419, 1413, 1884, 1882, 1880, 1882, 1882, 1874, 1866, 1863, 1857, 1860, 1864, 1865, 1869, 1881, 1884, 1890, 1890, 1880, 1881, 1879, 1876, 1870, 1860, 1854, 1834, 1822, 1811, 1812, 1812, 1811, 1813, 1817, 1818, 1820, 1822, 1821, 1822, 1816, 1816, 1811, 1810, 1809, 1804, 1798, 1787, 1771, 1762, 1754, 1749, 1746, 1744, 1739, 1726, 1717, 1715, 1718, 1719, 1720, 1719, 1718, 1719, 1722, 1727, 1727, 1715, 1702, 1691, 1679, 1672, 1670, 1668, 1666, 1666, 1658, 1656, 1655, 1647, 1639, 1631, 1626, 1621, 1608, 1597, 1587, 1581, 1580, 1573, 1567, 1556, 1546, 1539, 1528, 1524, 1522, 1511, 1501, 1495, 1488, 1481, 1477, 1470, 1459, 1446, 1434, 1433, 1432, 1434, 1429, 1426, 1430, 1433, 1438, 1445, 1448, 1454, 1459, 1469, 1477, 1494, 1496, 1504, 1509, 1519, 1522, 1534, 1542, 1562, 1581, 1594, 1600, 1609, 1626, 1648, 1657, 1662, 1673, 1687, 1698, 1707, 1713, 1724, 1728, 1741, 1760, 1766, 1773, 1780, 1778, 1770, 1767, 1760, 1755, 1748, 1741, 1731, 1718, 1707, 1693, 1681, 1675, 1670, 1663, 1654, 1643, 1626, 1603, 1585, 1571, 1561, 1550, 1537, 1522, 1502, 1486, 1475, 1460, 1448, 1433, 1424, 1414, 1405, 1885, 1886, 1889, 1891, 1886, 1886, 1884, 1885, 1867, 1869, 1870, 1872, 1880, 1885, 1890, 1894, 1896, 1895, 1888, 1885, 1879, 1876, 1876, 1860, 1839, 1826, 1825, 1829, 1825, 1825, 1823, 1827, 1828, 1828, 1823, 1823, 1821, 1819, 1815, 1818, 1817, 1814, 1811, 1805, 1798, 1784, 1772, 1762, 1753, 1752, 1749, 1743, 1732, 1721, 1723, 1724, 1724, 1722, 1721, 1720, 1721, 1722, 1728, 1726, 1716, 1705, 1691, 1682, 1675, 1672, 1670, 1668, 1665, 1663, 1659, 1655, 1650, 1642, 1639, 1634, 1628, 1614, 1601, 1591, 1585, 1578, 1573, 1570, 1560, 1551, 1542, 1531, 1527, 1520, 1509, 1498, 1492, 1484, 1480, 1477, 1471, 1457, 1441, 1435, 1436, 1433, 1431, 1431, 1429, 1430, 1434, 1441, 1447, 1446, 1450, 1457, 1466, 1474, 1488, 1493, 1492, 1501, 1508, 1518, 1522, 1538, 1554, 1569, 1583, 1595, 1597, 1620, 1639, 1648, 1653, 1665, 1685, 1699, 1702, 1706, 1716, 1727, 1742, 1764, 1769, 1777, 1779, 1777, 1770, 1761, 1755, 1745, 1735, 1729, 1720, 1709, 1698, 1684, 1675, 1669, 1665, 1656, 1645, 1629, 1613, 1593, 1574, 1563, 1556, 1538, 1524, 1514, 1498, 1483, 1468, 1457, 1446, 1430, 1419, 1407, 1398, 1890, 1890, 1892, 1896, 1891, 1890, 1892, 1890, 1893, 1882, 1878, 1880, 1884, 1887, 1892, 1898, 1899, 1900, 1895, 1895, 1885, 1885, 1877, 1862, 1839, 1833, 1834, 1839, 1839, 1840, 1835, 1832, 1827, 1824, 1822, 1823, 1819, 1818, 1818, 1819, 1816, 1811, 1808, 1804, 1799, 1792, 1781, 1770, 1763, 1758, 1753, 1747, 1735, 1727, 1725, 1722, 1722, 1723, 1723, 1724, 1731, 1732, 1729, 1728, 1722, 1710, 1696, 1688, 1680, 1678, 1676, 1672, 1673, 1675, 1666, 1661, 1655, 1647, 1645, 1641, 1634, 1619, 1606, 1596, 1586, 1579, 1572, 1570, 1566, 1557, 1544, 1536, 1524, 1516, 1506, 1493, 1486, 1480, 1478, 1476, 1461, 1451, 1439, 1430, 1430, 1431, 1432, 1429, 1430, 1431, 1436, 1443, 1441, 1444, 1449, 1455, 1461, 1472, 1477, 1483, 1486, 1491, 1500, 1512, 1518, 1528, 1540, 1551, 1572, 1580, 1595, 1610, 1622, 1643, 1648, 1662, 1675, 1687, 1693, 1703, 1712, 1725, 1747, 1758, 1763, 1769, 1768, 1763, 1756, 1746, 1742, 1731, 1720, 1710, 1699, 1690, 1680, 1669, 1661, 1655, 1651, 1642, 1628, 1613, 1598, 1588, 1571, 1558, 1546, 1525, 1511, 1498, 1487, 1474, 1465, 1458, 1443, 1425, 1407, 1392, 1381, 1885, 1884, 1886, 1887, 1894, 1892, 1891, 1892, 1898, 1897, 1891, 1895, 1893, 1896, 1898, 1899, 1901, 1900, 1898, 1896, 1891, 1892, 1883, 1871, 1855, 1841, 1842, 1848, 1849, 1848, 1846, 1840, 1836, 1828, 1824, 1825, 1824, 1822, 1823, 1822, 1818, 1812, 1810, 1806, 1802, 1798, 1788, 1779, 1772, 1767, 1760, 1749, 1741, 1730, 1728, 1724, 1720, 1720, 1725, 1729, 1734, 1733, 1731, 1731, 1729, 1722, 1710, 1694, 1683, 1681, 1680, 1678, 1681, 1679, 1672, 1666, 1657, 1650, 1645, 1644, 1636, 1628, 1613, 1599, 1589, 1582, 1577, 1575, 1568, 1564, 1555, 1538, 1522, 1510, 1500, 1487, 1479, 1475, 1475, 1466, 1454, 1448, 1432, 1423, 1418, 1427, 1426, 1426, 1428, 1431, 1435, 1439, 1442, 1446, 1451, 1455, 1459, 1466, 1471, 1478, 1483, 1492, 1502, 1510, 1516, 1524, 1531, 1542, 1558, 1576, 1589, 1602, 1610, 1624, 1638, 1656, 1673, 1684, 1693, 1699, 1711, 1726, 1740, 1749, 1757, 1761, 1761, 1754, 1742, 1733, 1724, 1714, 1702, 1690, 1678, 1666, 1658, 1650, 1648, 1646, 1640, 1631, 1617, 1601, 1587, 1576, 1563, 1553, 1543, 1516, 1499, 1494, 1480, 1470, 1459, 1448, 1431, 1414, 1399, 1384, 1371, 1885, 1884, 1886, 1890, 1895, 1890, 1893, 1894, 1898, 1902, 1904, 1906, 1903, 1905, 1910, 1909, 1906, 1908, 1907, 1906, 1903, 1901, 1890, 1876, 1866, 1854, 1854, 1862, 1862, 1860, 1853, 1848, 1843, 1836, 1829, 1829, 1828, 1830, 1830, 1827, 1817, 1813, 1812, 1808, 1806, 1803, 1799, 1788, 1781, 1776, 1768, 1756, 1746, 1737, 1732, 1726, 1720, 1722, 1728, 1732, 1735, 1734, 1734, 1737, 1732, 1727, 1722, 1706, 1689, 1681, 1678, 1681, 1686, 1685, 1679, 1672, 1664, 1656, 1650, 1645, 1639, 1631, 1618, 1605, 1596, 1590, 1585, 1579, 1570, 1564, 1561, 1541, 1519, 1504, 1491, 1480, 1473, 1471, 1468, 1460, 1450, 1439, 1426, 1418, 1415, 1423, 1424, 1424, 1428, 1434, 1435, 1437, 1439, 1443, 1452, 1456, 1457, 1461, 1467, 1473, 1480, 1492, 1502, 1510, 1517, 1521, 1528, 1541, 1554, 1575, 1586, 1594, 1603, 1616, 1636, 1651, 1672, 1689, 1696, 1706, 1716, 1731, 1745, 1748, 1751, 1751, 1752, 1744, 1730, 1720, 1708, 1696, 1682, 1670, 1659, 1649, 1641, 1632, 1628, 1626, 1624, 1613, 1603, 1595, 1580, 1570, 1558, 1549, 1534, 1510, 1493, 1486, 1473, 1462, 1449, 1437, 1420, 1403, 1388, 1374, 1364, 1889, 1889, 1890, 1894, 1894, 1892, 1898, 1900, 1900, 1902, 1905, 1909, 1909, 1917, 1921, 1923, 1918, 1917, 1920, 1917, 1911, 1907, 1896, 1883, 1873, 1866, 1867, 1872, 1874, 1872, 1865, 1858, 1851, 1845, 1837, 1836, 1834, 1835, 1837, 1832, 1821, 1816, 1813, 1812, 1811, 1810, 1806, 1793, 1784, 1777, 1770, 1760, 1751, 1743, 1738, 1733, 1730, 1731, 1733, 1736, 1736, 1736, 1738, 1738, 1735, 1728, 1723, 1712, 1698, 1689, 1684, 1686, 1690, 1687, 1683, 1678, 1670, 1662, 1657, 1650, 1643, 1632, 1621, 1611, 1603, 1596, 1590, 1582, 1571, 1562, 1553, 1537, 1511, 1496, 1483, 1474, 1469, 1468, 1461, 1455, 1447, 1436, 1426, 1418, 1414, 1418, 1421, 1422, 1427, 1434, 1433, 1433, 1436, 1439, 1450, 1454, 1454, 1458, 1464, 1470, 1477, 1487, 1499, 1508, 1516, 1521, 1526, 1540, 1558, 1576, 1586, 1590, 1603, 1614, 1635, 1656, 1671, 1693, 1709, 1718, 1728, 1738, 1749, 1752, 1751, 1748, 1743, 1732, 1719, 1709, 1693, 1679, 1663, 1651, 1643, 1636, 1627, 1615, 1609, 1606, 1603, 1592, 1582, 1578, 1572, 1565, 1554, 1540, 1526, 1506, 1491, 1477, 1465, 1453, 1441, 1428, 1412, 1397, 1382, 1369, 1359, 1889, 1891, 1893, 1895, 1894, 1894, 1900, 1906, 1906, 1905, 1905, 1914, 1916, 1924, 1929, 1933, 1930, 1930, 1935, 1928, 1917, 1909, 1903, 1890, 1881, 1879, 1879, 1883, 1886, 1882, 1876, 1870, 1862, 1856, 1848, 1844, 1842, 1841, 1841, 1836, 1831, 1823, 1818, 1818, 1818, 1817, 1812, 1798, 1788, 1778, 1772, 1764, 1757, 1749, 1744, 1739, 1736, 1738, 1739, 1741, 1741, 1741, 1740, 1738, 1737, 1731, 1722, 1715, 1707, 1703, 1699, 1696, 1694, 1691, 1690, 1686, 1679, 1671, 1665, 1658, 1649, 1635, 1623, 1615, 1607, 1599, 1591, 1582, 1568, 1558, 1544, 1527, 1498, 1485, 1477, 1471, 1467, 1465, 1455, 1450, 1439, 1433, 1425, 1419, 1413, 1415, 1419, 1420, 1426, 1432, 1432, 1431, 1435, 1436, 1444, 1449, 1452, 1457, 1463, 1470, 1477, 1488, 1497, 1509, 1515, 1522, 1528, 1542, 1562, 1576, 1589, 1592, 1604, 1614, 1635, 1662, 1675, 1698, 1721, 1727, 1738, 1744, 1751, 1754, 1752, 1746, 1736, 1723, 1709, 1697, 1679, 1663, 1647, 1636, 1628, 1623, 1613, 1598, 1593, 1589, 1582, 1572, 1561, 1559, 1556, 1550, 1541, 1529, 1518, 1504, 1490, 1473, 1459, 1446, 1435, 1424, 1406, 1389, 1375, 1365, 1353, 1889, 1890, 1892, 1894, 1895, 1895, 1898, 1909, 1911, 1912, 1912, 1922, 1925, 1929, 1934, 1939, 1941, 1946, 1948, 1940, 1928, 1916, 1911, 1899, 1893, 1894, 1893, 1895, 1897, 1890, 1884, 1880, 1871, 1868, 1859, 1854, 1852, 1850, 1848, 1844, 1841, 1832, 1828, 1825, 1826, 1823, 1818, 1807, 1795, 1784, 1776, 1769, 1763, 1757, 1748, 1740, 1738, 1740, 1743, 1745, 1745, 1745, 1744, 1742, 1742, 1736, 1727, 1720, 1714, 1713, 1712, 1706, 1700, 1699, 1699, 1696, 1689, 1681, 1675, 1667, 1656, 1641, 1626, 1618, 1608, 1597, 1591, 1579, 1564, 1551, 1536, 1514, 1486, 1477, 1474, 1469, 1464, 1459, 1448, 1441, 1431, 1427, 1422, 1418, 1413, 1414, 1418, 1420, 1424, 1428, 1430, 1432, 1433, 1435, 1440, 1446, 1451, 1455, 1460, 1468, 1477, 1493, 1500, 1509, 1514, 1520, 1530, 1544, 1561, 1575, 1589, 1593, 1602, 1616, 1638, 1662, 1681, 1705, 1726, 1733, 1741, 1748, 1753, 1753, 1749, 1741, 1731, 1719, 1702, 1687, 1671, 1653, 1638, 1627, 1616, 1610, 1599, 1584, 1579, 1573, 1563, 1552, 1545, 1544, 1536, 1530, 1525, 1518, 1511, 1501, 1486, 1471, 1458, 1443, 1429, 1421, 1400, 1377, 1364, 1354, 1341, 1891, 1892, 1891, 1894, 1895, 1898, 1898, 1908, 1916, 1918, 1921, 1927, 1933, 1937, 1940, 1945, 1949, 1955, 1955, 1950, 1943, 1934, 1922, 1913, 1909, 1911, 1909, 1906, 1904, 1897, 1890, 1886, 1876, 1878, 1866, 1863, 1861, 1859, 1860, 1853, 1846, 1839, 1836, 1833, 1831, 1828, 1824, 1815, 1804, 1792, 1783, 1775, 1769, 1765, 1752, 1740, 1740, 1740, 1743, 1746, 1745, 1748, 1749, 1749, 1748, 1743, 1734, 1725, 1720, 1718, 1715, 1709, 1705, 1703, 1702, 1702, 1697, 1690, 1684, 1672, 1657, 1645, 1630, 1620, 1609, 1596, 1587, 1574, 1559, 1544, 1520, 1498, 1482, 1477, 1472, 1466, 1461, 1449, 1441, 1429, 1427, 1422, 1419, 1413, 1412, 1414, 1416, 1419, 1423, 1423, 1427, 1430, 1434, 1438, 1441, 1445, 1449, 1451, 1455, 1462, 1473, 1490, 1501, 1505, 1513, 1519, 1528, 1543, 1556, 1574, 1586, 1592, 1602, 1622, 1644, 1661, 1684, 1710, 1726, 1737, 1743, 1748, 1754, 1752, 1743, 1737, 1727, 1714, 1698, 1679, 1668, 1653, 1636, 1621, 1610, 1600, 1585, 1571, 1564, 1556, 1547, 1535, 1530, 1531, 1523, 1518, 1514, 1508, 1501, 1490, 1477, 1465, 1456, 1441, 1424, 1417, 1393, 1367, 1354, 1337, 1328, 1885, 1890, 1889, 1892, 1895, 1898, 1905, 1907, 1916, 1919, 1927, 1933, 1935, 1939, 1945, 1951, 1957, 1958, 1958, 1956, 1951, 1948, 1939, 1930, 1926, 1924, 1919, 1916, 1912, 1903, 1896, 1890, 1886, 1882, 1874, 1874, 1872, 1868, 1864, 1855, 1851, 1845, 1839, 1838, 1833, 1834, 1830, 1823, 1814, 1803, 1796, 1790, 1781, 1769, 1756, 1744, 1743, 1748, 1749, 1749, 1749, 1748, 1753, 1755, 1753, 1750, 1742, 1731, 1726, 1724, 1720, 1712, 1706, 1704, 1704, 1707, 1704, 1699, 1690, 1676, 1659, 1643, 1632, 1622, 1610, 1598, 1583, 1568, 1553, 1530, 1506, 1489, 1479, 1477, 1471, 1466, 1455, 1443, 1438, 1425, 1416, 1419, 1416, 1410, 1411, 1416, 1418, 1420, 1422, 1425, 1427, 1430, 1434, 1437, 1439, 1442, 1446, 1449, 1450, 1462, 1475, 1482, 1494, 1504, 1514, 1520, 1530, 1542, 1559, 1574, 1590, 1602, 1615, 1633, 1650, 1667, 1689, 1715, 1731, 1742, 1745, 1753, 1757, 1752, 1739, 1731, 1721, 1707, 1684, 1660, 1655, 1642, 1630, 1614, 1602, 1589, 1574, 1562, 1551, 1539, 1530, 1524, 1519, 1514, 1506, 1502, 1498, 1494, 1486, 1476, 1464, 1455, 1445, 1429, 1422, 1407, 1383, 1363, 1343, 1329, 1320, 1878, 1881, 1886, 1891, 1895, 1901, 1902, 1911, 1917, 1925, 1937, 1934, 1938, 1940, 1943, 1953, 1961, 1966, 1963, 1962, 1964, 1959, 1953, 1944, 1938, 1923, 1918, 1917, 1915, 1908, 1904, 1901, 1894, 1892, 1881, 1878, 1876, 1872, 1869, 1865, 1854, 1853, 1849, 1845, 1838, 1842, 1841, 1833, 1823, 1816, 1812, 1804, 1794, 1782, 1769, 1757, 1753, 1761, 1761, 1757, 1758, 1759, 1760, 1765, 1766, 1760, 1750, 1739, 1736, 1732, 1725, 1718, 1707, 1705, 1706, 1705, 1706, 1700, 1690, 1679, 1664, 1646, 1632, 1622, 1608, 1596, 1584, 1564, 1541, 1511, 1496, 1487, 1483, 1478, 1472, 1463, 1446, 1438, 1431, 1423, 1415, 1415, 1412, 1409, 1414, 1419, 1420, 1420, 1423, 1429, 1429, 1431, 1433, 1435, 1438, 1443, 1446, 1450, 1456, 1466, 1472, 1486, 1496, 1505, 1516, 1520, 1534, 1554, 1569, 1586, 1604, 1616, 1628, 1644, 1657, 1671, 1696, 1713, 1733, 1747, 1748, 1754, 1755, 1749, 1737, 1727, 1717, 1701, 1672, 1647, 1641, 1628, 1617, 1603, 1591, 1576, 1559, 1548, 1540, 1531, 1520, 1511, 1500, 1493, 1484, 1479, 1476, 1473, 1470, 1463, 1454, 1444, 1433, 1423, 1414, 1399, 1376, 1358, 1342, 1331, 1320, 1877, 1879, 1883, 1888, 1892, 1896, 1898, 1907, 1919, 1929, 1934, 1937, 1931, 1938, 1945, 1953, 1954, 1961, 1970, 1974, 1972, 1969, 1959, 1954, 1941, 1933, 1928, 1920, 1917, 1913, 1910, 1905, 1904, 1903, 1891, 1885, 1885, 1882, 1879, 1882, 1864, 1863, 1856, 1850, 1848, 1850, 1849, 1844, 1839, 1829, 1822, 1812, 1803, 1792, 1778, 1770, 1768, 1765, 1765, 1763, 1764, 1765, 1769, 1773, 1772, 1767, 1758, 1749, 1744, 1736, 1727, 1719, 1713, 1709, 1706, 1702, 1697, 1689, 1681, 1670, 1660, 1643, 1630, 1619, 1605, 1587, 1574, 1557, 1525, 1501, 1489, 1485, 1484, 1479, 1467, 1453, 1440, 1432, 1426, 1419, 1413, 1410, 1415, 1412, 1415, 1416, 1420, 1421, 1422, 1426, 1429, 1432, 1433, 1433, 1435, 1441, 1443, 1444, 1454, 1465, 1471, 1485, 1496, 1513, 1519, 1528, 1542, 1560, 1577, 1583, 1605, 1626, 1638, 1654, 1673, 1686, 1698, 1713, 1733, 1745, 1755, 1757, 1756, 1750, 1740, 1725, 1710, 1690, 1667, 1647, 1629, 1618, 1610, 1601, 1585, 1562, 1546, 1535, 1527, 1518, 1508, 1498, 1486, 1477, 1467, 1459, 1457, 1454, 1450, 1447, 1439, 1429, 1420, 1409, 1402, 1389, 1369, 1348, 1337, 1328, 1323, 1874, 1874, 1878, 1883, 1887, 1889, 1891, 1898, 1912, 1925, 1932, 1930, 1932, 1931, 1936, 1943, 1956, 1969, 1971, 1970, 1972, 1970, 1964, 1964, 1947, 1937, 1933, 1930, 1925, 1921, 1919, 1917, 1915, 1912, 1899, 1894, 1892, 1890, 1887, 1887, 1879, 1872, 1862, 1855, 1851, 1855, 1855, 1854, 1852, 1845, 1835, 1821, 1807, 1798, 1787, 1782, 1775, 1773, 1774, 1772, 1773, 1773, 1773, 1773, 1773, 1770, 1763, 1756, 1750, 1741, 1730, 1724, 1718, 1709, 1702, 1696, 1688, 1678, 1669, 1660, 1649, 1633, 1621, 1611, 1600, 1581, 1564, 1549, 1519, 1495, 1484, 1483, 1481, 1477, 1462, 1448, 1438, 1431, 1425, 1419, 1415, 1412, 1413, 1412, 1413, 1412, 1419, 1421, 1422, 1423, 1426, 1429, 1431, 1432, 1434, 1438, 1442, 1445, 1453, 1464, 1470, 1486, 1504, 1518, 1520, 1535, 1553, 1568, 1583, 1595, 1620, 1635, 1653, 1671, 1684, 1697, 1708, 1717, 1731, 1745, 1753, 1760, 1760, 1755, 1738, 1713, 1695, 1680, 1661, 1637, 1618, 1612, 1606, 1594, 1574, 1555, 1540, 1528, 1515, 1507, 1497, 1483, 1468, 1455, 1444, 1438, 1437, 1437, 1434, 1432, 1428, 1422, 1411, 1398, 1385, 1374, 1361, 1347, 1335, 1324, 1314, 1864, 1864, 1868, 1873, 1880, 1880, 1884, 1893, 1906, 1917, 1924, 1926, 1932, 1933, 1940, 1944, 1954, 1969, 1971, 1974, 1973, 1972, 1969, 1967, 1956, 1944, 1938, 1935, 1928, 1927, 1928, 1927, 1924, 1918, 1911, 1906, 1901, 1896, 1892, 1888, 1883, 1877, 1868, 1859, 1855, 1858, 1857, 1858, 1858, 1854, 1844, 1830, 1815, 1805, 1799, 1790, 1784, 1781, 1778, 1778, 1781, 1782, 1778, 1776, 1774, 1770, 1766, 1761, 1755, 1745, 1736, 1731, 1721, 1706, 1695, 1687, 1679, 1672, 1661, 1650, 1639, 1625, 1614, 1599, 1587, 1571, 1552, 1532, 1509, 1489, 1480, 1479, 1479, 1470, 1455, 1445, 1434, 1428, 1422, 1415, 1409, 1409, 1409, 1410, 1411, 1409, 1417, 1420, 1421, 1421, 1426, 1426, 1429, 1430, 1432, 1436, 1443, 1446, 1450, 1457, 1470, 1487, 1507, 1520, 1524, 1540, 1560, 1574, 1591, 1607, 1627, 1643, 1663, 1686, 1700, 1708, 1715, 1724, 1734, 1748, 1756, 1764, 1761, 1754, 1734, 1703, 1687, 1674, 1658, 1635, 1616, 1610, 1599, 1580, 1562, 1547, 1535, 1520, 1506, 1496, 1483, 1466, 1449, 1434, 1426, 1421, 1418, 1420, 1419, 1416, 1414, 1411, 1402, 1390, 1377, 1368, 1358, 1344, 1327, 1313, 1299, 1859, 1857, 1862, 1866, 1871, 1872, 1877, 1886, 1898, 1908, 1918, 1927, 1931, 1935, 1943, 1946, 1953, 1961, 1970, 1974, 1976, 1977, 1973, 1965, 1960, 1951, 1947, 1942, 1939, 1936, 1935, 1935, 1930, 1924, 1919, 1916, 1912, 1903, 1898, 1891, 1885, 1882, 1874, 1864, 1861, 1864, 1863, 1862, 1861, 1857, 1849, 1838, 1827, 1814, 1807, 1799, 1793, 1787, 1784, 1787, 1790, 1792, 1787, 1780, 1776, 1772, 1770, 1767, 1762, 1749, 1740, 1733, 1720, 1703, 1690, 1679, 1673, 1665, 1654, 1643, 1635, 1623, 1609, 1593, 1576, 1561, 1541, 1522, 1503, 1484, 1479, 1476, 1471, 1459, 1448, 1441, 1431, 1425, 1418, 1411, 1405, 1405, 1406, 1407, 1410, 1411, 1416, 1419, 1420, 1422, 1425, 1425, 1427, 1429, 1433, 1439, 1445, 1448, 1451, 1456, 1466, 1483, 1507, 1519, 1531, 1548, 1566, 1579, 1597, 1614, 1633, 1656, 1676, 1696, 1707, 1715, 1721, 1734, 1743, 1754, 1764, 1768, 1762, 1751, 1728, 1700, 1690, 1675, 1657, 1636, 1617, 1609, 1594, 1570, 1551, 1540, 1529, 1509, 1493, 1480, 1466, 1450, 1434, 1420, 1413, 1409, 1406, 1408, 1408, 1403, 1400, 1399, 1392, 1383, 1373, 1365, 1354, 1339, 1321, 1307, 1289, 1860, 1857, 1860, 1864, 1866, 1868, 1873, 1881, 1890, 1901, 1915, 1926, 1929, 1933, 1940, 1944, 1953, 1958, 1965, 1968, 1972, 1975, 1972, 1964, 1960, 1955, 1954, 1951, 1948, 1943, 1940, 1940, 1931, 1926, 1921, 1920, 1918, 1912, 1908, 1899, 1893, 1889, 1882, 1873, 1869, 1871, 1869, 1865, 1861, 1856, 1851, 1844, 1836, 1824, 1815, 1808, 1802, 1795, 1795, 1796, 1798, 1798, 1794, 1786, 1780, 1776, 1775, 1773, 1766, 1752, 1740, 1732, 1720, 1704, 1688, 1676, 1667, 1656, 1646, 1637, 1630, 1620, 1606, 1590, 1569, 1550, 1532, 1516, 1500, 1484, 1481, 1477, 1464, 1451, 1442, 1435, 1428, 1422, 1415, 1409, 1404, 1406, 1404, 1408, 1411, 1414, 1416, 1418, 1420, 1423, 1426, 1427, 1429, 1431, 1437, 1443, 1447, 1451, 1455, 1460, 1466, 1481, 1508, 1521, 1539, 1556, 1571, 1585, 1602, 1617, 1642, 1663, 1685, 1700, 1709, 1717, 1728, 1745, 1751, 1759, 1769, 1768, 1760, 1747, 1722, 1702, 1694, 1676, 1656, 1635, 1618, 1607, 1590, 1564, 1545, 1537, 1521, 1498, 1480, 1463, 1451, 1439, 1425, 1412, 1404, 1399, 1396, 1399, 1399, 1391, 1387, 1385, 1381, 1378, 1370, 1359, 1349, 1333, 1315, 1306, 1286, 1863, 1862, 1862, 1864, 1863, 1866, 1872, 1881, 1885, 1894, 1910, 1921, 1927, 1931, 1936, 1941, 1951, 1961, 1960, 1964, 1966, 1969, 1969, 1966, 1961, 1958, 1957, 1955, 1948, 1944, 1944, 1940, 1933, 1927, 1921, 1919, 1918, 1921, 1920, 1911, 1905, 1898, 1890, 1884, 1879, 1877, 1875, 1869, 1861, 1856, 1853, 1847, 1840, 1832, 1826, 1818, 1811, 1806, 1805, 1802, 1802, 1800, 1798, 1792, 1785, 1782, 1779, 1774, 1764, 1750, 1738, 1728, 1720, 1708, 1690, 1675, 1660, 1649, 1640, 1630, 1620, 1610, 1599, 1585, 1564, 1541, 1525, 1511, 1497, 1486, 1485, 1477, 1462, 1447, 1436, 1431, 1426, 1420, 1413, 1407, 1404, 1407, 1405, 1409, 1413, 1415, 1417, 1416, 1419, 1424, 1428, 1430, 1433, 1436, 1441, 1445, 1449, 1454, 1459, 1465, 1471, 1485, 1508, 1526, 1544, 1560, 1573, 1589, 1605, 1620, 1645, 1661, 1682, 1699, 1711, 1720, 1735, 1752, 1755, 1761, 1768, 1763, 1754, 1740, 1719, 1704, 1691, 1674, 1654, 1633, 1619, 1604, 1585, 1562, 1542, 1532, 1512, 1490, 1472, 1454, 1443, 1433, 1421, 1408, 1397, 1391, 1385, 1387, 1386, 1378, 1375, 1370, 1368, 1367, 1361, 1350, 1340, 1326, 1308, 1302, 1284, 1868, 1865, 1863, 1859, 1858, 1861, 1868, 1878, 1883, 1887, 1902, 1915, 1926, 1932, 1937, 1941, 1947, 1957, 1964, 1967, 1968, 1968, 1972, 1969, 1963, 1962, 1957, 1954, 1948, 1941, 1942, 1939, 1935, 1928, 1922, 1918, 1920, 1922, 1926, 1923, 1915, 1906, 1894, 1890, 1887, 1884, 1882, 1875, 1864, 1860, 1855, 1848, 1844, 1838, 1834, 1827, 1817, 1815, 1811, 1808, 1805, 1801, 1797, 1791, 1787, 1785, 1779, 1769, 1756, 1742, 1729, 1720, 1713, 1702, 1691, 1674, 1659, 1649, 1638, 1624, 1611, 1599, 1588, 1577, 1559, 1538, 1526, 1514, 1501, 1491, 1486, 1473, 1459, 1445, 1432, 1429, 1425, 1416, 1412, 1409, 1407, 1405, 1405, 1409, 1413, 1414, 1417, 1412, 1417, 1425, 1428, 1430, 1435, 1438, 1442, 1446, 1451, 1455, 1460, 1467, 1474, 1488, 1507, 1527, 1546, 1560, 1572, 1587, 1604, 1622, 1639, 1657, 1676, 1693, 1705, 1720, 1736, 1747, 1752, 1758, 1762, 1756, 1749, 1732, 1714, 1698, 1683, 1668, 1649, 1631, 1619, 1603, 1585, 1566, 1538, 1523, 1504, 1485, 1468, 1450, 1439, 1431, 1419, 1402, 1390, 1383, 1377, 1371, 1370, 1365, 1364, 1358, 1352, 1349, 1344, 1338, 1325, 1313, 1300, 1293, 1281, 1873, 1862, 1857, 1854, 1852, 1859, 1865, 1873, 1877, 1882, 1892, 1904, 1915, 1929, 1938, 1940, 1948, 1954, 1953, 1963, 1963, 1964, 1972, 1966, 1958, 1965, 1961, 1955, 1946, 1946, 1944, 1941, 1936, 1927, 1920, 1916, 1921, 1927, 1929, 1927, 1917, 1904, 1892, 1891, 1889, 1887, 1886, 1877, 1866, 1863, 1857, 1850, 1846, 1842, 1838, 1832, 1824, 1819, 1815, 1813, 1810, 1802, 1794, 1786, 1779, 1776, 1773, 1760, 1743, 1732, 1722, 1711, 1699, 1689, 1682, 1672, 1658, 1656, 1645, 1627, 1613, 1597, 1582, 1567, 1553, 1540, 1525, 1511, 1502, 1495, 1483, 1465, 1451, 1442, 1433, 1425, 1419, 1412, 1412, 1411, 1409, 1409, 1411, 1411, 1413, 1415, 1417, 1414, 1416, 1424, 1429, 1432, 1438, 1443, 1440, 1443, 1448, 1455, 1461, 1472, 1485, 1497, 1506, 1519, 1538, 1555, 1567, 1585, 1601, 1614, 1628, 1645, 1670, 1685, 1699, 1714, 1728, 1736, 1749, 1757, 1754, 1752, 1743, 1723, 1704, 1686, 1672, 1660, 1645, 1626, 1614, 1602, 1585, 1563, 1535, 1515, 1495, 1477, 1460, 1446, 1437, 1431, 1420, 1401, 1383, 1373, 1367, 1360, 1356, 1352, 1345, 1340, 1338, 1335, 1330, 1323, 1311, 1304, 1292, 1282, 1269, 1879, 1867, 1859, 1858, 1855, 1860, 1865, 1869, 1873, 1881, 1889, 1897, 1907, 1920, 1931, 1941, 1942, 1953, 1954, 1956, 1957, 1957, 1963, 1959, 1965, 1958, 1956, 1949, 1943, 1939, 1939, 1936, 1930, 1917, 1912, 1910, 1912, 1919, 1927, 1925, 1917, 1903, 1896, 1890, 1890, 1890, 1885, 1879, 1873, 1864, 1857, 1848, 1842, 1840, 1837, 1832, 1826, 1822, 1819, 1812, 1804, 1795, 1786, 1777, 1771, 1764, 1757, 1746, 1730, 1720, 1713, 1702, 1689, 1680, 1674, 1665, 1660, 1654, 1644, 1632, 1620, 1604, 1587, 1564, 1549, 1536, 1518, 1504, 1494, 1486, 1474, 1457, 1445, 1436, 1429, 1420, 1416, 1417, 1415, 1413, 1412, 1407, 1414, 1413, 1414, 1416, 1417, 1414, 1419, 1423, 1429, 1437, 1437, 1444, 1445, 1446, 1449, 1454, 1463, 1475, 1490, 1499, 1508, 1513, 1528, 1548, 1562, 1581, 1598, 1612, 1625, 1647, 1664, 1678, 1692, 1705, 1723, 1735, 1746, 1749, 1750, 1747, 1735, 1714, 1694, 1676, 1659, 1645, 1634, 1620, 1612, 1598, 1577, 1552, 1528, 1516, 1497, 1473, 1455, 1442, 1435, 1426, 1411, 1394, 1376, 1364, 1356, 1346, 1342, 1338, 1332, 1324, 1319, 1316, 1313, 1304, 1294, 1287, 1284, 1275, 1258, 1879, 1870, 1864, 1862, 1860, 1861, 1865, 1867, 1870, 1876, 1885, 1895, 1901, 1912, 1924, 1934, 1941, 1941, 1943, 1947, 1948, 1949, 1949, 1946, 1954, 1954, 1953, 1942, 1937, 1934, 1932, 1929, 1922, 1911, 1903, 1906, 1907, 1914, 1920, 1918, 1910, 1896, 1892, 1890, 1889, 1888, 1881, 1876, 1871, 1863, 1855, 1846, 1840, 1836, 1832, 1828, 1823, 1820, 1814, 1803, 1793, 1784, 1777, 1769, 1763, 1756, 1743, 1731, 1721, 1709, 1701, 1693, 1681, 1671, 1665, 1659, 1654, 1646, 1636, 1627, 1618, 1604, 1582, 1559, 1540, 1524, 1508, 1496, 1486, 1474, 1463, 1454, 1443, 1433, 1425, 1418, 1414, 1416, 1415, 1412, 1412, 1411, 1412, 1412, 1414, 1415, 1415, 1416, 1418, 1422, 1427, 1435, 1442, 1443, 1450, 1450, 1456, 1461, 1467, 1477, 1492, 1504, 1510, 1514, 1521, 1545, 1562, 1580, 1591, 1604, 1619, 1643, 1659, 1676, 1694, 1702, 1712, 1726, 1739, 1749, 1752, 1746, 1733, 1711, 1689, 1670, 1656, 1642, 1625, 1613, 1606, 1593, 1567, 1543, 1526, 1514, 1492, 1472, 1457, 1442, 1433, 1412, 1392, 1383, 1368, 1356, 1344, 1334, 1333, 1327, 1320, 1311, 1305, 1302, 1298, 1292, 1283, 1275, 1268, 1259, 1242, 1886, 1876, 1871, 1866, 1862, 1860, 1859, 1859, 1865, 1871, 1880, 1891, 1901, 1908, 1920, 1928, 1932, 1930, 1932, 1936, 1937, 1939, 1940, 1940, 1942, 1942, 1939, 1932, 1930, 1924, 1920, 1919, 1912, 1905, 1897, 1900, 1902, 1907, 1910, 1906, 1899, 1889, 1884, 1881, 1877, 1876, 1871, 1865, 1862, 1855, 1844, 1834, 1831, 1829, 1825, 1821, 1818, 1814, 1805, 1792, 1782, 1774, 1765, 1757, 1751, 1742, 1729, 1716, 1708, 1698, 1690, 1683, 1676, 1666, 1658, 1651, 1645, 1637, 1624, 1615, 1602, 1587, 1568, 1549, 1530, 1513, 1498, 1488, 1480, 1466, 1455, 1448, 1438, 1428, 1418, 1415, 1413, 1414, 1413, 1410, 1409, 1411, 1413, 1412, 1413, 1414, 1412, 1416, 1419, 1423, 1428, 1435, 1441, 1444, 1453, 1454, 1458, 1463, 1471, 1479, 1492, 1504, 1513, 1515, 1524, 1543, 1558, 1576, 1584, 1596, 1615, 1641, 1658, 1672, 1690, 1693, 1706, 1724, 1741, 1752, 1752, 1745, 1732, 1712, 1690, 1670, 1654, 1638, 1622, 1608, 1600, 1585, 1563, 1539, 1520, 1508, 1495, 1483, 1463, 1442, 1430, 1407, 1384, 1374, 1358, 1340, 1334, 1329, 1325, 1319, 1311, 1302, 1295, 1288, 1284, 1281, 1274, 1269, 1259, 1246, 1230, 1892, 1884, 1878, 1868, 1861, 1856, 1856, 1853, 1860, 1867, 1874, 1888, 1901, 1908, 1916, 1922, 1924, 1925, 1929, 1930, 1931, 1933, 1933, 1934, 1932, 1929, 1925, 1921, 1916, 1911, 1908, 1907, 1902, 1895, 1889, 1889, 1894, 1897, 1898, 1894, 1889, 1880, 1872, 1868, 1864, 1861, 1858, 1852, 1848, 1841, 1832, 1823, 1822, 1820, 1816, 1812, 1811, 1804, 1796, 1783, 1770, 1760, 1752, 1745, 1739, 1728, 1716, 1703, 1693, 1684, 1678, 1675, 1670, 1661, 1652, 1646, 1637, 1625, 1612, 1601, 1585, 1569, 1552, 1536, 1518, 1502, 1490, 1482, 1473, 1459, 1450, 1441, 1434, 1425, 1418, 1415, 1412, 1411, 1406, 1406, 1406, 1408, 1410, 1410, 1414, 1414, 1413, 1416, 1419, 1423, 1429, 1436, 1439, 1447, 1452, 1457, 1460, 1464, 1470, 1478, 1491, 1504, 1513, 1519, 1527, 1542, 1560, 1574, 1583, 1592, 1615, 1635, 1650, 1661, 1683, 1689, 1704, 1725, 1742, 1752, 1752, 1746, 1734, 1720, 1698, 1676, 1659, 1641, 1627, 1610, 1599, 1582, 1557, 1533, 1518, 1510, 1499, 1485, 1467, 1444, 1426, 1405, 1385, 1369, 1350, 1332, 1325, 1319, 1316, 1315, 1306, 1295, 1286, 1278, 1272, 1269, 1266, 1261, 1251, 1236, 1222, 1900, 1893, 1882, 1871, 1863, 1858, 1856, 1852, 1857, 1866, 1870, 1883, 1898, 1906, 1910, 1915, 1920, 1924, 1928, 1928, 1927, 1927, 1927, 1926, 1922, 1915, 1911, 1906, 1899, 1897, 1896, 1895, 1890, 1882, 1880, 1880, 1884, 1886, 1885, 1882, 1877, 1868, 1857, 1850, 1848, 1843, 1840, 1837, 1832, 1827, 1819, 1811, 1809, 1806, 1801, 1797, 1793, 1788, 1783, 1773, 1760, 1748, 1740, 1734, 1726, 1715, 1703, 1689, 1676, 1669, 1669, 1667, 1660, 1651, 1642, 1635, 1627, 1613, 1599, 1586, 1570, 1554, 1537, 1517, 1503, 1491, 1482, 1474, 1464, 1453, 1445, 1434, 1428, 1423, 1419, 1414, 1411, 1408, 1403, 1402, 1404, 1404, 1405, 1408, 1415, 1414, 1415, 1416, 1420, 1424, 1429, 1439, 1444, 1450, 1453, 1459, 1463, 1467, 1472, 1480, 1494, 1504, 1512, 1521, 1529, 1542, 1560, 1571, 1584, 1595, 1613, 1627, 1641, 1653, 1677, 1689, 1705, 1724, 1741, 1750, 1752, 1749, 1740, 1729, 1710, 1688, 1667, 1650, 1634, 1614, 1600, 1583, 1556, 1532, 1522, 1512, 1502, 1487, 1470, 1447, 1426, 1408, 1391, 1370, 1349, 1330, 1317, 1310, 1309, 1311, 1300, 1289, 1280, 1273, 1266, 1260, 1255, 1248, 1238, 1225, 1214, 1908, 1900, 1885, 1873, 1865, 1861, 1858, 1856, 1855, 1863, 1866, 1875, 1888, 1900, 1905, 1910, 1917, 1922, 1925, 1925, 1921, 1918, 1921, 1918, 1910, 1900, 1895, 1889, 1886, 1886, 1887, 1885, 1880, 1873, 1873, 1875, 1875, 1875, 1873, 1871, 1865, 1853, 1841, 1833, 1829, 1823, 1819, 1818, 1814, 1811, 1806, 1796, 1791, 1787, 1782, 1777, 1772, 1770, 1767, 1760, 1751, 1740, 1734, 1726, 1714, 1702, 1690, 1675, 1662, 1658, 1659, 1654, 1645, 1635, 1628, 1618, 1612, 1600, 1585, 1570, 1555, 1537, 1518, 1499, 1488, 1482, 1475, 1465, 1455, 1446, 1436, 1427, 1422, 1419, 1415, 1411, 1408, 1403, 1405, 1402, 1403, 1402, 1402, 1406, 1411, 1413, 1415, 1416, 1420, 1426, 1431, 1439, 1449, 1453, 1458, 1461, 1466, 1470, 1478, 1485, 1498, 1506, 1513, 1522, 1531, 1543, 1557, 1570, 1585, 1601, 1614, 1624, 1639, 1655, 1674, 1691, 1707, 1724, 1739, 1748, 1751, 1749, 1742, 1732, 1718, 1699, 1674, 1656, 1636, 1616, 1601, 1585, 1561, 1538, 1526, 1513, 1502, 1489, 1471, 1447, 1427, 1412, 1392, 1369, 1349, 1329, 1314, 1307, 1304, 1300, 1292, 1283, 1277, 1268, 1260, 1253, 1244, 1234, 1224, 1214, 1207, 1912, 1902, 1887, 1870, 1862, 1859, 1860, 1861, 1856, 1859, 1861, 1866, 1874, 1889, 1900, 1906, 1913, 1917, 1921, 1917, 1913, 1908, 1913, 1908, 1897, 1888, 1881, 1877, 1878, 1878, 1880, 1878, 1874, 1869, 1868, 1868, 1865, 1862, 1860, 1861, 1852, 1837, 1828, 1821, 1815, 1806, 1796, 1794, 1793, 1793, 1788, 1778, 1768, 1764, 1760, 1755, 1755, 1754, 1747, 1745, 1739, 1732, 1729, 1722, 1703, 1687, 1675, 1662, 1655, 1650, 1641, 1634, 1628, 1619, 1615, 1605, 1593, 1583, 1570, 1554, 1536, 1516, 1499, 1486, 1479, 1475, 1469, 1458, 1447, 1436, 1426, 1419, 1414, 1411, 1407, 1406, 1403, 1400, 1406, 1405, 1402, 1402, 1400, 1404, 1408, 1409, 1411, 1414, 1417, 1426, 1432, 1436, 1448, 1456, 1459, 1463, 1467, 1471, 1482, 1490, 1500, 1512, 1518, 1527, 1535, 1545, 1559, 1572, 1586, 1605, 1620, 1628, 1644, 1660, 1674, 1693, 1710, 1728, 1742, 1750, 1751, 1746, 1740, 1730, 1719, 1704, 1680, 1655, 1632, 1616, 1603, 1585, 1559, 1540, 1526, 1512, 1497, 1483, 1463, 1442, 1425, 1408, 1384, 1361, 1344, 1330, 1317, 1308, 1298, 1289, 1283, 1277, 1272, 1261, 1253, 1247, 1238, 1228, 1216, 1207, 1201, 1915, 1900, 1885, 1870, 1862, 1860, 1859, 1859, 1858, 1857, 1859, 1862, 1865, 1875, 1892, 1901, 1907, 1909, 1912, 1912, 1909, 1908, 1905, 1898, 1883, 1876, 1867, 1870, 1872, 1873, 1873, 1872, 1869, 1864, 1861, 1858, 1855, 1851, 1845, 1848, 1834, 1819, 1815, 1810, 1801, 1789, 1773, 1766, 1769, 1773, 1765, 1754, 1743, 1739, 1736, 1736, 1737, 1730, 1724, 1727, 1724, 1721, 1719, 1708, 1689, 1671, 1659, 1654, 1649, 1636, 1621, 1617, 1616, 1607, 1598, 1592, 1578, 1566, 1552, 1537, 1519, 1500, 1489, 1478, 1472, 1467, 1460, 1450, 1440, 1427, 1417, 1410, 1407, 1406, 1399, 1401, 1401, 1402, 1405, 1403, 1401, 1399, 1398, 1404, 1407, 1406, 1409, 1412, 1415, 1425, 1430, 1437, 1448, 1457, 1462, 1464, 1472, 1477, 1484, 1492, 1498, 1510, 1525, 1534, 1542, 1553, 1565, 1577, 1591, 1608, 1623, 1638, 1651, 1667, 1679, 1697, 1718, 1734, 1747, 1752, 1753, 1748, 1738, 1727, 1717, 1705, 1683, 1655, 1628, 1613, 1599, 1576, 1547, 1533, 1521, 1508, 1490, 1472, 1454, 1438, 1422, 1404, 1381, 1357, 1344, 1336, 1324, 1309, 1295, 1285, 1278, 1271, 1264, 1257, 1251, 1241, 1232, 1224, 1213, 1203, 1199, 1914, 1900, 1886, 1872, 1862, 1857, 1855, 1860, 1859, 1858, 1858, 1859, 1863, 1871, 1881, 1891, 1899, 1905, 1908, 1909, 1908, 1904, 1892, 1882, 1870, 1861, 1854, 1857, 1864, 1862, 1862, 1860, 1858, 1853, 1854, 1853, 1850, 1846, 1838, 1822, 1814, 1804, 1797, 1791, 1782, 1773, 1759, 1755, 1749, 1741, 1738, 1734, 1721, 1713, 1710, 1712, 1712, 1704, 1704, 1708, 1709, 1707, 1703, 1695, 1678, 1662, 1653, 1650, 1637, 1622, 1617, 1613, 1606, 1596, 1586, 1574, 1559, 1548, 1537, 1519, 1500, 1486, 1479, 1474, 1467, 1460, 1451, 1443, 1433, 1419, 1413, 1405, 1403, 1408, 1402, 1401, 1401, 1405, 1402, 1402, 1403, 1399, 1398, 1409, 1411, 1406, 1406, 1410, 1417, 1420, 1425, 1436, 1452, 1464, 1470, 1470, 1476, 1481, 1487, 1491, 1498, 1511, 1532, 1546, 1555, 1562, 1573, 1585, 1598, 1618, 1632, 1647, 1658, 1678, 1691, 1704, 1721, 1740, 1749, 1753, 1753, 1749, 1740, 1725, 1713, 1698, 1676, 1651, 1624, 1606, 1595, 1571, 1546, 1533, 1520, 1505, 1484, 1461, 1448, 1434, 1420, 1406, 1385, 1363, 1349, 1344, 1328, 1310, 1293, 1281, 1273, 1263, 1256, 1249, 1242, 1235, 1228, 1220, 1209, 1201, 1198, 1913, 1900, 1887, 1871, 1860, 1853, 1854, 1864, 1861, 1858, 1857, 1857, 1861, 1869, 1877, 1884, 1888, 1894, 1894, 1894, 1895, 1889, 1884, 1876, 1865, 1853, 1842, 1845, 1845, 1845, 1843, 1843, 1842, 1842, 1842, 1840, 1839, 1835, 1822, 1811, 1796, 1788, 1780, 1775, 1765, 1748, 1740, 1736, 1728, 1721, 1723, 1713, 1703, 1696, 1689, 1687, 1690, 1687, 1686, 1689, 1687, 1681, 1682, 1676, 1661, 1652, 1642, 1635, 1626, 1618, 1611, 1606, 1595, 1584, 1575, 1561, 1545, 1536, 1524, 1505, 1488, 1477, 1474, 1468, 1461, 1451, 1442, 1433, 1422, 1413, 1409, 1406, 1406, 1406, 1411, 1405, 1406, 1408, 1408, 1409, 1410, 1402, 1402, 1405, 1405, 1405, 1405, 1408, 1419, 1422, 1427, 1438, 1454, 1468, 1475, 1480, 1482, 1485, 1488, 1495, 1505, 1520, 1538, 1550, 1561, 1566, 1576, 1587, 1605, 1626, 1637, 1659, 1670, 1691, 1702, 1710, 1725, 1736, 1745, 1751, 1752, 1746, 1736, 1724, 1714, 1700, 1677, 1652, 1628, 1608, 1590, 1569, 1554, 1541, 1525, 1507, 1487, 1466, 1449, 1435, 1425, 1410, 1398, 1379, 1358, 1347, 1332, 1314, 1295, 1281, 1270, 1259, 1252, 1248, 1241, 1234, 1227, 1217, 1207, 1200, 1197, 1911, 1898, 1885, 1867, 1857, 1853, 1856, 1861, 1861, 1859, 1857, 1857, 1860, 1865, 1873, 1876, 1882, 1882, 1883, 1878, 1879, 1877, 1874, 1866, 1852, 1842, 1838, 1835, 1836, 1835, 1833, 1834, 1835, 1834, 1832, 1829, 1825, 1821, 1812, 1792, 1782, 1776, 1771, 1764, 1749, 1731, 1720, 1711, 1704, 1698, 1695, 1689, 1685, 1683, 1674, 1670, 1670, 1670, 1669, 1671, 1668, 1662, 1661, 1658, 1653, 1644, 1633, 1625, 1616, 1608, 1601, 1596, 1581, 1570, 1563, 1551, 1540, 1525, 1511, 1498, 1483, 1478, 1472, 1464, 1455, 1444, 1430, 1420, 1412, 1405, 1403, 1411, 1406, 1405, 1410, 1406, 1406, 1410, 1409, 1410, 1408, 1404, 1406, 1405, 1404, 1403, 1403, 1407, 1417, 1424, 1432, 1448, 1462, 1474, 1482, 1486, 1487, 1488, 1492, 1500, 1513, 1524, 1542, 1555, 1565, 1570, 1581, 1589, 1603, 1622, 1635, 1659, 1670, 1694, 1703, 1709, 1720, 1730, 1739, 1747, 1747, 1743, 1735, 1723, 1713, 1702, 1675, 1649, 1629, 1610, 1594, 1580, 1561, 1539, 1524, 1509, 1487, 1463, 1446, 1432, 1423, 1411, 1403, 1389, 1367, 1354, 1338, 1316, 1297, 1283, 1272, 1260, 1253, 1246, 1239, 1232, 1225, 1216, 1208, 1202, 1196, 1907, 1894, 1883, 1863, 1853, 1852, 1856, 1859, 1860, 1856, 1856, 1856, 1858, 1861, 1869, 1873, 1877, 1873, 1872, 1866, 1865, 1862, 1859, 1853, 1845, 1837, 1831, 1831, 1831, 1827, 1827, 1826, 1826, 1824, 1822, 1816, 1812, 1809, 1798, 1781, 1771, 1763, 1755, 1745, 1731, 1714, 1700, 1692, 1686, 1682, 1679, 1677, 1676, 1674, 1665, 1662, 1660, 1657, 1655, 1654, 1656, 1654, 1654, 1652, 1643, 1628, 1612, 1604, 1596, 1590, 1587, 1580, 1569, 1562, 1554, 1544, 1534, 1517, 1500, 1487, 1479, 1478, 1469, 1458, 1446, 1430, 1417, 1410, 1405, 1405, 1407, 1413, 1408, 1404, 1406, 1401, 1403, 1408, 1410, 1411, 1407, 1403, 1405, 1405, 1404, 1401, 1399, 1404, 1413, 1423, 1441, 1456, 1470, 1480, 1487, 1489, 1489, 1490, 1498, 1508, 1523, 1533, 1540, 1555, 1567, 1574, 1583, 1591, 1604, 1618, 1633, 1651, 1661, 1680, 1698, 1704, 1714, 1724, 1733, 1743, 1742, 1742, 1736, 1723, 1711, 1700, 1675, 1646, 1630, 1612, 1595, 1582, 1560, 1536, 1520, 1505, 1486, 1465, 1446, 1431, 1421, 1408, 1401, 1390, 1372, 1357, 1337, 1313, 1296, 1284, 1273, 1264, 1256, 1248, 1240, 1233, 1224, 1217, 1209, 1203, 1199, 1904, 1894, 1887, 1866, 1855, 1852, 1857, 1856, 1855, 1854, 1855, 1857, 1858, 1864, 1869, 1873, 1872, 1866, 1863, 1858, 1854, 1847, 1844, 1840, 1838, 1833, 1828, 1829, 1827, 1824, 1823, 1819, 1817, 1816, 1814, 1807, 1801, 1794, 1787, 1771, 1757, 1746, 1733, 1724, 1713, 1697, 1686, 1679, 1673, 1672, 1669, 1668, 1669, 1667, 1661, 1655, 1647, 1644, 1642, 1643, 1645, 1645, 1646, 1644, 1630, 1610, 1596, 1588, 1582, 1577, 1575, 1566, 1558, 1553, 1547, 1537, 1523, 1503, 1486, 1477, 1474, 1471, 1459, 1450, 1435, 1419, 1410, 1405, 1404, 1409, 1410, 1410, 1408, 1404, 1403, 1400, 1402, 1405, 1408, 1409, 1406, 1404, 1403, 1404, 1405, 1399, 1396, 1403, 1414, 1424, 1444, 1462, 1473, 1479, 1484, 1487, 1488, 1494, 1504, 1516, 1531, 1537, 1543, 1555, 1566, 1574, 1581, 1591, 1602, 1611, 1626, 1641, 1650, 1662, 1685, 1696, 1708, 1720, 1730, 1740, 1740, 1744, 1736, 1723, 1709, 1695, 1673, 1648, 1631, 1613, 1593, 1578, 1559, 1535, 1518, 1504, 1488, 1468, 1448, 1432, 1419, 1406, 1396, 1385, 1370, 1353, 1331, 1310, 1296, 1283, 1275, 1267, 1260, 1252, 1243, 1235, 1225, 1218, 1212, 1207, 1203, 1904, 1897, 1888, 1871, 1860, 1854, 1858, 1854, 1853, 1853, 1858, 1861, 1865, 1872, 1872, 1875, 1873, 1866, 1862, 1856, 1848, 1839, 1833, 1832, 1831, 1831, 1828, 1828, 1824, 1822, 1818, 1813, 1812, 1812, 1809, 1802, 1795, 1784, 1780, 1762, 1743, 1727, 1716, 1705, 1696, 1684, 1674, 1666, 1662, 1662, 1660, 1661, 1660, 1658, 1655, 1642, 1629, 1627, 1627, 1630, 1630, 1629, 1630, 1626, 1616, 1600, 1590, 1580, 1574, 1571, 1564, 1553, 1546, 1545, 1541, 1529, 1513, 1490, 1478, 1470, 1465, 1458, 1446, 1439, 1427, 1415, 1410, 1408, 1408, 1411, 1411, 1407, 1406, 1404, 1402, 1400, 1401, 1403, 1405, 1406, 1406, 1404, 1401, 1402, 1403, 1399, 1397, 1403, 1415, 1427, 1440, 1463, 1471, 1477, 1481, 1485, 1489, 1497, 1507, 1520, 1532, 1537, 1546, 1555, 1564, 1570, 1577, 1588, 1598, 1607, 1619, 1631, 1641, 1651, 1670, 1686, 1701, 1719, 1730, 1738, 1740, 1744, 1734, 1722, 1708, 1687, 1670, 1649, 1630, 1612, 1590, 1574, 1558, 1535, 1518, 1505, 1488, 1466, 1444, 1429, 1416, 1402, 1390, 1378, 1364, 1345, 1327, 1311, 1299, 1285, 1278, 1270, 1263, 1255, 1246, 1238, 1228, 1220, 1215, 1212, 1207, 1908, 1901, 1887, 1875, 1866, 1858, 1859, 1855, 1856, 1854, 1863, 1869, 1875, 1880, 1877, 1879, 1879, 1874, 1869, 1860, 1849, 1839, 1832, 1831, 1827, 1829, 1827, 1827, 1822, 1817, 1813, 1811, 1811, 1811, 1807, 1801, 1793, 1780, 1771, 1754, 1732, 1716, 1707, 1691, 1684, 1674, 1663, 1654, 1652, 1652, 1653, 1654, 1647, 1642, 1639, 1624, 1614, 1611, 1611, 1612, 1611, 1611, 1611, 1606, 1601, 1593, 1586, 1572, 1563, 1560, 1548, 1537, 1532, 1533, 1528, 1517, 1504, 1486, 1476, 1466, 1453, 1444, 1436, 1428, 1421, 1414, 1411, 1411, 1412, 1412, 1410, 1407, 1404, 1403, 1402, 1398, 1398, 1401, 1402, 1404, 1404, 1404, 1398, 1400, 1398, 1398, 1399, 1402, 1414, 1428, 1436, 1459, 1468, 1478, 1483, 1487, 1491, 1498, 1508, 1522, 1530, 1540, 1547, 1555, 1560, 1564, 1570, 1581, 1594, 1605, 1614, 1625, 1634, 1645, 1660, 1676, 1694, 1720, 1731, 1737, 1739, 1739, 1732, 1721, 1705, 1682, 1665, 1645, 1625, 1606, 1584, 1568, 1555, 1533, 1517, 1503, 1483, 1458, 1438, 1424, 1410, 1394, 1383, 1372, 1359, 1340, 1327, 1315, 1305, 1294, 1285, 1276, 1267, 1258, 1250, 1243, 1233, 1225, 1219, 1217, 1213, 1909, 1902, 1889, 1882, 1873, 1866, 1862, 1857, 1858, 1860, 1867, 1876, 1881, 1880, 1879, 1882, 1887, 1884, 1878, 1869, 1855, 1847, 1840, 1834, 1829, 1827, 1824, 1825, 1818, 1815, 1814, 1812, 1811, 1810, 1808, 1803, 1792, 1777, 1763, 1747, 1727, 1714, 1697, 1685, 1676, 1666, 1657, 1647, 1644, 1642, 1644, 1640, 1629, 1618, 1617, 1611, 1605, 1601, 1596, 1592, 1592, 1595, 1598, 1592, 1589, 1582, 1574, 1557, 1546, 1538, 1528, 1523, 1519, 1513, 1508, 1502, 1491, 1481, 1473, 1458, 1443, 1435, 1430, 1420, 1413, 1411, 1410, 1412, 1414, 1413, 1410, 1406, 1404, 1402, 1401, 1397, 1397, 1398, 1399, 1401, 1401, 1403, 1398, 1397, 1396, 1396, 1400, 1403, 1413, 1424, 1438, 1453, 1466, 1477, 1485, 1489, 1492, 1500, 1511, 1525, 1534, 1543, 1548, 1554, 1556, 1558, 1563, 1574, 1587, 1603, 1611, 1623, 1632, 1641, 1656, 1666, 1688, 1721, 1731, 1736, 1738, 1737, 1732, 1719, 1702, 1678, 1660, 1638, 1614, 1595, 1575, 1559, 1550, 1530, 1511, 1499, 1477, 1451, 1435, 1420, 1402, 1386, 1379, 1370, 1358, 1342, 1330, 1322, 1314, 1304, 1294, 1284, 1272, 1261, 1254, 1247, 1238, 1232, 1227, 1226, 1224, 1908, 1901, 1894, 1887, 1878, 1873, 1867, 1862, 1861, 1862, 1866, 1879, 1885, 1882, 1883, 1884, 1886, 1885, 1883, 1874, 1864, 1854, 1848, 1840, 1834, 1831, 1827, 1822, 1817, 1814, 1816, 1814, 1813, 1811, 1810, 1804, 1794, 1779, 1763, 1753, 1734, 1712, 1694, 1684, 1674, 1665, 1658, 1642, 1634, 1633, 1630, 1624, 1621, 1602, 1601, 1600, 1593, 1589, 1580, 1578, 1580, 1583, 1582, 1579, 1577, 1570, 1554, 1541, 1536, 1527, 1520, 1514, 1505, 1498, 1492, 1485, 1478, 1470, 1461, 1447, 1435, 1428, 1422, 1418, 1412, 1411, 1412, 1411, 1412, 1413, 1410, 1407, 1406, 1404, 1402, 1399, 1396, 1396, 1397, 1398, 1398, 1401, 1400, 1399, 1396, 1400, 1402, 1405, 1411, 1422, 1431, 1445, 1456, 1473, 1481, 1490, 1497, 1505, 1516, 1525, 1537, 1542, 1549, 1553, 1554, 1555, 1561, 1570, 1584, 1598, 1609, 1620, 1630, 1638, 1649, 1659, 1675, 1713, 1724, 1734, 1736, 1736, 1730, 1714, 1691, 1663, 1647, 1625, 1600, 1585, 1568, 1554, 1540, 1521, 1506, 1493, 1467, 1444, 1432, 1416, 1395, 1380, 1377, 1371, 1360, 1352, 1342, 1334, 1327, 1316, 1306, 1293, 1277, 1265, 1260, 1253, 1246, 1242, 1237, 1235, 1238, 1902, 1901, 1891, 1881, 1878, 1873, 1870, 1867, 1865, 1869, 1875, 1884, 1887, 1890, 1891, 1891, 1893, 1890, 1891, 1876, 1867, 1856, 1849, 1844, 1841, 1837, 1834, 1829, 1824, 1821, 1819, 1817, 1815, 1810, 1805, 1798, 1790, 1781, 1767, 1756, 1738, 1716, 1700, 1685, 1678, 1668, 1653, 1636, 1626, 1619, 1613, 1607, 1603, 1597, 1595, 1588, 1581, 1580, 1572, 1570, 1572, 1572, 1573, 1571, 1568, 1553, 1538, 1533, 1528, 1522, 1514, 1503, 1494, 1487, 1479, 1471, 1464, 1457, 1450, 1439, 1429, 1425, 1422, 1415, 1412, 1410, 1410, 1411, 1412, 1413, 1410, 1408, 1408, 1405, 1404, 1403, 1402, 1403, 1401, 1401, 1401, 1402, 1404, 1403, 1403, 1403, 1404, 1408, 1411, 1417, 1423, 1430, 1441, 1461, 1473, 1489, 1499, 1508, 1518, 1525, 1534, 1539, 1546, 1550, 1550, 1553, 1562, 1568, 1578, 1591, 1603, 1612, 1622, 1636, 1645, 1655, 1667, 1699, 1716, 1731, 1734, 1733, 1724, 1707, 1683, 1655, 1636, 1611, 1590, 1575, 1561, 1545, 1526, 1513, 1499, 1478, 1458, 1441, 1424, 1412, 1398, 1382, 1378, 1375, 1367, 1361, 1354, 1344, 1338, 1326, 1314, 1297, 1280, 1267, 1263, 1260, 1256, 1253, 1251, 1248, 1251, 1902, 1898, 1889, 1882, 1880, 1877, 1881, 1879, 1881, 1885, 1890, 1892, 1892, 1894, 1898, 1899, 1900, 1900, 1895, 1887, 1877, 1864, 1856, 1848, 1845, 1842, 1839, 1837, 1833, 1828, 1827, 1824, 1822, 1817, 1806, 1798, 1791, 1784, 1775, 1756, 1737, 1719, 1701, 1686, 1678, 1667, 1648, 1632, 1618, 1610, 1602, 1596, 1591, 1587, 1584, 1577, 1572, 1571, 1564, 1563, 1565, 1563, 1560, 1555, 1547, 1538, 1533, 1529, 1523, 1513, 1500, 1487, 1480, 1473, 1465, 1457, 1451, 1446, 1441, 1436, 1429, 1428, 1426, 1418, 1412, 1413, 1413, 1414, 1413, 1413, 1408, 1405, 1405, 1405, 1405, 1405, 1405, 1406, 1402, 1404, 1405, 1405, 1406, 1406, 1405, 1404, 1404, 1410, 1412, 1411, 1417, 1426, 1435, 1447, 1462, 1481, 1493, 1504, 1514, 1518, 1526, 1532, 1538, 1542, 1542, 1549, 1559, 1564, 1575, 1585, 1596, 1605, 1617, 1634, 1646, 1657, 1670, 1701, 1708, 1725, 1731, 1730, 1722, 1706, 1684, 1661, 1640, 1616, 1593, 1575, 1559, 1541, 1524, 1513, 1501, 1481, 1460, 1444, 1431, 1418, 1403, 1394, 1387, 1380, 1375, 1370, 1363, 1352, 1342, 1333, 1317, 1299, 1283, 1273, 1269, 1267, 1269, 1267, 1265, 1262, 1264, 1908, 1899, 1890, 1886, 1884, 1884, 1891, 1898, 1899, 1899, 1903, 1902, 1902, 1899, 1905, 1908, 1909, 1908, 1901, 1898, 1888, 1877, 1866, 1858, 1850, 1846, 1844, 1841, 1835, 1828, 1829, 1830, 1829, 1822, 1812, 1807, 1799, 1788, 1778, 1763, 1743, 1720, 1700, 1686, 1675, 1667, 1649, 1632, 1617, 1607, 1598, 1591, 1585, 1578, 1574, 1566, 1562, 1560, 1559, 1559, 1559, 1551, 1544, 1537, 1529, 1526, 1523, 1519, 1512, 1502, 1487, 1475, 1468, 1463, 1458, 1453, 1447, 1441, 1436, 1433, 1429, 1427, 1424, 1419, 1416, 1416, 1415, 1414, 1413, 1411, 1406, 1403, 1404, 1406, 1406, 1406, 1406, 1405, 1406, 1405, 1407, 1405, 1406, 1406, 1404, 1405, 1405, 1410, 1412, 1409, 1413, 1422, 1430, 1438, 1451, 1470, 1484, 1497, 1507, 1513, 1519, 1525, 1527, 1530, 1532, 1539, 1551, 1560, 1573, 1581, 1591, 1605, 1617, 1633, 1647, 1660, 1677, 1698, 1705, 1720, 1729, 1726, 1723, 1708, 1685, 1661, 1645, 1622, 1599, 1581, 1560, 1541, 1527, 1515, 1505, 1487, 1467, 1450, 1434, 1419, 1409, 1402, 1395, 1389, 1382, 1375, 1366, 1354, 1342, 1330, 1316, 1300, 1288, 1282, 1277, 1279, 1281, 1281, 1278, 1274, 1274, 1913, 1902, 1896, 1895, 1895, 1899, 1905, 1912, 1911, 1916, 1915, 1913, 1913, 1911, 1915, 1916, 1917, 1914, 1908, 1906, 1896, 1886, 1876, 1866, 1856, 1849, 1846, 1842, 1835, 1826, 1826, 1829, 1828, 1823, 1817, 1814, 1805, 1794, 1778, 1765, 1747, 1726, 1700, 1686, 1673, 1663, 1646, 1632, 1621, 1609, 1599, 1590, 1580, 1570, 1561, 1554, 1550, 1550, 1550, 1549, 1546, 1536, 1527, 1520, 1515, 1514, 1512, 1508, 1501, 1491, 1479, 1469, 1462, 1457, 1453, 1449, 1444, 1439, 1433, 1430, 1427, 1423, 1423, 1420, 1418, 1418, 1415, 1413, 1412, 1409, 1405, 1405, 1405, 1407, 1407, 1407, 1407, 1406, 1409, 1407, 1404, 1400, 1405, 1404, 1403, 1404, 1407, 1409, 1412, 1412, 1414, 1419, 1426, 1431, 1444, 1460, 1476, 1488, 1497, 1507, 1513, 1518, 1519, 1520, 1524, 1531, 1546, 1560, 1571, 1577, 1584, 1600, 1616, 1628, 1647, 1663, 1682, 1690, 1700, 1715, 1724, 1723, 1722, 1708, 1683, 1659, 1644, 1622, 1600, 1581, 1560, 1541, 1529, 1518, 1506, 1487, 1466, 1449, 1434, 1421, 1415, 1409, 1400, 1392, 1385, 1378, 1368, 1356, 1343, 1328, 1316, 1302, 1294, 1289, 1286, 1291, 1293, 1293, 1291, 1285, 1281, 1916, 1905, 1904, 1911, 1912, 1918, 1922, 1925, 1923, 1928, 1927, 1923, 1923, 1926, 1927, 1926, 1927, 1921, 1915, 1913, 1903, 1891, 1883, 1869, 1863, 1854, 1850, 1843, 1834, 1823, 1819, 1822, 1823, 1820, 1816, 1811, 1803, 1795, 1777, 1764, 1753, 1739, 1707, 1685, 1673, 1661, 1641, 1626, 1621, 1612, 1600, 1589, 1577, 1564, 1552, 1545, 1540, 1538, 1537, 1536, 1533, 1526, 1517, 1511, 1507, 1505, 1502, 1497, 1491, 1482, 1474, 1466, 1461, 1456, 1451, 1446, 1441, 1437, 1431, 1427, 1423, 1420, 1422, 1420, 1418, 1417, 1414, 1412, 1412, 1410, 1407, 1407, 1407, 1409, 1409, 1407, 1406, 1407, 1409, 1406, 1401, 1395, 1404, 1403, 1403, 1404, 1407, 1409, 1412, 1416, 1417, 1420, 1425, 1430, 1439, 1455, 1469, 1479, 1488, 1499, 1507, 1512, 1513, 1515, 1521, 1528, 1542, 1556, 1566, 1573, 1580, 1596, 1613, 1626, 1645, 1664, 1682, 1685, 1698, 1711, 1719, 1721, 1718, 1705, 1680, 1656, 1639, 1621, 1599, 1580, 1561, 1543, 1529, 1519, 1506, 1486, 1464, 1448, 1436, 1424, 1418, 1411, 1403, 1393, 1385, 1378, 1369, 1357, 1343, 1326, 1316, 1306, 1299, 1293, 1294, 1300, 1305, 1306, 1305, 1299, 1292, 1921, 1911, 1915, 1926, 1929, 1935, 1937, 1939, 1937, 1937, 1938, 1935, 1935, 1941, 1941, 1940, 1939, 1934, 1923, 1919, 1906, 1895, 1888, 1875, 1869, 1858, 1853, 1844, 1831, 1820, 1813, 1814, 1815, 1812, 1807, 1802, 1798, 1791, 1777, 1766, 1759, 1749, 1720, 1687, 1675, 1663, 1640, 1619, 1616, 1610, 1598, 1587, 1574, 1561, 1549, 1541, 1534, 1529, 1527, 1527, 1526, 1522, 1515, 1508, 1502, 1500, 1494, 1487, 1481, 1475, 1469, 1465, 1462, 1456, 1450, 1444, 1438, 1434, 1430, 1425, 1421, 1420, 1421, 1419, 1417, 1416, 1413, 1412, 1412, 1411, 1410, 1409, 1409, 1411, 1412, 1407, 1406, 1407, 1407, 1403, 1398, 1396, 1404, 1403, 1404, 1406, 1408, 1409, 1411, 1418, 1420, 1423, 1427, 1432, 1437, 1452, 1463, 1470, 1480, 1492, 1499, 1506, 1510, 1513, 1520, 1527, 1537, 1548, 1558, 1569, 1580, 1594, 1613, 1626, 1642, 1662, 1677, 1686, 1697, 1709, 1714, 1715, 1709, 1698, 1676, 1653, 1636, 1621, 1596, 1580, 1565, 1546, 1531, 1518, 1505, 1487, 1467, 1454, 1443, 1432, 1422, 1413, 1404, 1394, 1385, 1379, 1369, 1356, 1342, 1325, 1316, 1311, 1304, 1301, 1304, 1309, 1316, 1320, 1319, 1314, 1308, 1926, 1922, 1926, 1938, 1944, 1948, 1946, 1950, 1951, 1950, 1952, 1952, 1953, 1953, 1955, 1954, 1951, 1947, 1933, 1922, 1907, 1898, 1890, 1883, 1871, 1859, 1853, 1842, 1830, 1818, 1810, 1808, 1804, 1798, 1796, 1793, 1794, 1787, 1778, 1768, 1760, 1749, 1729, 1695, 1678, 1666, 1644, 1617, 1609, 1602, 1593, 1583, 1572, 1562, 1551, 1539, 1531, 1526, 1523, 1523, 1523, 1520, 1514, 1507, 1499, 1497, 1491, 1481, 1475, 1469, 1465, 1463, 1460, 1454, 1449, 1443, 1436, 1430, 1430, 1427, 1424, 1424, 1422, 1419, 1418, 1416, 1414, 1413, 1413, 1411, 1410, 1410, 1411, 1412, 1413, 1410, 1410, 1409, 1407, 1404, 1398, 1400, 1404, 1404, 1406, 1407, 1409, 1410, 1411, 1415, 1418, 1425, 1430, 1433, 1439, 1448, 1455, 1463, 1475, 1486, 1493, 1501, 1507, 1513, 1519, 1526, 1534, 1542, 1552, 1565, 1579, 1593, 1615, 1625, 1639, 1660, 1671, 1688, 1698, 1707, 1711, 1707, 1702, 1692, 1672, 1651, 1637, 1623, 1596, 1579, 1568, 1552, 1537, 1522, 1506, 1490, 1473, 1464, 1456, 1444, 1431, 1417, 1405, 1396, 1387, 1378, 1367, 1354, 1343, 1331, 1321, 1318, 1314, 1313, 1319, 1324, 1330, 1334, 1333, 1328, 1325, 1935, 1934, 1938, 1948, 1956, 1958, 1959, 1960, 1963, 1964, 1971, 1971, 1973, 1966, 1966, 1962, 1957, 1952, 1939, 1927, 1913, 1903, 1893, 1887, 1872, 1858, 1849, 1838, 1830, 1817, 1807, 1803, 1796, 1790, 1790, 1789, 1789, 1782, 1772, 1764, 1756, 1745, 1730, 1702, 1679, 1667, 1647, 1617, 1601, 1596, 1588, 1578, 1571, 1565, 1552, 1536, 1530, 1525, 1521, 1519, 1518, 1516, 1512, 1506, 1498, 1496, 1491, 1481, 1474, 1468, 1462, 1459, 1455, 1452, 1449, 1442, 1435, 1429, 1430, 1431, 1430, 1429, 1425, 1422, 1418, 1415, 1415, 1414, 1413, 1410, 1409, 1410, 1412, 1412, 1412, 1411, 1410, 1410, 1407, 1407, 1399, 1401, 1404, 1404, 1404, 1406, 1408, 1409, 1410, 1411, 1415, 1423, 1430, 1433, 1439, 1442, 1449, 1459, 1474, 1485, 1492, 1502, 1509, 1515, 1520, 1525, 1534, 1542, 1552, 1563, 1573, 1587, 1615, 1624, 1637, 1655, 1668, 1686, 1698, 1706, 1710, 1707, 1702, 1691, 1671, 1653, 1641, 1624, 1600, 1582, 1571, 1562, 1550, 1532, 1512, 1497, 1484, 1474, 1466, 1458, 1443, 1425, 1411, 1399, 1390, 1380, 1367, 1356, 1348, 1340, 1331, 1329, 1327, 1328, 1334, 1339, 1342, 1346, 1345, 1342, 1340, 1945, 1947, 1952, 1963, 1967, 1974, 1977, 1978, 1979, 1987, 1992, 1987, 1987, 1976, 1972, 1966, 1959, 1954, 1945, 1934, 1919, 1908, 1895, 1884, 1869, 1856, 1845, 1835, 1825, 1814, 1803, 1795, 1786, 1781, 1778, 1779, 1781, 1778, 1765, 1758, 1752, 1746, 1731, 1705, 1682, 1666, 1649, 1618, 1597, 1593, 1588, 1578, 1572, 1567, 1551, 1535, 1530, 1523, 1516, 1510, 1511, 1510, 1507, 1502, 1494, 1489, 1488, 1482, 1475, 1467, 1457, 1452, 1451, 1450, 1447, 1440, 1435, 1432, 1433, 1432, 1429, 1428, 1425, 1425, 1421, 1417, 1414, 1413, 1412, 1410, 1412, 1413, 1413, 1411, 1411, 1408, 1407, 1407, 1404, 1407, 1399, 1397, 1401, 1402, 1401, 1406, 1407, 1409, 1410, 1412, 1412, 1417, 1427, 1431, 1436, 1439, 1449, 1458, 1471, 1482, 1494, 1505, 1512, 1519, 1523, 1528, 1538, 1547, 1556, 1562, 1569, 1584, 1610, 1625, 1634, 1652, 1667, 1683, 1697, 1705, 1712, 1711, 1704, 1691, 1674, 1656, 1644, 1631, 1611, 1593, 1581, 1570, 1559, 1542, 1521, 1506, 1494, 1484, 1474, 1468, 1453, 1432, 1415, 1402, 1393, 1385, 1373, 1363, 1358, 1351, 1344, 1341, 1339, 1342, 1347, 1353, 1357, 1357, 1355, 1353, 1350, 1953, 1953, 1960, 1973, 1981, 1991, 1994, 1997, 1999, 2004, 2006, 2001, 1996, 1989, 1978, 1973, 1964, 1957, 1949, 1938, 1922, 1906, 1890, 1875, 1861, 1848, 1836, 1826, 1819, 1809, 1798, 1789, 1777, 1770, 1767, 1767, 1771, 1773, 1764, 1758, 1755, 1747, 1732, 1710, 1685, 1662, 1642, 1622, 1602, 1592, 1586, 1581, 1575, 1565, 1547, 1533, 1526, 1519, 1511, 1505, 1502, 1501, 1499, 1491, 1483, 1478, 1477, 1475, 1471, 1464, 1453, 1447, 1445, 1445, 1443, 1440, 1436, 1436, 1435, 1432, 1429, 1426, 1423, 1423, 1421, 1419, 1418, 1415, 1410, 1411, 1413, 1414, 1414, 1412, 1411, 1409, 1405, 1404, 1404, 1405, 1399, 1399, 1400, 1400, 1404, 1406, 1407, 1409, 1411, 1414, 1414, 1415, 1424, 1429, 1434, 1442, 1452, 1462, 1471, 1484, 1496, 1506, 1514, 1522, 1527, 1533, 1542, 1551, 1558, 1564, 1572, 1589, 1611, 1626, 1634, 1648, 1668, 1684, 1694, 1704, 1711, 1713, 1708, 1695, 1678, 1661, 1650, 1643, 1624, 1607, 1593, 1578, 1565, 1549, 1530, 1513, 1503, 1494, 1481, 1469, 1456, 1435, 1418, 1407, 1398, 1393, 1387, 1377, 1371, 1365, 1358, 1353, 1351, 1354, 1358, 1362, 1367, 1369, 1368, 1367, 1366, 1957, 1959, 1965, 1978, 1988, 1999, 2003, 2004, 2006, 2008, 2010, 2008, 1999, 1993, 1983, 1976, 1968, 1959, 1951, 1938, 1919, 1898, 1880, 1867, 1857, 1843, 1830, 1818, 1811, 1803, 1791, 1780, 1770, 1763, 1761, 1761, 1764, 1766, 1761, 1758, 1756, 1743, 1731, 1714, 1687, 1659, 1636, 1623, 1610, 1597, 1588, 1581, 1573, 1559, 1542, 1531, 1524, 1517, 1507, 1501, 1497, 1493, 1489, 1482, 1475, 1470, 1468, 1467, 1464, 1458, 1448, 1442, 1441, 1440, 1439, 1438, 1437, 1435, 1436, 1433, 1430, 1427, 1425, 1423, 1422, 1421, 1422, 1417, 1410, 1414, 1415, 1415, 1415, 1413, 1410, 1411, 1408, 1406, 1404, 1403, 1401, 1399, 1399, 1400, 1404, 1405, 1405, 1407, 1410, 1413, 1416, 1417, 1421, 1426, 1431, 1444, 1454, 1466, 1476, 1485, 1498, 1506, 1513, 1521, 1526, 1534, 1541, 1549, 1557, 1568, 1578, 1595, 1614, 1628, 1636, 1646, 1666, 1686, 1693, 1704, 1711, 1713, 1709, 1700, 1686, 1673, 1660, 1647, 1634, 1620, 1600, 1584, 1572, 1557, 1538, 1523, 1513, 1503, 1486, 1469, 1455, 1436, 1420, 1412, 1405, 1403, 1398, 1389, 1382, 1377, 1369, 1362, 1357, 1359, 1364, 1368, 1375, 1381, 1385, 1386, 1385, 1961, 1967, 1973, 1979, 1987, 1998, 2006, 2008, 2005, 2006, 2008, 2006, 2003, 1994, 1984, 1976, 1968, 1958, 1946, 1934, 1913, 1890, 1873, 1860, 1853, 1841, 1828, 1813, 1804, 1796, 1783, 1773, 1765, 1761, 1760, 1758, 1757, 1755, 1752, 1749, 1747, 1735, 1725, 1714, 1687, 1657, 1634, 1626, 1615, 1603, 1590, 1580, 1569, 1554, 1538, 1530, 1523, 1513, 1501, 1495, 1491, 1488, 1483, 1477, 1470, 1465, 1462, 1460, 1456, 1452, 1446, 1440, 1440, 1440, 1437, 1436, 1434, 1432, 1435, 1432, 1429, 1428, 1427, 1424, 1423, 1421, 1420, 1416, 1411, 1415, 1416, 1413, 1412, 1412, 1410, 1412, 1410, 1408, 1404, 1403, 1402, 1402, 1400, 1399, 1401, 1402, 1404, 1405, 1407, 1411, 1414, 1416, 1417, 1424, 1433, 1441, 1452, 1469, 1478, 1485, 1499, 1507, 1512, 1520, 1526, 1535, 1542, 1548, 1556, 1568, 1581, 1599, 1615, 1628, 1638, 1646, 1663, 1684, 1693, 1705, 1712, 1714, 1710, 1708, 1697, 1686, 1673, 1656, 1646, 1628, 1605, 1591, 1577, 1564, 1549, 1535, 1521, 1505, 1489, 1473, 1458, 1443, 1428, 1423, 1419, 1414, 1410, 1400, 1394, 1391, 1383, 1371, 1365, 1364, 1370, 1377, 1386, 1395, 1401, 1401, 1400, 1967, 1975, 1979, 1980, 1986, 1995, 2006, 2009, 2004, 2004, 2007, 2004, 2003, 1995, 1983, 1974, 1963, 1952, 1939, 1928, 1911, 1889, 1871, 1855, 1849, 1838, 1826, 1813, 1802, 1792, 1779, 1770, 1766, 1762, 1759, 1753, 1748, 1743, 1739, 1735, 1732, 1722, 1715, 1708, 1685, 1655, 1636, 1630, 1621, 1608, 1591, 1581, 1568, 1552, 1538, 1530, 1522, 1509, 1497, 1490, 1485, 1482, 1480, 1473, 1467, 1463, 1458, 1454, 1450, 1448, 1447, 1444, 1445, 1443, 1439, 1435, 1431, 1430, 1432, 1431, 1429, 1428, 1426, 1425, 1421, 1417, 1415, 1412, 1411, 1414, 1413, 1410, 1411, 1413, 1411, 1411, 1410, 1407, 1403, 1402, 1402, 1404, 1402, 1399, 1400, 1401, 1403, 1404, 1405, 1409, 1412, 1415, 1417, 1426, 1437, 1439, 1449, 1468, 1476, 1483, 1497, 1509, 1514, 1520, 1529, 1539, 1546, 1550, 1557, 1566, 1579, 1596, 1610, 1620, 1632, 1647, 1664, 1681, 1691, 1703, 1712, 1718, 1717, 1717, 1709, 1697, 1685, 1668, 1656, 1635, 1610, 1597, 1583, 1570, 1556, 1542, 1525, 1509, 1494, 1480, 1468, 1455, 1441, 1435, 1433, 1427, 1422, 1412, 1404, 1401, 1397, 1387, 1378, 1376, 1381, 1390, 1400, 1409, 1412, 1411, 1410, 1975, 1978, 1981, 1982, 1986, 1993, 2005, 2007, 2005, 2004, 2006, 2004, 2001, 1992, 1978, 1966, 1952, 1941, 1930, 1924, 1914, 1896, 1875, 1857, 1848, 1838, 1827, 1815, 1804, 1794, 1780, 1770, 1767, 1759, 1754, 1746, 1737, 1731, 1724, 1721, 1716, 1709, 1707, 1698, 1682, 1657, 1641, 1634, 1626, 1612, 1593, 1581, 1568, 1552, 1539, 1530, 1521, 1508, 1496, 1488, 1483, 1479, 1477, 1472, 1467, 1463, 1458, 1453, 1449, 1448, 1448, 1450, 1451, 1448, 1441, 1436, 1432, 1430, 1430, 1429, 1430, 1429, 1426, 1424, 1418, 1414, 1413, 1412, 1413, 1417, 1409, 1407, 1412, 1413, 1411, 1411, 1410, 1406, 1403, 1402, 1401, 1402, 1401, 1401, 1403, 1402, 1403, 1404, 1405, 1408, 1411, 1415, 1419, 1428, 1436, 1438, 1446, 1464, 1475, 1482, 1498, 1513, 1518, 1521, 1531, 1543, 1552, 1553, 1557, 1564, 1576, 1589, 1603, 1614, 1624, 1645, 1666, 1679, 1687, 1698, 1711, 1722, 1724, 1723, 1717, 1705, 1693, 1676, 1662, 1643, 1619, 1603, 1587, 1574, 1561, 1545, 1529, 1517, 1505, 1492, 1481, 1470, 1458, 1451, 1447, 1441, 1436, 1425, 1416, 1412, 1411, 1406, 1400, 1396, 1399, 1407, 1416, 1422, 1423, 1421, 1421, 1980, 1980, 1984, 1985, 1986, 1993, 2003, 2003, 2003, 2001, 2001, 2000, 1995, 1983, 1967, 1952, 1940, 1930, 1924, 1922, 1915, 1903, 1884, 1866, 1852, 1840, 1830, 1819, 1807, 1797, 1782, 1772, 1765, 1754, 1746, 1736, 1725, 1717, 1709, 1707, 1703, 1698, 1699, 1689, 1679, 1661, 1646, 1636, 1626, 1612, 1595, 1582, 1570, 1554, 1540, 1530, 1521, 1511, 1499, 1489, 1485, 1480, 1477, 1474, 1469, 1465, 1460, 1455, 1452, 1450, 1450, 1452, 1453, 1449, 1443, 1439, 1435, 1432, 1430, 1428, 1430, 1430, 1425, 1421, 1416, 1415, 1415, 1415, 1417, 1421, 1410, 1407, 1410, 1411, 1412, 1412, 1410, 1407, 1402, 1401, 1399, 1399, 1400, 1404, 1405, 1403, 1404, 1405, 1404, 1407, 1410, 1415, 1421, 1427, 1431, 1435, 1443, 1456, 1475, 1482, 1499, 1513, 1520, 1522, 1531, 1542, 1552, 1555, 1558, 1563, 1575, 1587, 1602, 1613, 1622, 1641, 1665, 1675, 1682, 1693, 1709, 1723, 1726, 1724, 1718, 1709, 1697, 1680, 1663, 1648, 1627, 1606, 1588, 1577, 1567, 1551, 1538, 1530, 1521, 1511, 1498, 1486, 1476, 1471, 1463, 1456, 1451, 1440, 1431, 1425, 1425, 1424, 1422, 1418, 1418, 1425, 1435, 1437, 1435, 1434, 1436, 1981, 1982, 1987, 1988, 1988, 1993, 1999, 1998, 1995, 1990, 1989, 1988, 1982, 1968, 1955, 1941, 1932, 1922, 1921, 1919, 1914, 1906, 1891, 1876, 1858, 1843, 1833, 1824, 1810, 1798, 1782, 1773, 1762, 1751, 1739, 1725, 1710, 1702, 1695, 1691, 1690, 1688, 1690, 1683, 1676, 1664, 1652, 1637, 1620, 1606, 1593, 1582, 1571, 1556, 1539, 1529, 1521, 1514, 1503, 1493, 1487, 1481, 1478, 1476, 1470, 1466, 1463, 1459, 1456, 1453, 1453, 1453, 1450, 1447, 1444, 1440, 1438, 1435, 1431, 1428, 1429, 1429, 1424, 1420, 1418, 1416, 1417, 1418, 1419, 1421, 1413, 1410, 1407, 1410, 1412, 1411, 1409, 1406, 1402, 1401, 1400, 1398, 1400, 1406, 1403, 1404, 1405, 1405, 1405, 1406, 1409, 1414, 1421, 1422, 1428, 1433, 1440, 1450, 1474, 1481, 1494, 1507, 1514, 1520, 1527, 1538, 1547, 1554, 1558, 1565, 1578, 1591, 1604, 1614, 1622, 1638, 1658, 1668, 1677, 1689, 1706, 1718, 1723, 1723, 1716, 1706, 1696, 1682, 1662, 1645, 1629, 1607, 1592, 1584, 1574, 1562, 1554, 1549, 1543, 1536, 1520, 1505, 1495, 1490, 1481, 1472, 1467, 1457, 1446, 1438, 1437, 1436, 1436, 1434, 1435, 1447, 1455, 1453, 1450, 1450, 1453, 1982, 1983, 1986, 1990, 1992, 1994, 1994, 1991, 1984, 1978, 1975, 1971, 1963, 1953, 1944, 1930, 1924, 1915, 1918, 1914, 1911, 1904, 1893, 1879, 1863, 1847, 1834, 1824, 1811, 1799, 1784, 1773, 1760, 1750, 1731, 1714, 1699, 1690, 1685, 1681, 1680, 1680, 1680, 1679, 1674, 1666, 1654, 1636, 1617, 1604, 1591, 1581, 1569, 1556, 1541, 1528, 1519, 1513, 1505, 1498, 1491, 1484, 1480, 1474, 1468, 1467, 1465, 1463, 1459, 1455, 1455, 1451, 1446, 1444, 1442, 1439, 1437, 1436, 1433, 1430, 1429, 1427, 1422, 1421, 1421, 1418, 1418, 1416, 1417, 1416, 1411, 1412, 1409, 1410, 1410, 1409, 1407, 1404, 1403, 1400, 1401, 1397, 1398, 1404, 1400, 1403, 1407, 1407, 1407, 1408, 1410, 1415, 1419, 1419, 1423, 1430, 1438, 1448, 1468, 1473, 1482, 1492, 1503, 1514, 1525, 1535, 1543, 1548, 1556, 1567, 1581, 1593, 1603, 1608, 1615, 1631, 1649, 1661, 1671, 1683, 1698, 1712, 1719, 1721, 1714, 1707, 1696, 1679, 1662, 1644, 1630, 1614, 1603, 1592, 1583, 1576, 1573, 1568, 1561, 1556, 1545, 1529, 1518, 1505, 1494, 1490, 1482, 1471, 1458, 1451, 1448, 1445, 1444, 1445, 1454, 1467, 1473, 1469, 1466, 1466, 1469, 1987, 1984, 1986, 1990, 1992, 1989, 1986, 1977, 1968, 1961, 1960, 1959, 1949, 1945, 1934, 1918, 1911, 1907, 1910, 1905, 1903, 1900, 1887, 1880, 1864, 1850, 1834, 1818, 1808, 1796, 1786, 1775, 1760, 1744, 1725, 1707, 1694, 1686, 1679, 1675, 1674, 1673, 1672, 1670, 1668, 1662, 1649, 1632, 1615, 1604, 1591, 1581, 1569, 1557, 1545, 1533, 1519, 1512, 1509, 1503, 1497, 1491, 1485, 1475, 1468, 1467, 1466, 1463, 1459, 1454, 1453, 1449, 1445, 1443, 1441, 1439, 1437, 1434, 1433, 1431, 1428, 1426, 1423, 1420, 1419, 1417, 1417, 1414, 1413, 1410, 1405, 1410, 1412, 1408, 1407, 1405, 1404, 1406, 1405, 1404, 1401, 1398, 1399, 1400, 1400, 1402, 1404, 1405, 1407, 1408, 1407, 1411, 1415, 1415, 1416, 1424, 1434, 1445, 1458, 1464, 1470, 1478, 1493, 1507, 1520, 1533, 1541, 1543, 1551, 1569, 1583, 1593, 1598, 1602, 1610, 1624, 1639, 1652, 1665, 1680, 1693, 1707, 1716, 1720, 1716, 1714, 1705, 1680, 1667, 1655, 1639, 1628, 1617, 1605, 1597, 1589, 1585, 1577, 1569, 1566, 1562, 1550, 1535, 1520, 1510, 1505, 1496, 1484, 1473, 1467, 1462, 1457, 1458, 1460, 1471, 1484, 1489, 1488, 1486, 1485, 1486, 1986, 1983, 1984, 1986, 1982, 1977, 1970, 1962, 1949, 1945, 1945, 1945, 1938, 1931, 1920, 1906, 1897, 1897, 1895, 1894, 1889, 1887, 1880, 1874, 1856, 1846, 1832, 1809, 1801, 1793, 1783, 1770, 1752, 1736, 1719, 1704, 1692, 1687, 1676, 1667, 1663, 1659, 1660, 1658, 1654, 1649, 1637, 1624, 1614, 1602, 1589, 1578, 1567, 1560, 1551, 1540, 1525, 1514, 1510, 1506, 1500, 1494, 1488, 1477, 1469, 1466, 1465, 1461, 1458, 1457, 1455, 1449, 1446, 1442, 1439, 1437, 1436, 1434, 1432, 1431, 1428, 1424, 1423, 1420, 1416, 1417, 1418, 1414, 1411, 1411, 1406, 1408, 1409, 1408, 1406, 1403, 1401, 1404, 1405, 1405, 1402, 1401, 1402, 1401, 1402, 1401, 1401, 1403, 1406, 1407, 1406, 1408, 1412, 1414, 1414, 1420, 1431, 1437, 1450, 1455, 1460, 1472, 1487, 1499, 1511, 1524, 1532, 1538, 1548, 1566, 1578, 1589, 1595, 1599, 1609, 1623, 1635, 1644, 1656, 1673, 1690, 1706, 1716, 1722, 1719, 1717, 1710, 1694, 1680, 1668, 1656, 1647, 1633, 1625, 1612, 1600, 1595, 1588, 1578, 1572, 1568, 1560, 1549, 1537, 1527, 1516, 1505, 1496, 1488, 1483, 1477, 1474, 1474, 1476, 1486, 1500, 1506, 1509, 1511, 1510, 1512, 1984, 1980, 1979, 1975, 1968, 1959, 1949, 1945, 1938, 1933, 1930, 1927, 1925, 1917, 1906, 1891, 1883, 1883, 1879, 1877, 1871, 1873, 1867, 1858, 1844, 1835, 1818, 1798, 1793, 1785, 1774, 1758, 1741, 1726, 1711, 1697, 1688, 1683, 1672, 1659, 1647, 1642, 1645, 1644, 1641, 1636, 1627, 1617, 1609, 1599, 1589, 1578, 1566, 1560, 1553, 1543, 1531, 1519, 1512, 1508, 1501, 1494, 1487, 1479, 1473, 1469, 1464, 1461, 1459, 1458, 1455, 1450, 1447, 1441, 1438, 1435, 1433, 1432, 1431, 1432, 1429, 1426, 1424, 1420, 1415, 1417, 1417, 1414, 1411, 1410, 1408, 1409, 1409, 1409, 1407, 1405, 1403, 1403, 1406, 1406, 1403, 1403, 1403, 1402, 1403, 1401, 1401, 1403, 1405, 1406, 1405, 1407, 1411, 1415, 1415, 1421, 1427, 1437, 1447, 1451, 1454, 1466, 1482, 1494, 1505, 1515, 1521, 1529, 1541, 1555, 1568, 1578, 1587, 1595, 1605, 1620, 1633, 1644, 1654, 1666, 1686, 1702, 1712, 1718, 1721, 1718, 1715, 1706, 1692, 1681, 1673, 1664, 1655, 1643, 1627, 1616, 1610, 1602, 1592, 1583, 1577, 1572, 1563, 1553, 1540, 1528, 1516, 1508, 1502, 1499, 1495, 1491, 1489, 1492, 1501, 1513, 1520, 1524, 1529, 1529, 1531, 1987, 1976, 1969, 1962, 1955, 1943, 1934, 1930, 1928, 1920, 1914, 1907, 1906, 1896, 1886, 1871, 1865, 1861, 1860, 1857, 1852, 1854, 1848, 1840, 1827, 1818, 1801, 1792, 1786, 1775, 1763, 1747, 1732, 1717, 1702, 1689, 1681, 1675, 1664, 1651, 1636, 1631, 1632, 1633, 1631, 1626, 1620, 1613, 1605, 1598, 1591, 1579, 1567, 1559, 1551, 1543, 1533, 1523, 1517, 1510, 1503, 1495, 1488, 1483, 1480, 1475, 1466, 1462, 1459, 1457, 1453, 1448, 1445, 1440, 1436, 1435, 1434, 1432, 1431, 1432, 1431, 1429, 1426, 1419, 1413, 1415, 1415, 1412, 1409, 1408, 1408, 1409, 1408, 1408, 1408, 1408, 1406, 1405, 1409, 1408, 1405, 1404, 1404, 1405, 1404, 1403, 1403, 1404, 1404, 1405, 1404, 1409, 1410, 1415, 1419, 1424, 1427, 1436, 1444, 1448, 1451, 1458, 1474, 1489, 1503, 1511, 1516, 1523, 1533, 1546, 1559, 1567, 1578, 1589, 1600, 1615, 1629, 1640, 1652, 1665, 1684, 1698, 1707, 1715, 1720, 1720, 1720, 1716, 1708, 1700, 1692, 1681, 1673, 1662, 1648, 1636, 1627, 1618, 1611, 1601, 1592, 1584, 1577, 1567, 1552, 1541, 1532, 1525, 1519, 1514, 1510, 1510, 1509, 1510, 1516, 1526, 1533, 1538, 1543, 1544, 1544, 1990, 1971, 1958, 1950, 1940, 1929, 1921, 1916, 1913, 1904, 1896, 1888, 1883, 1870, 1861, 1851, 1845, 1839, 1838, 1835, 1831, 1831, 1828, 1824, 1810, 1801, 1791, 1788, 1781, 1770, 1755, 1739, 1726, 1711, 1696, 1683, 1675, 1668, 1658, 1645, 1633, 1627, 1625, 1626, 1625, 1622, 1618, 1613, 1605, 1599, 1591, 1578, 1568, 1560, 1552, 1542, 1532, 1524, 1519, 1511, 1504, 1497, 1491, 1487, 1485, 1480, 1470, 1463, 1459, 1454, 1450, 1446, 1442, 1438, 1436, 1437, 1436, 1433, 1431, 1431, 1429, 1429, 1425, 1419, 1414, 1414, 1412, 1410, 1407, 1407, 1406, 1408, 1407, 1407, 1408, 1411, 1410, 1409, 1410, 1408, 1405, 1404, 1404, 1407, 1406, 1407, 1406, 1404, 1404, 1403, 1406, 1411, 1411, 1414, 1420, 1424, 1427, 1434, 1441, 1445, 1448, 1453, 1463, 1482, 1497, 1508, 1514, 1522, 1530, 1541, 1553, 1559, 1569, 1581, 1593, 1608, 1622, 1634, 1649, 1665, 1682, 1691, 1701, 1711, 1718, 1723, 1725, 1725, 1722, 1718, 1712, 1700, 1688, 1681, 1673, 1661, 1650, 1639, 1629, 1620, 1611, 1599, 1589, 1580, 1567, 1556, 1549, 1543, 1535, 1529, 1524, 1529, 1530, 1530, 1534, 1540, 1546, 1551, 1554, 1557, 1557, 1983, 1963, 1949, 1937, 1923, 1913, 1907, 1901, 1896, 1887, 1877, 1867, 1862, 1847, 1839, 1834, 1829, 1821, 1818, 1815, 1811, 1810, 1810, 1807, 1798, 1790, 1788, 1785, 1779, 1768, 1752, 1735, 1722, 1709, 1695, 1681, 1672, 1666, 1656, 1644, 1633, 1627, 1623, 1623, 1622, 1620, 1616, 1611, 1604, 1597, 1586, 1575, 1568, 1560, 1552, 1542, 1530, 1523, 1519, 1513, 1505, 1498, 1494, 1490, 1487, 1481, 1472, 1463, 1458, 1452, 1448, 1444, 1440, 1437, 1436, 1435, 1436, 1434, 1429, 1428, 1425, 1425, 1422, 1417, 1415, 1412, 1411, 1408, 1406, 1407, 1406, 1406, 1405, 1407, 1409, 1413, 1412, 1411, 1408, 1405, 1405, 1404, 1403, 1406, 1408, 1409, 1407, 1405, 1405, 1404, 1410, 1412, 1413, 1412, 1416, 1422, 1425, 1431, 1437, 1441, 1444, 1448, 1453, 1474, 1490, 1503, 1512, 1523, 1530, 1540, 1548, 1555, 1562, 1573, 1586, 1600, 1614, 1628, 1641, 1660, 1675, 1683, 1694, 1706, 1715, 1723, 1728, 1731, 1729, 1727, 1727, 1719, 1704, 1698, 1695, 1686, 1677, 1664, 1648, 1637, 1628, 1615, 1603, 1593, 1582, 1571, 1564, 1557, 1551, 1547, 1543, 1546, 1547, 1550, 1553, 1555, 1560, 1565, 1567, 1571, 1573, 1972, 1955, 1941, 1926, 1909, 1899, 1894, 1888, 1882, 1872, 1861, 1849, 1844, 1834, 1826, 1822, 1819, 1809, 1805, 1800, 1796, 1794, 1793, 1791, 1790, 1786, 1785, 1782, 1778, 1767, 1753, 1735, 1722, 1711, 1698, 1684, 1674, 1665, 1656, 1646, 1636, 1631, 1625, 1623, 1621, 1619, 1614, 1607, 1599, 1590, 1578, 1571, 1565, 1557, 1549, 1540, 1530, 1523, 1520, 1513, 1505, 1499, 1497, 1493, 1488, 1479, 1470, 1463, 1458, 1452, 1448, 1443, 1440, 1438, 1434, 1433, 1434, 1432, 1427, 1425, 1423, 1422, 1419, 1414, 1414, 1411, 1410, 1408, 1406, 1407, 1407, 1406, 1406, 1408, 1410, 1414, 1414, 1414, 1408, 1407, 1408, 1406, 1405, 1406, 1409, 1409, 1405, 1408, 1410, 1408, 1413, 1413, 1414, 1413, 1414, 1418, 1421, 1427, 1432, 1437, 1440, 1444, 1447, 1467, 1488, 1500, 1511, 1522, 1531, 1538, 1546, 1553, 1559, 1568, 1580, 1594, 1608, 1621, 1633, 1651, 1665, 1677, 1687, 1701, 1713, 1721, 1726, 1730, 1727, 1728, 1733, 1731, 1723, 1715, 1710, 1706, 1700, 1686, 1668, 1655, 1643, 1629, 1617, 1606, 1595, 1587, 1579, 1571, 1567, 1569, 1568, 1565, 1564, 1566, 1571, 1571, 1577, 1584, 1586, 1589, 1592, 1971, 1954, 1936, 1920, 1903, 1892, 1885, 1879, 1870, 1861, 1849, 1838, 1831, 1825, 1817, 1811, 1807, 1799, 1792, 1785, 1780, 1778, 1777, 1776, 1779, 1780, 1780, 1779, 1774, 1765, 1755, 1738, 1726, 1713, 1701, 1688, 1678, 1666, 1655, 1647, 1639, 1636, 1630, 1626, 1622, 1618, 1611, 1601, 1591, 1580, 1573, 1566, 1559, 1552, 1545, 1539, 1529, 1522, 1515, 1509, 1504, 1499, 1498, 1494, 1487, 1478, 1468, 1460, 1454, 1450, 1447, 1442, 1438, 1436, 1432, 1431, 1431, 1428, 1424, 1423, 1421, 1418, 1417, 1414, 1413, 1411, 1411, 1409, 1407, 1407, 1407, 1405, 1407, 1409, 1411, 1413, 1416, 1416, 1410, 1409, 1409, 1408, 1406, 1407, 1409, 1406, 1404, 1410, 1414, 1414, 1417, 1415, 1413, 1414, 1414, 1413, 1415, 1422, 1428, 1432, 1435, 1441, 1446, 1463, 1489, 1500, 1510, 1521, 1532, 1538, 1547, 1555, 1562, 1569, 1579, 1593, 1604, 1616, 1630, 1647, 1658, 1671, 1682, 1699, 1711, 1719, 1720, 1724, 1725, 1728, 1733, 1738, 1737, 1731, 1724, 1720, 1712, 1699, 1685, 1673, 1657, 1640, 1629, 1618, 1609, 1601, 1594, 1590, 1588, 1591, 1591, 1586, 1583, 1581, 1584, 1586, 1599, 1609, 1611, 1611, 1613, 1971, 1956, 1933, 1915, 1904, 1895, 1886, 1873, 1865, 1857, 1843, 1834, 1823, 1816, 1809, 1802, 1796, 1788, 1780, 1772, 1767, 1765, 1765, 1765, 1765, 1766, 1769, 1772, 1767, 1762, 1752, 1742, 1730, 1711, 1699, 1687, 1678, 1666, 1653, 1646, 1641, 1639, 1633, 1627, 1622, 1617, 1610, 1596, 1586, 1577, 1570, 1563, 1558, 1547, 1542, 1539, 1531, 1520, 1510, 1506, 1503, 1498, 1496, 1491, 1484, 1476, 1467, 1457, 1449, 1445, 1444, 1440, 1435, 1431, 1428, 1427, 1425, 1423, 1422, 1421, 1418, 1416, 1414, 1413, 1411, 1413, 1411, 1409, 1407, 1406, 1405, 1405, 1408, 1409, 1411, 1412, 1416, 1414, 1412, 1410, 1407, 1407, 1406, 1408, 1410, 1408, 1406, 1407, 1413, 1414, 1419, 1418, 1415, 1416, 1411, 1411, 1413, 1417, 1425, 1428, 1431, 1438, 1449, 1466, 1489, 1503, 1511, 1523, 1535, 1542, 1551, 1563, 1571, 1577, 1584, 1592, 1602, 1616, 1630, 1647, 1661, 1669, 1680, 1698, 1708, 1716, 1713, 1715, 1724, 1728, 1734, 1742, 1743, 1739, 1736, 1732, 1722, 1706, 1694, 1688, 1670, 1655, 1646, 1632, 1621, 1616, 1611, 1612, 1609, 1607, 1603, 1600, 1597, 1598, 1601, 1607, 1622, 1633, 1633, 1630, 1629, 1964, 1955, 1939, 1926, 1910, 1902, 1893, 1881, 1873, 1860, 1843, 1831, 1821, 1813, 1805, 1797, 1791, 1783, 1775, 1768, 1762, 1759, 1757, 1757, 1757, 1756, 1757, 1760, 1758, 1755, 1748, 1745, 1732, 1712, 1694, 1684, 1674, 1665, 1653, 1645, 1640, 1637, 1635, 1632, 1625, 1617, 1609, 1597, 1589, 1581, 1575, 1569, 1560, 1550, 1545, 1539, 1534, 1522, 1512, 1505, 1501, 1493, 1490, 1486, 1479, 1470, 1464, 1456, 1447, 1442, 1439, 1436, 1432, 1428, 1425, 1422, 1421, 1420, 1418, 1416, 1415, 1412, 1410, 1410, 1408, 1408, 1407, 1405, 1404, 1406, 1408, 1411, 1412, 1410, 1408, 1408, 1410, 1409, 1408, 1407, 1407, 1408, 1408, 1408, 1414, 1415, 1413, 1413, 1412, 1411, 1414, 1416, 1414, 1412, 1412, 1416, 1415, 1414, 1421, 1425, 1428, 1435, 1449, 1472, 1484, 1498, 1509, 1523, 1537, 1549, 1557, 1566, 1577, 1585, 1592, 1598, 1605, 1620, 1632, 1648, 1665, 1678, 1685, 1697, 1702, 1709, 1713, 1715, 1723, 1729, 1734, 1739, 1740, 1742, 1741, 1735, 1731, 1719, 1706, 1699, 1686, 1672, 1661, 1651, 1637, 1630, 1627, 1628, 1625, 1623, 1616, 1611, 1611, 1613, 1621, 1629, 1638, 1647, 1649, 1644, 1639, 1955, 1951, 1942, 1933, 1921, 1909, 1900, 1892, 1882, 1869, 1853, 1839, 1829, 1819, 1810, 1802, 1794, 1785, 1777, 1765, 1756, 1752, 1750, 1748, 1747, 1747, 1748, 1747, 1747, 1744, 1742, 1737, 1724, 1706, 1691, 1682, 1673, 1663, 1654, 1645, 1640, 1637, 1634, 1630, 1624, 1616, 1608, 1599, 1592, 1583, 1575, 1567, 1561, 1553, 1545, 1539, 1532, 1520, 1510, 1503, 1497, 1491, 1487, 1483, 1475, 1466, 1458, 1454, 1449, 1443, 1440, 1438, 1432, 1428, 1424, 1421, 1420, 1419, 1417, 1413, 1412, 1409, 1408, 1407, 1406, 1407, 1409, 1410, 1409, 1410, 1410, 1412, 1410, 1408, 1406, 1406, 1407, 1408, 1406, 1406, 1407, 1409, 1409, 1410, 1414, 1414, 1414, 1414, 1414, 1413, 1412, 1414, 1414, 1412, 1412, 1413, 1413, 1413, 1420, 1425, 1427, 1433, 1447, 1469, 1477, 1490, 1503, 1521, 1540, 1552, 1560, 1567, 1579, 1592, 1599, 1604, 1611, 1625, 1637, 1651, 1668, 1682, 1687, 1695, 1696, 1698, 1704, 1713, 1720, 1723, 1726, 1726, 1727, 1732, 1737, 1737, 1735, 1728, 1717, 1709, 1702, 1691, 1681, 1673, 1658, 1648, 1646, 1645, 1642, 1639, 1633, 1631, 1630, 1633, 1641, 1653, 1661, 1664, 1665, 1658, 1654, 1945, 1945, 1943, 1937, 1932, 1923, 1913, 1902, 1885, 1872, 1861, 1849, 1838, 1828, 1819, 1812, 1803, 1793, 1778, 1763, 1753, 1745, 1742, 1741, 1740, 1738, 1738, 1736, 1739, 1735, 1733, 1727, 1714, 1693, 1687, 1679, 1669, 1662, 1652, 1644, 1639, 1638, 1635, 1630, 1623, 1616, 1609, 1600, 1590, 1581, 1572, 1565, 1556, 1549, 1542, 1535, 1527, 1517, 1509, 1501, 1494, 1487, 1482, 1477, 1470, 1463, 1456, 1453, 1452, 1448, 1443, 1439, 1433, 1429, 1426, 1424, 1421, 1418, 1414, 1410, 1409, 1409, 1408, 1409, 1410, 1411, 1413, 1413, 1412, 1410, 1412, 1411, 1410, 1409, 1408, 1407, 1407, 1408, 1408, 1408, 1408, 1409, 1410, 1413, 1415, 1413, 1412, 1412, 1415, 1415, 1413, 1414, 1414, 1412, 1410, 1408, 1408, 1409, 1417, 1423, 1426, 1432, 1445, 1467, 1474, 1484, 1498, 1519, 1540, 1551, 1563, 1575, 1588, 1596, 1605, 1611, 1617, 1629, 1644, 1662, 1671, 1682, 1687, 1687, 1685, 1688, 1693, 1703, 1711, 1714, 1714, 1713, 1712, 1715, 1721, 1727, 1730, 1729, 1723, 1717, 1714, 1709, 1701, 1693, 1681, 1670, 1666, 1663, 1660, 1656, 1652, 1651, 1654, 1655, 1663, 1675, 1680, 1682, 1679, 1673, 1669, 1931, 1934, 1936, 1936, 1934, 1927, 1920, 1912, 1891, 1877, 1866, 1855, 1845, 1836, 1829, 1821, 1812, 1802, 1784, 1766, 1753, 1743, 1738, 1735, 1732, 1726, 1724, 1722, 1725, 1725, 1722, 1716, 1703, 1682, 1676, 1672, 1661, 1657, 1648, 1642, 1638, 1638, 1634, 1629, 1622, 1617, 1611, 1600, 1588, 1577, 1568, 1560, 1550, 1542, 1535, 1528, 1521, 1514, 1507, 1498, 1490, 1482, 1477, 1472, 1467, 1461, 1455, 1454, 1454, 1451, 1447, 1441, 1436, 1432, 1428, 1426, 1421, 1417, 1413, 1411, 1410, 1410, 1412, 1414, 1415, 1414, 1413, 1412, 1412, 1410, 1412, 1410, 1409, 1411, 1410, 1410, 1408, 1409, 1411, 1411, 1410, 1411, 1413, 1416, 1416, 1413, 1412, 1412, 1416, 1415, 1415, 1416, 1416, 1411, 1408, 1404, 1404, 1405, 1413, 1418, 1425, 1435, 1446, 1463, 1472, 1482, 1498, 1519, 1537, 1548, 1563, 1578, 1592, 1599, 1613, 1618, 1622, 1634, 1652, 1668, 1673, 1681, 1683, 1676, 1670, 1674, 1682, 1691, 1698, 1702, 1704, 1703, 1700, 1701, 1705, 1712, 1718, 1721, 1722, 1722, 1720, 1720, 1717, 1707, 1697, 1693, 1688, 1681, 1676, 1672, 1671, 1670, 1673, 1675, 1683, 1690, 1693, 1694, 1691, 1688, 1685, 1919, 1922, 1923, 1925, 1925, 1923, 1920, 1918, 1900, 1885, 1873, 1863, 1852, 1844, 1839, 1829, 1819, 1808, 1790, 1773, 1756, 1745, 1737, 1730, 1722, 1716, 1712, 1708, 1709, 1710, 1708, 1704, 1692, 1675, 1668, 1663, 1655, 1651, 1645, 1640, 1634, 1632, 1628, 1624, 1620, 1616, 1610, 1599, 1586, 1574, 1561, 1554, 1545, 1538, 1532, 1524, 1518, 1514, 1506, 1496, 1486, 1479, 1474, 1469, 1466, 1461, 1454, 1453, 1452, 1449, 1447, 1443, 1439, 1434, 1428, 1424, 1419, 1417, 1414, 1413, 1412, 1413, 1417, 1417, 1417, 1413, 1411, 1409, 1412, 1410, 1412, 1410, 1409, 1410, 1410, 1409, 1409, 1412, 1412, 1413, 1413, 1415, 1417, 1418, 1415, 1412, 1413, 1417, 1416, 1415, 1415, 1418, 1417, 1412, 1407, 1403, 1401, 1403, 1410, 1414, 1423, 1438, 1445, 1458, 1470, 1482, 1497, 1518, 1532, 1543, 1557, 1572, 1587, 1598, 1611, 1618, 1622, 1636, 1655, 1666, 1670, 1672, 1669, 1662, 1658, 1663, 1672, 1678, 1683, 1687, 1690, 1689, 1686, 1690, 1692, 1696, 1702, 1707, 1714, 1721, 1721, 1725, 1724, 1715, 1708, 1708, 1706, 1698, 1691, 1689, 1688, 1686, 1685, 1692, 1702, 1704, 1704, 1707, 1706, 1704, 1700, 1907, 1908, 1908, 1908, 1912, 1916, 1918, 1916, 1904, 1893, 1884, 1874, 1859, 1851, 1845, 1836, 1822, 1811, 1794, 1779, 1761, 1749, 1736, 1726, 1716, 1710, 1704, 1699, 1697, 1696, 1694, 1692, 1684, 1670, 1665, 1656, 1652, 1647, 1642, 1636, 1629, 1624, 1620, 1616, 1614, 1610, 1605, 1595, 1583, 1571, 1556, 1549, 1543, 1537, 1533, 1526, 1519, 1513, 1504, 1493, 1485, 1478, 1472, 1468, 1464, 1459, 1452, 1451, 1448, 1447, 1445, 1441, 1437, 1432, 1426, 1422, 1420, 1418, 1416, 1415, 1415, 1416, 1418, 1417, 1415, 1412, 1410, 1408, 1410, 1409, 1411, 1409, 1409, 1409, 1407, 1407, 1410, 1414, 1412, 1413, 1414, 1419, 1421, 1420, 1414, 1412, 1415, 1422, 1418, 1415, 1416, 1419, 1418, 1414, 1408, 1402, 1401, 1406, 1411, 1415, 1421, 1437, 1440, 1453, 1466, 1480, 1494, 1514, 1525, 1537, 1547, 1562, 1576, 1592, 1601, 1610, 1619, 1633, 1649, 1660, 1664, 1660, 1654, 1651, 1653, 1660, 1663, 1665, 1668, 1670, 1672, 1670, 1669, 1676, 1677, 1678, 1686, 1694, 1701, 1710, 1715, 1720, 1722, 1718, 1716, 1718, 1719, 1715, 1707, 1704, 1700, 1698, 1697, 1708, 1720, 1720, 1719, 1722, 1723, 1719, 1714, 1892, 1892, 1891, 1892, 1898, 1908, 1913, 1910, 1904, 1896, 1891, 1882, 1866, 1856, 1846, 1839, 1820, 1809, 1793, 1779, 1765, 1750, 1737, 1722, 1715, 1708, 1702, 1695, 1690, 1685, 1683, 1681, 1677, 1667, 1662, 1654, 1650, 1643, 1638, 1632, 1624, 1617, 1613, 1609, 1606, 1601, 1596, 1589, 1579, 1568, 1555, 1547, 1544, 1538, 1534, 1529, 1520, 1512, 1501, 1491, 1485, 1478, 1472, 1467, 1462, 1458, 1453, 1450, 1446, 1445, 1442, 1438, 1435, 1431, 1427, 1423, 1423, 1421, 1417, 1416, 1417, 1417, 1416, 1415, 1415, 1415, 1411, 1408, 1407, 1406, 1409, 1409, 1409, 1410, 1405, 1407, 1410, 1413, 1414, 1414, 1414, 1420, 1421, 1421, 1415, 1415, 1416, 1424, 1422, 1418, 1417, 1424, 1424, 1417, 1408, 1404, 1403, 1409, 1414, 1418, 1420, 1433, 1436, 1448, 1462, 1477, 1492, 1507, 1517, 1529, 1540, 1554, 1566, 1581, 1590, 1601, 1616, 1628, 1641, 1654, 1658, 1656, 1652, 1649, 1652, 1656, 1655, 1653, 1654, 1656, 1655, 1651, 1652, 1657, 1660, 1664, 1672, 1682, 1689, 1695, 1701, 1706, 1711, 1714, 1719, 1726, 1729, 1731, 1727, 1719, 1712, 1712, 1714, 1726, 1734, 1737, 1736, 1737, 1737, 1732, 1727, 1872, 1873, 1876, 1879, 1884, 1892, 1902, 1905, 1901, 1894, 1888, 1879, 1864, 1853, 1845, 1835, 1816, 1800, 1787, 1776, 1763, 1750, 1736, 1719, 1713, 1708, 1701, 1693, 1684, 1679, 1678, 1674, 1669, 1664, 1660, 1657, 1652, 1644, 1634, 1631, 1626, 1617, 1610, 1605, 1603, 1598, 1591, 1583, 1575, 1564, 1554, 1548, 1545, 1540, 1534, 1527, 1520, 1512, 1501, 1494, 1486, 1480, 1474, 1469, 1463, 1457, 1451, 1447, 1444, 1442, 1439, 1435, 1433, 1432, 1430, 1425, 1422, 1421, 1419, 1416, 1416, 1415, 1414, 1413, 1415, 1417, 1410, 1406, 1407, 1405, 1404, 1410, 1409, 1410, 1407, 1410, 1414, 1415, 1416, 1418, 1417, 1420, 1421, 1422, 1417, 1415, 1414, 1420, 1428, 1426, 1422, 1424, 1426, 1419, 1410, 1407, 1407, 1411, 1414, 1417, 1423, 1429, 1435, 1440, 1459, 1475, 1495, 1502, 1512, 1523, 1537, 1550, 1562, 1574, 1586, 1597, 1611, 1624, 1635, 1645, 1652, 1655, 1653, 1646, 1647, 1645, 1644, 1642, 1642, 1642, 1636, 1636, 1637, 1640, 1644, 1652, 1656, 1662, 1674, 1686, 1689, 1695, 1700, 1708, 1720, 1730, 1734, 1739, 1745, 1743, 1734, 1728, 1734, 1741, 1744, 1748, 1750, 1752, 1752, 1743, 1738, 1858, 1859, 1862, 1866, 1870, 1877, 1882, 1891, 1892, 1886, 1883, 1873, 1856, 1847, 1841, 1826, 1811, 1794, 1779, 1773, 1762, 1748, 1734, 1718, 1711, 1705, 1697, 1688, 1680, 1676, 1673, 1670, 1666, 1663, 1662, 1656, 1651, 1643, 1636, 1632, 1628, 1619, 1612, 1605, 1599, 1596, 1589, 1582, 1574, 1567, 1557, 1551, 1546, 1540, 1534, 1527, 1521, 1513, 1506, 1499, 1491, 1484, 1478, 1469, 1460, 1454, 1448, 1444, 1443, 1440, 1436, 1432, 1431, 1430, 1426, 1422, 1419, 1418, 1419, 1416, 1414, 1413, 1413, 1412, 1412, 1412, 1408, 1404, 1402, 1405, 1405, 1410, 1410, 1411, 1411, 1413, 1415, 1416, 1414, 1415, 1416, 1416, 1422, 1418, 1412, 1417, 1415, 1415, 1425, 1431, 1425, 1423, 1418, 1413, 1411, 1407, 1410, 1411, 1412, 1414, 1421, 1429, 1441, 1436, 1454, 1478, 1490, 1501, 1507, 1518, 1532, 1543, 1555, 1569, 1584, 1597, 1611, 1621, 1629, 1634, 1639, 1642, 1641, 1635, 1634, 1633, 1631, 1629, 1628, 1628, 1628, 1632, 1633, 1636, 1639, 1643, 1648, 1650, 1658, 1673, 1682, 1689, 1695, 1704, 1714, 1724, 1736, 1742, 1747, 1749, 1753, 1752, 1755, 1757, 1759, 1763, 1758, 1757, 1759, 1757, 1750, 1844, 1849, 1849, 1851, 1855, 1861, 1867, 1874, 1880, 1880, 1873, 1859, 1846, 1842, 1836, 1823, 1810, 1796, 1783, 1775, 1763, 1747, 1732, 1720, 1711, 1702, 1693, 1686, 1677, 1673, 1669, 1667, 1668, 1666, 1664, 1659, 1652, 1644, 1637, 1633, 1629, 1622, 1616, 1610, 1604, 1599, 1592, 1584, 1576, 1569, 1560, 1552, 1546, 1540, 1533, 1528, 1522, 1514, 1504, 1497, 1488, 1481, 1475, 1467, 1457, 1451, 1450, 1446, 1441, 1437, 1433, 1433, 1431, 1427, 1423, 1419, 1418, 1417, 1419, 1416, 1412, 1412, 1414, 1415, 1416, 1414, 1412, 1407, 1406, 1408, 1408, 1412, 1412, 1410, 1410, 1413, 1414, 1414, 1412, 1412, 1413, 1416, 1420, 1415, 1411, 1410, 1411, 1416, 1421, 1425, 1425, 1423, 1415, 1410, 1409, 1410, 1407, 1410, 1409, 1411, 1415, 1426, 1439, 1441, 1448, 1469, 1482, 1496, 1502, 1515, 1528, 1539, 1552, 1568, 1583, 1591, 1603, 1611, 1618, 1624, 1626, 1632, 1635, 1633, 1628, 1626, 1622, 1619, 1618, 1616, 1614, 1615, 1615, 1620, 1628, 1632, 1636, 1641, 1649, 1658, 1672, 1688, 1697, 1700, 1704, 1710, 1720, 1728, 1739, 1750, 1757, 1760, 1760, 1766, 1771, 1774, 1776, 1773, 1769, 1773, 1773, 1826, 1831, 1834, 1838, 1847, 1854, 1854, 1859, 1865, 1863, 1860, 1851, 1841, 1836, 1827, 1816, 1802, 1791, 1779, 1771, 1759, 1742, 1725, 1714, 1707, 1698, 1691, 1682, 1675, 1671, 1669, 1667, 1668, 1667, 1665, 1659, 1654, 1648, 1640, 1634, 1630, 1624, 1619, 1615, 1609, 1603, 1594, 1583, 1577, 1570, 1562, 1553, 1545, 1539, 1530, 1524, 1518, 1507, 1500, 1493, 1486, 1479, 1473, 1466, 1458, 1451, 1449, 1446, 1441, 1437, 1434, 1432, 1430, 1425, 1422, 1420, 1421, 1421, 1422, 1415, 1411, 1412, 1414, 1415, 1415, 1415, 1415, 1412, 1409, 1407, 1406, 1408, 1412, 1411, 1410, 1413, 1415, 1413, 1410, 1407, 1409, 1415, 1421, 1420, 1415, 1412, 1414, 1417, 1420, 1419, 1422, 1419, 1415, 1410, 1408, 1408, 1407, 1407, 1409, 1408, 1413, 1422, 1433, 1438, 1449, 1466, 1476, 1488, 1498, 1511, 1523, 1536, 1550, 1561, 1576, 1585, 1595, 1603, 1607, 1610, 1615, 1618, 1626, 1634, 1624, 1614, 1612, 1612, 1608, 1602, 1600, 1604, 1608, 1611, 1616, 1623, 1629, 1632, 1637, 1647, 1662, 1678, 1690, 1695, 1695, 1695, 1703, 1711, 1724, 1734, 1744, 1751, 1758, 1767, 1774, 1780, 1783, 1783, 1784, 1782, 1785, 1807, 1813, 1817, 1821, 1835, 1842, 1844, 1842, 1845, 1843, 1845, 1840, 1832, 1824, 1810, 1799, 1793, 1784, 1769, 1760, 1748, 1733, 1719, 1709, 1700, 1694, 1686, 1679, 1671, 1667, 1666, 1665, 1665, 1665, 1664, 1659, 1657, 1653, 1644, 1637, 1633, 1625, 1621, 1614, 1608, 1602, 1595, 1585, 1578, 1571, 1562, 1552, 1544, 1535, 1526, 1520, 1512, 1502, 1495, 1490, 1484, 1479, 1474, 1465, 1456, 1450, 1447, 1443, 1438, 1435, 1434, 1433, 1428, 1424, 1422, 1423, 1424, 1425, 1422, 1416, 1415, 1414, 1414, 1414, 1414, 1415, 1415, 1410, 1410, 1406, 1405, 1408, 1410, 1411, 1411, 1412, 1414, 1414, 1411, 1408, 1411, 1415, 1420, 1421, 1420, 1417, 1416, 1413, 1417, 1415, 1415, 1418, 1416, 1413, 1411, 1411, 1408, 1405, 1404, 1403, 1409, 1421, 1429, 1431, 1443, 1458, 1477, 1487, 1494, 1506, 1521, 1532, 1544, 1553, 1567, 1577, 1586, 1593, 1598, 1600, 1602, 1603, 1611, 1619, 1610, 1601, 1601, 1600, 1594, 1586, 1585, 1586, 1592, 1598, 1602, 1610, 1615, 1619, 1629, 1640, 1649, 1663, 1673, 1677, 1680, 1683, 1691, 1700, 1710, 1721, 1731, 1740, 1749, 1760, 1767, 1776, 1786, 1788, 1790, 1793, 1797, 1786, 1793, 1798, 1802, 1820, 1825, 1829, 1828, 1829, 1829, 1829, 1825, 1818, 1809, 1796, 1787, 1785, 1773, 1759, 1747, 1736, 1724, 1714, 1703, 1693, 1687, 1681, 1674, 1665, 1661, 1660, 1659, 1660, 1660, 1659, 1657, 1655, 1653, 1648, 1642, 1635, 1625, 1618, 1612, 1606, 1600, 1593, 1586, 1579, 1571, 1562, 1552, 1544, 1533, 1523, 1516, 1507, 1498, 1493, 1488, 1484, 1480, 1474, 1464, 1454, 1449, 1446, 1441, 1438, 1437, 1434, 1431, 1428, 1424, 1422, 1424, 1426, 1425, 1419, 1417, 1418, 1418, 1416, 1415, 1413, 1412, 1411, 1412, 1412, 1405, 1405, 1410, 1411, 1413, 1411, 1411, 1413, 1413, 1413, 1411, 1415, 1416, 1418, 1418, 1420, 1418, 1416, 1414, 1413, 1412, 1413, 1420, 1416, 1413, 1415, 1413, 1410, 1408, 1403, 1401, 1405, 1418, 1427, 1426, 1437, 1455, 1474, 1486, 1494, 1500, 1512, 1523, 1536, 1548, 1560, 1568, 1575, 1581, 1587, 1589, 1590, 1593, 1595, 1596, 1591, 1587, 1586, 1582, 1574, 1567, 1567, 1566, 1572, 1582, 1587, 1595, 1600, 1606, 1619, 1633, 1640, 1649, 1657, 1662, 1669, 1675, 1683, 1693, 1703, 1712, 1722, 1730, 1739, 1750, 1759, 1768, 1782, 1789, 1793, 1798, 1803, 1768, 1776, 1782, 1787, 1801, 1805, 1811, 1815, 1818, 1817, 1814, 1809, 1803, 1795, 1785, 1778, 1774, 1760, 1749, 1735, 1727, 1717, 1708, 1697, 1687, 1678, 1674, 1666, 1659, 1655, 1653, 1652, 1655, 1654, 1654, 1653, 1650, 1649, 1648, 1644, 1635, 1625, 1617, 1612, 1606, 1602, 1591, 1586, 1579, 1570, 1560, 1551, 1542, 1533, 1522, 1513, 1503, 1495, 1490, 1485, 1481, 1477, 1471, 1461, 1451, 1447, 1445, 1441, 1438, 1437, 1432, 1427, 1428, 1424, 1421, 1424, 1425, 1424, 1420, 1419, 1419, 1420, 1418, 1415, 1412, 1411, 1408, 1411, 1412, 1410, 1407, 1411, 1414, 1415, 1412, 1411, 1411, 1412, 1413, 1413, 1416, 1417, 1419, 1419, 1419, 1417, 1417, 1418, 1416, 1414, 1418, 1422, 1415, 1411, 1413, 1415, 1413, 1412, 1404, 1404, 1405, 1415, 1424, 1426, 1437, 1453, 1469, 1483, 1491, 1493, 1501, 1512, 1527, 1545, 1554, 1559, 1563, 1567, 1572, 1575, 1580, 1584, 1580, 1576, 1574, 1572, 1568, 1562, 1554, 1549, 1549, 1550, 1557, 1566, 1573, 1584, 1590, 1596, 1607, 1623, 1632, 1634, 1641, 1649, 1657, 1665, 1677, 1687, 1696, 1704, 1713, 1723, 1732, 1743, 1751, 1758, 1772, 1783, 1791, 1797, 1802, 1759, 1765, 1769, 1775, 1781, 1786, 1792, 1800, 1805, 1803, 1800, 1795, 1789, 1782, 1774, 1767, 1759, 1749, 1737, 1725, 1719, 1711, 1699, 1690, 1680, 1670, 1665, 1658, 1654, 1650, 1647, 1648, 1649, 1647, 1647, 1647, 1645, 1644, 1644, 1640, 1633, 1623, 1618, 1613, 1607, 1603, 1592, 1585, 1577, 1567, 1557, 1547, 1538, 1531, 1524, 1512, 1500, 1492, 1485, 1478, 1473, 1470, 1463, 1455, 1448, 1445, 1443, 1441, 1437, 1434, 1431, 1427, 1428, 1426, 1423, 1422, 1422, 1423, 1422, 1422, 1421, 1421, 1420, 1415, 1413, 1415, 1409, 1407, 1411, 1417, 1413, 1414, 1417, 1418, 1415, 1411, 1408, 1412, 1413, 1415, 1416, 1418, 1421, 1422, 1418, 1417, 1418, 1418, 1422, 1418, 1424, 1423, 1416, 1411, 1411, 1415, 1415, 1413, 1405, 1406, 1407, 1417, 1421, 1428, 1439, 1448, 1463, 1477, 1484, 1487, 1492, 1503, 1519, 1539, 1549, 1550, 1553, 1556, 1558, 1562, 1571, 1574, 1569, 1564, 1560, 1557, 1552, 1545, 1540, 1536, 1536, 1539, 1546, 1554, 1562, 1575, 1583, 1587, 1594, 1608, 1617, 1620, 1627, 1635, 1642, 1652, 1670, 1675, 1681, 1692, 1703, 1716, 1729, 1741, 1747, 1750, 1758, 1769, 1782, 1793, 1800, 1757, 1756, 1758, 1761, 1765, 1769, 1771, 1779, 1786, 1785, 1786, 1783, 1776, 1769, 1761, 1754, 1749, 1737, 1724, 1717, 1711, 1705, 1693, 1683, 1674, 1664, 1658, 1651, 1651, 1645, 1642, 1646, 1641, 1639, 1639, 1642, 1640, 1640, 1639, 1636, 1629, 1618, 1612, 1609, 1606, 1598, 1592, 1584, 1574, 1565, 1552, 1542, 1534, 1529, 1524, 1513, 1497, 1488, 1480, 1470, 1465, 1462, 1457, 1451, 1444, 1441, 1441, 1439, 1436, 1435, 1434, 1430, 1428, 1428, 1425, 1421, 1420, 1422, 1423, 1423, 1423, 1424, 1421, 1417, 1420, 1415, 1413, 1411, 1412, 1416, 1417, 1419, 1420, 1420, 1420, 1412, 1407, 1409, 1415, 1418, 1420, 1421, 1425, 1421, 1422, 1419, 1420, 1419, 1422, 1420, 1426, 1422, 1417, 1412, 1410, 1411, 1411, 1412, 1408, 1408, 1411, 1418, 1424, 1429, 1437, 1447, 1458, 1470, 1479, 1482, 1485, 1495, 1512, 1531, 1541, 1540, 1545, 1549, 1550, 1553, 1560, 1563, 1559, 1553, 1546, 1542, 1537, 1535, 1533, 1527, 1527, 1530, 1537, 1548, 1554, 1562, 1571, 1577, 1583, 1593, 1604, 1613, 1620, 1625, 1633, 1643, 1658, 1661, 1663, 1677, 1695, 1707, 1724, 1740, 1749, 1747, 1747, 1755, 1772, 1785, 1794, 1754, 1751, 1749, 1748, 1746, 1747, 1751, 1754, 1757, 1762, 1771, 1766, 1757, 1750, 1745, 1746, 1742, 1733, 1721, 1712, 1705, 1698, 1685, 1675, 1667, 1659, 1654, 1648, 1643, 1640, 1636, 1634, 1631, 1632, 1636, 1639, 1635, 1634, 1634, 1631, 1627, 1615, 1608, 1605, 1601, 1595, 1591, 1583, 1571, 1559, 1543, 1536, 1534, 1529, 1522, 1510, 1496, 1485, 1474, 1465, 1462, 1459, 1454, 1448, 1442, 1440, 1439, 1436, 1434, 1435, 1432, 1426, 1423, 1419, 1417, 1419, 1420, 1421, 1422, 1424, 1425, 1422, 1421, 1422, 1421, 1419, 1413, 1411, 1411, 1410, 1417, 1415, 1416, 1416, 1422, 1415, 1413, 1409, 1417, 1426, 1424, 1423, 1424, 1421, 1420, 1421, 1422, 1420, 1419, 1424, 1424, 1421, 1416, 1414, 1410, 1411, 1409, 1412, 1416, 1420, 1416, 1420, 1426, 1431, 1437, 1443, 1455, 1465, 1471, 1474, 1477, 1487, 1502, 1518, 1529, 1534, 1539, 1541, 1542, 1542, 1545, 1547, 1541, 1536, 1530, 1528, 1529, 1529, 1528, 1524, 1519, 1522, 1528, 1539, 1545, 1548, 1554, 1565, 1577, 1587, 1598, 1608, 1611, 1616, 1625, 1633, 1642, 1650, 1655, 1664, 1682, 1694, 1711, 1732, 1748, 1751, 1746, 1752, 1765, 1777, 1783, 1746, 1743, 1741, 1740, 1738, 1736, 1735, 1733, 1737, 1741, 1752, 1749, 1741, 1735, 1734, 1738, 1734, 1728, 1717, 1707, 1698, 1690, 1680, 1674, 1666, 1658, 1654, 1645, 1640, 1632, 1624, 1626, 1629, 1632, 1632, 1633, 1625, 1626, 1630, 1629, 1620, 1614, 1609, 1608, 1602, 1594, 1588, 1578, 1569, 1558, 1543, 1538, 1534, 1526, 1516, 1507, 1497, 1482, 1472, 1464, 1458, 1455, 1453, 1447, 1444, 1442, 1438, 1434, 1434, 1433, 1426, 1421, 1422, 1422, 1421, 1422, 1422, 1423, 1423, 1422, 1421, 1421, 1418, 1417, 1420, 1422, 1417, 1416, 1416, 1414, 1409, 1413, 1414, 1415, 1416, 1416, 1415, 1414, 1420, 1430, 1431, 1424, 1425, 1424, 1422, 1423, 1423, 1422, 1425, 1424, 1423, 1419, 1415, 1408, 1408, 1410, 1410, 1411, 1413, 1420, 1417, 1417, 1423, 1427, 1430, 1437, 1448, 1459, 1463, 1470, 1473, 1482, 1495, 1509, 1518, 1525, 1529, 1529, 1528, 1529, 1529, 1527, 1521, 1520, 1519, 1520, 1525, 1521, 1516, 1518, 1521, 1521, 1522, 1525, 1536, 1541, 1545, 1555, 1567, 1578, 1587, 1592, 1601, 1609, 1616, 1622, 1632, 1641, 1648, 1663, 1677, 1692, 1710, 1723, 1738, 1745, 1747, 1750, 1764, 1770, 1774, 1738, 1734, 1731, 1728, 1722, 1719, 1721, 1720, 1723, 1728, 1734, 1733, 1729, 1727, 1727, 1730, 1727, 1721, 1713, 1703, 1693, 1683, 1674, 1669, 1662, 1659, 1653, 1647, 1637, 1624, 1621, 1621, 1622, 1624, 1621, 1619, 1618, 1618, 1619, 1625, 1618, 1612, 1609, 1607, 1598, 1590, 1583, 1576, 1568, 1556, 1541, 1536, 1527, 1519, 1510, 1503, 1492, 1479, 1469, 1464, 1456, 1451, 1446, 1442, 1440, 1437, 1435, 1434, 1433, 1430, 1424, 1423, 1426, 1425, 1423, 1421, 1420, 1420, 1420, 1420, 1421, 1422, 1420, 1417, 1419, 1425, 1426, 1419, 1418, 1416, 1411, 1410, 1416, 1418, 1423, 1419, 1414, 1416, 1422, 1426, 1428, 1423, 1426, 1426, 1424, 1423, 1429, 1427, 1426, 1423, 1422, 1418, 1415, 1411, 1407, 1406, 1408, 1411, 1412, 1420, 1419, 1419, 1421, 1425, 1428, 1438, 1446, 1452, 1457, 1470, 1474, 1482, 1490, 1500, 1507, 1508, 1514, 1517, 1515, 1512, 1513, 1515, 1513, 1512, 1510, 1508, 1509, 1509, 1507, 1508, 1515, 1513, 1513, 1517, 1523, 1530, 1537, 1546, 1556, 1565, 1570, 1577, 1585, 1596, 1603, 1610, 1625, 1637, 1646, 1662, 1677, 1688, 1703, 1717, 1729, 1738, 1740, 1746, 1755, 1761, 1766, 1731, 1728, 1723, 1718, 1713, 1710, 1710, 1709, 1710, 1716, 1718, 1722, 1719, 1716, 1721, 1723, 1723, 1717, 1709, 1700, 1691, 1681, 1673, 1667, 1660, 1658, 1654, 1645, 1637, 1625, 1620, 1616, 1614, 1616, 1614, 1609, 1612, 1611, 1611, 1618, 1613, 1609, 1605, 1600, 1593, 1588, 1583, 1575, 1565, 1554, 1543, 1534, 1524, 1515, 1506, 1496, 1486, 1475, 1467, 1460, 1452, 1447, 1441, 1435, 1433, 1431, 1432, 1432, 1430, 1430, 1428, 1426, 1426, 1427, 1423, 1421, 1420, 1420, 1422, 1422, 1422, 1423, 1421, 1425, 1427, 1429, 1429, 1424, 1421, 1419, 1416, 1412, 1416, 1416, 1421, 1421, 1419, 1420, 1422, 1427, 1427, 1422, 1426, 1426, 1424, 1422, 1425, 1425, 1424, 1422, 1421, 1417, 1415, 1411, 1406, 1405, 1410, 1411, 1411, 1417, 1418, 1420, 1423, 1423, 1430, 1439, 1442, 1449, 1457, 1467, 1471, 1479, 1486, 1493, 1499, 1497, 1501, 1505, 1504, 1498, 1502, 1506, 1504, 1503, 1501, 1499, 1496, 1497, 1498, 1500, 1505, 1507, 1506, 1505, 1511, 1520, 1529, 1536, 1545, 1558, 1564, 1567, 1572, 1584, 1593, 1601, 1616, 1632, 1647, 1663, 1675, 1689, 1701, 1713, 1724, 1731, 1736, 1744, 1750, 1753, 1757, 1726, 1722, 1717, 1711, 1707, 1705, 1704, 1702, 1702, 1704, 1706, 1709, 1708, 1706, 1709, 1711, 1711, 1706, 1699, 1694, 1688, 1678, 1669, 1660, 1652, 1650, 1646, 1640, 1634, 1628, 1622, 1616, 1612, 1612, 1611, 1605, 1605, 1604, 1605, 1609, 1607, 1605, 1600, 1596, 1592, 1588, 1585, 1576, 1563, 1552, 1541, 1530, 1520, 1511, 1502, 1489, 1481, 1472, 1465, 1456, 1450, 1445, 1440, 1434, 1434, 1433, 1432, 1431, 1429, 1430, 1429, 1428, 1428, 1428, 1425, 1425, 1424, 1423, 1425, 1423, 1423, 1425, 1424, 1429, 1432, 1432, 1432, 1428, 1426, 1424, 1423, 1418, 1417, 1417, 1419, 1419, 1421, 1423, 1425, 1427, 1428, 1424, 1427, 1421, 1424, 1423, 1423, 1422, 1421, 1420, 1418, 1417, 1416, 1412, 1407, 1404, 1408, 1411, 1412, 1414, 1418, 1421, 1425, 1426, 1433, 1438, 1442, 1449, 1458, 1464, 1467, 1472, 1476, 1483, 1488, 1490, 1491, 1493, 1493, 1489, 1491, 1495, 1492, 1490, 1488, 1485, 1480, 1481, 1486, 1492, 1493, 1497, 1497, 1495, 1498, 1505, 1516, 1526, 1540, 1557, 1563, 1562, 1566, 1579, 1591, 1600, 1612, 1633, 1649, 1658, 1672, 1688, 1698, 1709, 1719, 1726, 1736, 1741, 1746, 1749, 1747, 1718, 1716, 1712, 1706, 1702, 1701, 1701, 1700, 1699, 1698, 1699, 1700, 1698, 1695, 1693, 1692, 1692, 1690, 1687, 1687, 1682, 1672, 1661, 1650, 1641, 1638, 1636, 1634, 1630, 1628, 1624, 1619, 1616, 1612, 1609, 1604, 1601, 1600, 1602, 1603, 1601, 1600, 1595, 1594, 1592, 1590, 1586, 1575, 1562, 1549, 1536, 1526, 1517, 1510, 1501, 1488, 1480, 1470, 1462, 1454, 1449, 1445, 1440, 1438, 1436, 1434, 1432, 1431, 1429, 1430, 1429, 1429, 1429, 1428, 1428, 1429, 1428, 1428, 1427, 1425, 1425, 1427, 1427, 1428, 1434, 1436, 1433, 1429, 1427, 1428, 1427, 1424, 1420, 1419, 1420, 1420, 1422, 1423, 1426, 1425, 1427, 1426, 1427, 1419, 1420, 1423, 1422, 1420, 1418, 1417, 1416, 1417, 1417, 1413, 1409, 1405, 1407, 1411, 1415, 1415, 1420, 1424, 1428, 1431, 1436, 1441, 1445, 1450, 1460, 1463, 1464, 1466, 1467, 1471, 1474, 1475, 1475, 1476, 1476, 1476, 1476, 1479, 1477, 1476, 1474, 1469, 1463, 1465, 1471, 1478, 1481, 1487, 1488, 1489, 1491, 1494, 1504, 1517, 1536, 1554, 1561, 1561, 1564, 1577, 1588, 1598, 1612, 1634, 1647, 1653, 1664, 1679, 1695, 1705, 1713, 1720, 1732, 1737, 1741, 1743, 1739, 1712, 1711, 1708, 1703, 1701, 1701, 1701, 1703, 1700, 1698, 1695, 1694, 1689, 1683, 1680, 1676, 1674, 1676, 1676, 1678, 1675, 1665, 1651, 1641, 1633, 1630, 1629, 1629, 1627, 1626, 1624, 1622, 1621, 1615, 1609, 1605, 1602, 1601, 1603, 1602, 1598, 1594, 1591, 1591, 1590, 1590, 1585, 1573, 1560, 1548, 1533, 1523, 1518, 1513, 1506, 1495, 1483, 1471, 1463, 1455, 1449, 1444, 1440, 1439, 1436, 1432, 1431, 1431, 1429, 1430, 1431, 1429, 1429, 1429, 1430, 1431, 1431, 1430, 1430, 1427, 1428, 1430, 1429, 1426, 1432, 1436, 1433, 1429, 1426, 1427, 1427, 1426, 1424, 1421, 1420, 1422, 1424, 1423, 1422, 1421, 1423, 1423, 1425, 1421, 1417, 1420, 1420, 1418, 1415, 1415, 1415, 1418, 1421, 1416, 1412, 1408, 1410, 1411, 1417, 1417, 1422, 1429, 1434, 1435, 1439, 1447, 1449, 1454, 1462, 1465, 1466, 1464, 1462, 1462, 1459, 1457, 1457, 1457, 1457, 1458, 1458, 1459, 1459, 1461, 1460, 1454, 1449, 1451, 1456, 1464, 1470, 1476, 1479, 1483, 1487, 1491, 1498, 1508, 1526, 1543, 1556, 1561, 1564, 1573, 1580, 1590, 1606, 1625, 1639, 1646, 1653, 1665, 1685, 1699, 1707, 1713, 1724, 1730, 1733, 1735, 1733, 1712, 1709, 1706, 1703, 1702, 1703, 1705, 1706, 1702, 1699, 1692, 1687, 1681, 1675, 1671, 1667, 1664, 1664, 1665, 1666, 1663, 1654, 1641, 1634, 1629, 1627, 1625, 1624, 1624, 1623, 1620, 1620, 1620, 1613, 1610, 1608, 1606, 1606, 1603, 1600, 1598, 1590, 1587, 1587, 1586, 1585, 1582, 1572, 1560, 1547, 1533, 1524, 1519, 1515, 1511, 1503, 1488, 1476, 1467, 1459, 1451, 1445, 1441, 1437, 1435, 1433, 1432, 1432, 1431, 1434, 1433, 1431, 1432, 1431, 1431, 1431, 1430, 1430, 1430, 1430, 1430, 1431, 1428, 1427, 1431, 1432, 1433, 1429, 1425, 1425, 1425, 1424, 1426, 1424, 1417, 1419, 1423, 1423, 1419, 1417, 1419, 1419, 1420, 1420, 1420, 1418, 1419, 1419, 1417, 1418, 1419, 1424, 1424, 1418, 1415, 1412, 1411, 1411, 1416, 1418, 1425, 1432, 1438, 1440, 1442, 1451, 1455, 1459, 1463, 1466, 1467, 1464, 1459, 1455, 1449, 1445, 1442, 1443, 1445, 1443, 1441, 1443, 1444, 1447, 1446, 1441, 1439, 1442, 1448, 1456, 1461, 1463, 1469, 1476, 1481, 1486, 1493, 1499, 1513, 1530, 1547, 1556, 1562, 1568, 1574, 1582, 1592, 1610, 1628, 1636, 1643, 1655, 1668, 1689, 1699, 1704, 1714, 1719, 1723, 1726, 1724, 1710, 1709, 1707, 1704, 1702, 1704, 1707, 1706, 1703, 1697, 1690, 1681, 1674, 1668, 1663, 1662, 1658, 1656, 1655, 1653, 1647, 1638, 1635, 1628, 1626, 1626, 1625, 1623, 1625, 1624, 1618, 1615, 1612, 1610, 1607, 1605, 1604, 1603, 1600, 1597, 1594, 1588, 1582, 1579, 1580, 1581, 1579, 1570, 1559, 1545, 1531, 1523, 1520, 1515, 1510, 1501, 1491, 1480, 1471, 1462, 1452, 1444, 1439, 1435, 1433, 1433, 1432, 1434, 1435, 1437, 1441, 1439, 1435, 1434, 1433, 1432, 1432, 1432, 1434, 1435, 1433, 1435, 1431, 1435, 1436, 1435, 1428, 1427, 1426, 1431, 1428, 1428, 1427, 1426, 1419, 1421, 1420, 1418, 1418, 1418, 1415, 1417, 1416, 1418, 1422, 1421, 1421, 1422, 1423, 1423, 1424, 1428, 1425, 1417, 1415, 1419, 1413, 1413, 1416, 1422, 1433, 1436, 1436, 1442, 1446, 1455, 1459, 1462, 1463, 1466, 1465, 1460, 1455, 1447, 1441, 1437, 1434, 1434, 1437, 1437, 1436, 1435, 1433, 1430, 1427, 1430, 1431, 1436, 1442, 1451, 1456, 1459, 1464, 1468, 1475, 1480, 1483, 1490, 1508, 1526, 1536, 1544, 1559, 1569, 1574, 1577, 1583, 1599, 1616, 1627, 1637, 1651, 1662, 1676, 1680, 1691, 1701, 1708, 1712, 1714, 1711, 1709, 1707, 1704, 1700, 1696, 1700, 1705, 1706, 1703, 1693, 1683, 1673, 1667, 1662, 1660, 1658, 1654, 1648, 1646, 1642, 1637, 1633, 1626, 1621, 1621, 1624, 1624, 1623, 1626, 1625, 1618, 1611, 1611, 1610, 1604, 1601, 1598, 1593, 1594, 1590, 1588, 1584, 1579, 1578, 1579, 1577, 1573, 1564, 1550, 1541, 1530, 1522, 1519, 1518, 1510, 1500, 1486, 1474, 1467, 1458, 1450, 1445, 1439, 1437, 1434, 1429, 1430, 1432, 1435, 1440, 1444, 1441, 1436, 1435, 1435, 1433, 1436, 1436, 1437, 1436, 1430, 1431, 1435, 1440, 1440, 1433, 1427, 1427, 1428, 1430, 1429, 1428, 1426, 1424, 1424, 1422, 1417, 1419, 1420, 1421, 1416, 1413, 1415, 1419, 1421, 1421, 1425, 1423, 1428, 1421, 1417, 1425, 1424, 1418, 1417, 1423, 1418, 1417, 1422, 1427, 1434, 1440, 1440, 1450, 1456, 1455, 1460, 1462, 1464, 1467, 1467, 1462, 1457, 1451, 1444, 1439, 1433, 1429, 1428, 1430, 1429, 1424, 1422, 1419, 1416, 1419, 1427, 1433, 1437, 1447, 1454, 1458, 1462, 1463, 1467, 1472, 1477, 1485, 1504, 1522, 1528, 1535, 1553, 1569, 1575, 1577, 1578, 1590, 1608, 1621, 1629, 1638, 1649, 1658, 1667, 1681, 1691, 1698, 1703, 1703, 1700, 1716, 1708, 1704, 1698, 1697, 1698, 1703, 1704, 1697, 1690, 1679, 1672, 1662, 1657, 1655, 1650, 1644, 1642, 1637, 1636, 1631, 1622, 1618, 1618, 1619, 1621, 1623, 1622, 1623, 1621, 1616, 1614, 1611, 1606, 1601, 1598, 1598, 1595, 1592, 1587, 1584, 1580, 1575, 1574, 1574, 1574, 1571, 1563, 1552, 1542, 1532, 1523, 1517, 1514, 1509, 1500, 1489, 1476, 1464, 1454, 1450, 1450, 1444, 1441, 1438, 1433, 1432, 1433, 1438, 1442, 1445, 1446, 1441, 1439, 1439, 1438, 1439, 1440, 1443, 1444, 1434, 1434, 1439, 1441, 1438, 1428, 1424, 1426, 1427, 1429, 1432, 1430, 1427, 1425, 1427, 1426, 1420, 1420, 1419, 1418, 1417, 1416, 1410, 1416, 1418, 1418, 1426, 1425, 1422, 1418, 1421, 1421, 1427, 1426, 1419, 1421, 1429, 1428, 1428, 1434, 1439, 1446, 1449, 1456, 1455, 1463, 1465, 1466, 1466, 1466, 1464, 1460, 1456, 1452, 1446, 1443, 1437, 1433, 1430, 1426, 1421, 1417, 1414, 1411, 1411, 1412, 1418, 1423, 1427, 1435, 1448, 1459, 1463, 1465, 1466, 1472, 1473, 1478, 1496, 1511, 1519, 1531, 1549, 1565, 1573, 1581, 1581, 1589, 1602, 1612, 1625, 1634, 1644, 1654, 1663, 1671, 1680, 1686, 1690, 1691, 1687, 1725, 1712, 1703, 1699, 1696, 1695, 1699, 1699, 1693, 1686, 1678, 1670, 1662, 1654, 1648, 1642, 1638, 1634, 1632, 1629, 1627, 1620, 1615, 1617, 1619, 1623, 1625, 1627, 1628, 1628, 1621, 1616, 1609, 1604, 1603, 1600, 1597, 1593, 1590, 1587, 1582, 1577, 1573, 1572, 1574, 1570, 1565, 1557, 1550, 1543, 1536, 1531, 1525, 1519, 1511, 1498, 1484, 1474, 1466, 1458, 1452, 1449, 1447, 1449, 1444, 1434, 1431, 1433, 1440, 1448, 1450, 1445, 1444, 1444, 1441, 1440, 1440, 1442, 1444, 1444, 1447, 1439, 1436, 1439, 1440, 1430, 1430, 1429, 1429, 1434, 1434, 1431, 1429, 1427, 1427, 1425, 1423, 1422, 1421, 1420, 1419, 1417, 1415, 1417, 1419, 1418, 1421, 1423, 1421, 1420, 1420, 1424, 1432, 1432, 1424, 1425, 1429, 1428, 1431, 1440, 1445, 1453, 1460, 1469, 1473, 1471, 1470, 1472, 1471, 1471, 1469, 1466, 1463, 1460, 1451, 1448, 1443, 1435, 1430, 1425, 1417, 1414, 1411, 1408, 1408, 1412, 1415, 1418, 1423, 1430, 1441, 1448, 1448, 1451, 1455, 1466, 1472, 1472, 1485, 1498, 1508, 1521, 1539, 1553, 1565, 1580, 1583, 1587, 1594, 1603, 1612, 1625, 1638, 1648, 1658, 1665, 1672, 1676, 1677, 1679, 1677, 1722, 1714, 1702, 1696, 1693, 1689, 1688, 1686, 1684, 1679, 1673, 1666, 1655, 1647, 1642, 1636, 1632, 1629, 1625, 1622, 1620, 1617, 1614, 1616, 1622, 1624, 1625, 1624, 1626, 1630, 1628, 1622, 1614, 1606, 1604, 1603, 1600, 1596, 1593, 1590, 1582, 1576, 1571, 1569, 1567, 1564, 1557, 1551, 1548, 1544, 1540, 1537, 1531, 1523, 1512, 1497, 1482, 1473, 1467, 1460, 1453, 1450, 1450, 1452, 1451, 1444, 1437, 1439, 1447, 1453, 1451, 1443, 1445, 1451, 1447, 1443, 1442, 1442, 1442, 1447, 1449, 1446, 1443, 1438, 1437, 1432, 1433, 1436, 1436, 1436, 1434, 1431, 1430, 1429, 1428, 1426, 1428, 1426, 1423, 1422, 1422, 1419, 1420, 1420, 1421, 1423, 1423, 1425, 1426, 1423, 1419, 1426, 1433, 1430, 1426, 1425, 1427, 1430, 1437, 1445, 1452, 1458, 1468, 1482, 1482, 1479, 1478, 1477, 1476, 1474, 1473, 1473, 1474, 1472, 1465, 1454, 1449, 1442, 1434, 1427, 1417, 1411, 1407, 1406, 1406, 1409, 1412, 1413, 1417, 1424, 1432, 1438, 1440, 1442, 1448, 1459, 1467, 1471, 1477, 1484, 1495, 1510, 1529, 1545, 1558, 1571, 1579, 1587, 1593, 1601, 1609, 1618, 1630, 1640, 1649, 1655, 1661, 1666, 1666, 1672, 1672, 1721, 1714, 1701, 1693, 1689, 1684, 1679, 1675, 1674, 1670, 1665, 1658, 1650, 1642, 1639, 1634, 1630, 1626, 1620, 1617, 1616, 1613, 1611, 1615, 1618, 1620, 1618, 1617, 1620, 1624, 1625, 1621, 1617, 1611, 1607, 1607, 1602, 1599, 1596, 1593, 1585, 1575, 1571, 1570, 1565, 1559, 1552, 1546, 1545, 1542, 1539, 1537, 1532, 1524, 1513, 1498, 1483, 1475, 1469, 1463, 1457, 1454, 1450, 1450, 1451, 1449, 1446, 1448, 1449, 1450, 1449, 1446, 1448, 1451, 1451, 1449, 1446, 1442, 1443, 1447, 1447, 1445, 1448, 1443, 1441, 1440, 1438, 1440, 1438, 1436, 1434, 1432, 1432, 1431, 1430, 1430, 1432, 1431, 1428, 1426, 1425, 1423, 1423, 1422, 1421, 1424, 1425, 1427, 1425, 1423, 1417, 1424, 1428, 1426, 1426, 1426, 1429, 1436, 1443, 1450, 1455, 1459, 1472, 1485, 1486, 1487, 1484, 1482, 1482, 1478, 1479, 1483, 1485, 1483, 1477, 1462, 1456, 1450, 1442, 1431, 1421, 1411, 1404, 1404, 1404, 1405, 1408, 1410, 1412, 1418, 1420, 1427, 1434, 1437, 1442, 1451, 1458, 1466, 1471, 1474, 1486, 1503, 1519, 1535, 1550, 1559, 1567, 1580, 1593, 1601, 1609, 1615, 1623, 1632, 1639, 1644, 1647, 1652, 1655, 1661, 1661, 1723, 1714, 1704, 1695, 1688, 1682, 1675, 1669, 1666, 1662, 1658, 1651, 1645, 1639, 1636, 1633, 1630, 1625, 1619, 1615, 1613, 1608, 1605, 1610, 1611, 1611, 1608, 1611, 1614, 1615, 1617, 1617, 1616, 1614, 1612, 1610, 1602, 1598, 1595, 1593, 1588, 1579, 1575, 1573, 1566, 1559, 1552, 1546, 1543, 1540, 1534, 1531, 1527, 1520, 1511, 1500, 1488, 1478, 1470, 1467, 1463, 1457, 1451, 1451, 1451, 1449, 1449, 1451, 1448, 1447, 1449, 1451, 1450, 1450, 1451, 1452, 1448, 1444, 1444, 1445, 1444, 1442, 1443, 1446, 1446, 1444, 1440, 1440, 1439, 1436, 1435, 1435, 1435, 1433, 1432, 1434, 1436, 1435, 1434, 1431, 1428, 1426, 1425, 1423, 1422, 1424, 1425, 1426, 1423, 1421, 1419, 1423, 1423, 1424, 1426, 1430, 1438, 1445, 1446, 1452, 1455, 1458, 1470, 1481, 1485, 1491, 1489, 1489, 1488, 1486, 1489, 1493, 1493, 1493, 1487, 1473, 1465, 1458, 1448, 1437, 1427, 1416, 1407, 1400, 1401, 1403, 1406, 1409, 1410, 1413, 1413, 1417, 1424, 1430, 1434, 1440, 1448, 1457, 1465, 1469, 1480, 1497, 1510, 1526, 1540, 1547, 1557, 1572, 1590, 1601, 1609, 1613, 1616, 1624, 1631, 1634, 1638, 1641, 1645, 1648, 1648, 1726, 1717, 1709, 1700, 1690, 1684, 1676, 1669, 1663, 1659, 1653, 1648, 1641, 1636, 1633, 1632, 1630, 1627, 1621, 1614, 1609, 1602, 1599, 1601, 1604, 1602, 1603, 1606, 1609, 1610, 1611, 1612, 1613, 1611, 1612, 1610, 1602, 1597, 1594, 1592, 1589, 1585, 1582, 1576, 1568, 1561, 1553, 1548, 1544, 1538, 1530, 1525, 1520, 1514, 1507, 1501, 1492, 1481, 1472, 1469, 1466, 1459, 1454, 1454, 1454, 1449, 1447, 1449, 1449, 1449, 1452, 1453, 1452, 1452, 1450, 1450, 1447, 1444, 1444, 1442, 1441, 1439, 1438, 1444, 1443, 1440, 1437, 1437, 1439, 1436, 1436, 1436, 1436, 1436, 1434, 1437, 1439, 1438, 1437, 1434, 1430, 1427, 1426, 1424, 1424, 1425, 1424, 1426, 1424, 1421, 1422, 1423, 1422, 1422, 1424, 1434, 1446, 1452, 1449, 1452, 1455, 1461, 1466, 1476, 1482, 1491, 1495, 1496, 1493, 1494, 1499, 1502, 1502, 1502, 1497, 1484, 1471, 1462, 1450, 1440, 1432, 1423, 1411, 1398, 1396, 1400, 1403, 1406, 1405, 1406, 1408, 1409, 1411, 1418, 1424, 1430, 1440, 1451, 1461, 1468, 1477, 1491, 1503, 1519, 1530, 1539, 1553, 1569, 1585, 1599, 1606, 1607, 1608, 1616, 1624, 1628, 1632, 1637, 1638, 1639, 1639, 1731, 1723, 1715, 1706, 1698, 1691, 1683, 1675, 1668, 1659, 1653, 1647, 1641, 1635, 1634, 1632, 1630, 1627, 1622, 1611, 1605, 1599, 1595, 1596, 1597, 1598, 1601, 1602, 1604, 1606, 1608, 1609, 1607, 1605, 1605, 1603, 1603, 1599, 1596, 1592, 1593, 1588, 1585, 1577, 1569, 1560, 1553, 1548, 1546, 1538, 1530, 1524, 1518, 1510, 1504, 1498, 1491, 1484, 1478, 1473, 1467, 1460, 1455, 1456, 1455, 1451, 1449, 1450, 1452, 1452, 1454, 1455, 1456, 1455, 1451, 1448, 1443, 1439, 1439, 1437, 1437, 1438, 1441, 1446, 1442, 1439, 1435, 1436, 1437, 1436, 1436, 1435, 1435, 1436, 1436, 1439, 1437, 1437, 1435, 1432, 1431, 1428, 1428, 1426, 1425, 1426, 1425, 1429, 1425, 1424, 1423, 1424, 1422, 1419, 1421, 1438, 1447, 1454, 1454, 1453, 1457, 1464, 1468, 1476, 1485, 1494, 1499, 1499, 1500, 1501, 1506, 1512, 1514, 1512, 1507, 1493, 1478, 1464, 1450, 1436, 1430, 1422, 1410, 1400, 1395, 1396, 1398, 1398, 1397, 1397, 1398, 1399, 1404, 1408, 1413, 1421, 1433, 1448, 1462, 1471, 1479, 1489, 1495, 1511, 1523, 1535, 1553, 1570, 1583, 1595, 1599, 1599, 1602, 1610, 1618, 1622, 1623, 1629, 1633, 1634, 1633, 1733, 1727, 1721, 1718, 1707, 1699, 1690, 1681, 1674, 1667, 1658, 1650, 1641, 1634, 1632, 1629, 1628, 1624, 1615, 1609, 1603, 1602, 1597, 1598, 1599, 1593, 1596, 1591, 1595, 1600, 1602, 1605, 1601, 1600, 1601, 1601, 1601, 1599, 1597, 1596, 1598, 1594, 1587, 1578, 1567, 1558, 1551, 1546, 1540, 1535, 1532, 1525, 1517, 1511, 1505, 1501, 1497, 1490, 1483, 1475, 1466, 1461, 1458, 1454, 1453, 1451, 1451, 1452, 1451, 1451, 1454, 1457, 1457, 1454, 1451, 1449, 1443, 1436, 1435, 1435, 1439, 1440, 1444, 1448, 1440, 1435, 1433, 1435, 1438, 1438, 1438, 1437, 1435, 1435, 1439, 1441, 1433, 1432, 1432, 1430, 1429, 1430, 1430, 1431, 1428, 1428, 1423, 1428, 1424, 1425, 1424, 1426, 1422, 1422, 1428, 1439, 1445, 1451, 1450, 1451, 1458, 1460, 1469, 1477, 1483, 1492, 1495, 1497, 1502, 1506, 1508, 1512, 1517, 1518, 1515, 1506, 1489, 1471, 1452, 1437, 1430, 1424, 1412, 1401, 1393, 1391, 1391, 1390, 1389, 1389, 1391, 1394, 1397, 1399, 1406, 1415, 1426, 1438, 1458, 1473, 1478, 1485, 1492, 1500, 1514, 1533, 1549, 1561, 1575, 1587, 1593, 1597, 1603, 1610, 1610, 1614, 1616, 1621, 1625, 1626, 1625, 1738, 1734, 1732, 1725, 1717, 1708, 1697, 1689, 1682, 1675, 1666, 1658, 1645, 1639, 1634, 1629, 1627, 1622, 1613, 1607, 1601, 1600, 1596, 1593, 1593, 1589, 1589, 1584, 1587, 1593, 1599, 1600, 1597, 1599, 1597, 1596, 1596, 1599, 1600, 1601, 1601, 1595, 1586, 1573, 1560, 1551, 1547, 1541, 1535, 1532, 1529, 1525, 1522, 1518, 1512, 1505, 1497, 1490, 1484, 1478, 1469, 1465, 1466, 1460, 1455, 1455, 1456, 1456, 1456, 1455, 1455, 1457, 1455, 1452, 1451, 1447, 1441, 1436, 1435, 1435, 1435, 1441, 1450, 1450, 1442, 1437, 1436, 1437, 1439, 1440, 1437, 1439, 1440, 1440, 1440, 1437, 1436, 1437, 1437, 1434, 1430, 1432, 1433, 1435, 1430, 1428, 1427, 1426, 1428, 1423, 1422, 1425, 1426, 1426, 1435, 1439, 1441, 1443, 1449, 1449, 1455, 1460, 1466, 1471, 1479, 1487, 1493, 1498, 1501, 1508, 1512, 1513, 1519, 1518, 1514, 1509, 1496, 1474, 1454, 1443, 1436, 1429, 1418, 1408, 1397, 1389, 1384, 1380, 1381, 1382, 1384, 1386, 1390, 1393, 1400, 1410, 1420, 1430, 1446, 1466, 1475, 1478, 1485, 1496, 1507, 1524, 1537, 1550, 1564, 1576, 1585, 1592, 1601, 1605, 1604, 1606, 1614, 1615, 1616, 1616, 1613, 1749, 1746, 1739, 1733, 1730, 1718, 1710, 1698, 1691, 1686, 1677, 1669, 1658, 1647, 1638, 1632, 1628, 1620, 1614, 1606, 1604, 1598, 1595, 1592, 1591, 1585, 1583, 1582, 1580, 1587, 1593, 1595, 1595, 1595, 1591, 1586, 1589, 1593, 1597, 1600, 1599, 1591, 1583, 1571, 1559, 1550, 1545, 1540, 1536, 1532, 1531, 1523, 1520, 1519, 1513, 1506, 1498, 1491, 1484, 1477, 1468, 1464, 1465, 1459, 1457, 1458, 1458, 1458, 1458, 1458, 1458, 1458, 1453, 1450, 1452, 1449, 1445, 1444, 1446, 1446, 1445, 1444, 1448, 1448, 1445, 1441, 1439, 1441, 1442, 1443, 1437, 1439, 1441, 1441, 1438, 1437, 1437, 1436, 1434, 1432, 1430, 1428, 1427, 1428, 1428, 1429, 1430, 1427, 1426, 1423, 1424, 1423, 1421, 1428, 1431, 1435, 1433, 1438, 1440, 1447, 1453, 1462, 1467, 1475, 1481, 1487, 1494, 1504, 1513, 1516, 1514, 1519, 1522, 1522, 1520, 1516, 1503, 1479, 1460, 1448, 1439, 1432, 1422, 1411, 1399, 1387, 1380, 1377, 1378, 1382, 1386, 1387, 1389, 1392, 1399, 1410, 1420, 1427, 1436, 1449, 1462, 1469, 1475, 1488, 1500, 1513, 1528, 1538, 1548, 1560, 1573, 1586, 1597, 1602, 1601, 1601, 1602, 1603, 1604, 1603, 1600, 1759, 1756, 1747, 1740, 1737, 1722, 1710, 1699, 1689, 1689, 1682, 1676, 1666, 1655, 1647, 1640, 1634, 1624, 1618, 1610, 1605, 1597, 1596, 1591, 1585, 1581, 1578, 1577, 1576, 1581, 1586, 1590, 1592, 1595, 1591, 1583, 1586, 1587, 1592, 1592, 1591, 1589, 1581, 1569, 1560, 1551, 1546, 1543, 1540, 1533, 1530, 1524, 1522, 1518, 1514, 1509, 1501, 1493, 1485, 1478, 1469, 1463, 1463, 1460, 1460, 1462, 1462, 1461, 1459, 1459, 1459, 1457, 1457, 1454, 1452, 1451, 1449, 1448, 1447, 1449, 1445, 1446, 1449, 1445, 1444, 1446, 1444, 1443, 1444, 1442, 1438, 1439, 1439, 1437, 1434, 1433, 1436, 1435, 1434, 1434, 1431, 1429, 1425, 1425, 1427, 1429, 1429, 1427, 1424, 1422, 1421, 1419, 1421, 1427, 1429, 1432, 1432, 1437, 1437, 1440, 1449, 1458, 1471, 1477, 1479, 1491, 1501, 1510, 1517, 1521, 1519, 1523, 1526, 1526, 1522, 1518, 1505, 1485, 1467, 1452, 1441, 1435, 1424, 1412, 1398, 1386, 1383, 1380, 1380, 1383, 1385, 1388, 1390, 1393, 1398, 1407, 1416, 1422, 1428, 1436, 1448, 1458, 1464, 1474, 1487, 1502, 1520, 1530, 1541, 1551, 1564, 1575, 1586, 1597, 1598, 1598, 1598, 1597, 1596, 1595, 1592, 1770, 1764, 1755, 1744, 1737, 1722, 1709, 1698, 1689, 1689, 1681, 1675, 1668, 1660, 1655, 1648, 1640, 1630, 1623, 1615, 1609, 1600, 1594, 1592, 1582, 1578, 1574, 1571, 1572, 1576, 1577, 1583, 1586, 1590, 1589, 1584, 1583, 1582, 1582, 1581, 1586, 1581, 1578, 1567, 1558, 1552, 1548, 1546, 1543, 1540, 1533, 1525, 1522, 1520, 1517, 1512, 1503, 1495, 1487, 1479, 1473, 1467, 1464, 1463, 1463, 1463, 1463, 1463, 1462, 1461, 1462, 1458, 1456, 1455, 1453, 1453, 1452, 1451, 1450, 1449, 1445, 1447, 1449, 1443, 1444, 1447, 1447, 1445, 1445, 1440, 1439, 1440, 1439, 1436, 1433, 1432, 1433, 1432, 1435, 1434, 1431, 1430, 1430, 1430, 1430, 1429, 1430, 1428, 1422, 1424, 1426, 1426, 1426, 1425, 1428, 1429, 1430, 1432, 1434, 1439, 1451, 1461, 1470, 1476, 1483, 1497, 1513, 1517, 1522, 1526, 1528, 1532, 1534, 1531, 1527, 1520, 1507, 1488, 1472, 1455, 1444, 1437, 1425, 1413, 1398, 1389, 1386, 1383, 1383, 1384, 1386, 1389, 1392, 1393, 1396, 1402, 1410, 1416, 1422, 1428, 1439, 1448, 1455, 1464, 1475, 1492, 1509, 1522, 1532, 1541, 1554, 1563, 1577, 1588, 1591, 1593, 1593, 1591, 1589, 1586, 1582, 1777, 1770, 1760, 1745, 1734, 1722, 1708, 1697, 1688, 1687, 1679, 1672, 1668, 1662, 1660, 1653, 1645, 1636, 1628, 1618, 1611, 1603, 1595, 1593, 1586, 1578, 1576, 1570, 1569, 1574, 1573, 1577, 1580, 1584, 1585, 1584, 1580, 1579, 1574, 1572, 1580, 1573, 1570, 1563, 1557, 1552, 1549, 1547, 1548, 1545, 1538, 1527, 1521, 1521, 1518, 1513, 1503, 1495, 1487, 1481, 1476, 1472, 1468, 1466, 1465, 1464, 1464, 1464, 1463, 1462, 1462, 1459, 1456, 1456, 1454, 1457, 1456, 1455, 1454, 1449, 1444, 1447, 1448, 1442, 1445, 1447, 1447, 1446, 1445, 1442, 1439, 1439, 1440, 1438, 1435, 1433, 1431, 1432, 1435, 1435, 1431, 1431, 1433, 1434, 1433, 1430, 1430, 1428, 1424, 1428, 1429, 1430, 1427, 1426, 1428, 1428, 1429, 1431, 1435, 1442, 1453, 1463, 1469, 1476, 1488, 1502, 1519, 1524, 1529, 1534, 1537, 1543, 1541, 1536, 1529, 1520, 1505, 1488, 1472, 1453, 1444, 1435, 1424, 1413, 1400, 1392, 1389, 1387, 1387, 1385, 1386, 1388, 1390, 1393, 1394, 1399, 1406, 1411, 1417, 1423, 1434, 1442, 1448, 1456, 1468, 1484, 1499, 1513, 1523, 1531, 1543, 1553, 1563, 1573, 1580, 1585, 1583, 1582, 1580, 1578, 1574, 1781, 1771, 1759, 1746, 1731, 1722, 1708, 1695, 1685, 1681, 1676, 1670, 1667, 1664, 1661, 1656, 1649, 1641, 1632, 1620, 1609, 1602, 1596, 1592, 1589, 1581, 1578, 1571, 1568, 1572, 1572, 1572, 1573, 1577, 1579, 1579, 1577, 1575, 1569, 1565, 1570, 1567, 1563, 1559, 1555, 1550, 1547, 1547, 1550, 1544, 1538, 1528, 1520, 1518, 1514, 1510, 1503, 1495, 1489, 1483, 1477, 1474, 1472, 1470, 1469, 1469, 1469, 1467, 1465, 1464, 1460, 1459, 1458, 1458, 1457, 1460, 1458, 1458, 1457, 1449, 1446, 1449, 1448, 1442, 1446, 1448, 1446, 1448, 1447, 1446, 1443, 1440, 1441, 1441, 1437, 1435, 1433, 1435, 1437, 1436, 1432, 1432, 1433, 1434, 1435, 1432, 1429, 1429, 1427, 1430, 1429, 1428, 1426, 1428, 1429, 1430, 1431, 1434, 1440, 1445, 1454, 1465, 1470, 1478, 1489, 1503, 1517, 1528, 1537, 1542, 1546, 1553, 1548, 1540, 1531, 1519, 1505, 1488, 1471, 1451, 1443, 1433, 1422, 1412, 1402, 1396, 1394, 1391, 1390, 1387, 1386, 1386, 1386, 1389, 1391, 1395, 1401, 1408, 1414, 1420, 1430, 1439, 1444, 1449, 1460, 1475, 1490, 1504, 1514, 1522, 1532, 1544, 1549, 1557, 1566, 1572, 1571, 1572, 1572, 1573, 1571, 1781, 1767, 1755, 1744, 1730, 1720, 1710, 1694, 1682, 1675, 1672, 1669, 1668, 1666, 1661, 1656, 1649, 1643, 1634, 1622, 1609, 1601, 1591, 1586, 1584, 1582, 1574, 1569, 1567, 1569, 1572, 1568, 1568, 1568, 1570, 1572, 1572, 1569, 1565, 1561, 1562, 1564, 1560, 1557, 1552, 1546, 1547, 1547, 1546, 1538, 1532, 1524, 1519, 1514, 1510, 1507, 1503, 1499, 1493, 1486, 1480, 1476, 1475, 1473, 1472, 1474, 1473, 1470, 1467, 1465, 1462, 1462, 1462, 1460, 1459, 1459, 1457, 1456, 1457, 1452, 1452, 1455, 1453, 1448, 1448, 1449, 1446, 1449, 1450, 1449, 1447, 1444, 1443, 1442, 1437, 1437, 1437, 1437, 1437, 1434, 1432, 1433, 1434, 1436, 1439, 1434, 1432, 1431, 1429, 1430, 1432, 1429, 1428, 1428, 1430, 1434, 1432, 1437, 1442, 1446, 1453, 1465, 1471, 1479, 1488, 1499, 1512, 1529, 1540, 1546, 1552, 1557, 1554, 1546, 1534, 1521, 1507, 1486, 1472, 1453, 1443, 1433, 1421, 1412, 1405, 1401, 1398, 1394, 1392, 1389, 1386, 1384, 1382, 1382, 1386, 1390, 1396, 1405, 1412, 1417, 1424, 1434, 1440, 1445, 1454, 1469, 1482, 1494, 1505, 1516, 1527, 1537, 1540, 1545, 1552, 1557, 1561, 1563, 1566, 1568, 1568, 1775, 1761, 1752, 1740, 1733, 1722, 1711, 1687, 1677, 1673, 1669, 1669, 1667, 1663, 1659, 1652, 1646, 1641, 1634, 1626, 1613, 1604, 1589, 1583, 1579, 1580, 1572, 1566, 1565, 1566, 1568, 1566, 1563, 1561, 1561, 1563, 1564, 1563, 1560, 1560, 1559, 1560, 1557, 1551, 1549, 1545, 1551, 1548, 1542, 1534, 1530, 1525, 1519, 1513, 1511, 1505, 1505, 1505, 1499, 1493, 1486, 1482, 1478, 1473, 1471, 1474, 1474, 1472, 1470, 1470, 1470, 1469, 1467, 1465, 1464, 1460, 1456, 1454, 1456, 1456, 1458, 1461, 1459, 1456, 1455, 1450, 1447, 1449, 1450, 1450, 1448, 1446, 1447, 1444, 1439, 1439, 1440, 1440, 1434, 1432, 1433, 1432, 1435, 1439, 1439, 1439, 1436, 1434, 1428, 1430, 1430, 1430, 1430, 1431, 1433, 1435, 1433, 1437, 1439, 1446, 1451, 1461, 1469, 1478, 1489, 1498, 1509, 1524, 1537, 1546, 1551, 1557, 1558, 1550, 1538, 1521, 1506, 1489, 1469, 1457, 1445, 1432, 1420, 1414, 1409, 1404, 1400, 1396, 1393, 1389, 1384, 1380, 1378, 1379, 1384, 1388, 1394, 1402, 1409, 1413, 1418, 1426, 1436, 1443, 1453, 1466, 1479, 1488, 1498, 1510, 1521, 1530, 1534, 1540, 1545, 1550, 1554, 1557, 1557, 1557, 1555, 1766, 1758, 1749, 1736, 1727, 1721, 1711, 1685, 1675, 1669, 1666, 1664, 1662, 1658, 1656, 1650, 1645, 1639, 1633, 1625, 1610, 1601, 1591, 1584, 1581, 1579, 1573, 1563, 1563, 1563, 1566, 1566, 1561, 1558, 1556, 1554, 1556, 1555, 1555, 1557, 1555, 1550, 1546, 1544, 1541, 1539, 1542, 1543, 1536, 1532, 1528, 1524, 1517, 1511, 1508, 1507, 1507, 1508, 1506, 1501, 1494, 1486, 1479, 1477, 1477, 1474, 1474, 1476, 1473, 1474, 1475, 1471, 1469, 1469, 1467, 1462, 1458, 1455, 1455, 1455, 1455, 1458, 1456, 1453, 1452, 1451, 1450, 1449, 1450, 1451, 1452, 1450, 1447, 1446, 1445, 1448, 1441, 1442, 1437, 1435, 1434, 1432, 1436, 1436, 1436, 1435, 1437, 1437, 1431, 1430, 1430, 1431, 1433, 1434, 1433, 1434, 1433, 1438, 1442, 1443, 1448, 1459, 1466, 1471, 1487, 1496, 1506, 1521, 1532, 1541, 1547, 1555, 1557, 1546, 1535, 1530, 1513, 1494, 1476, 1462, 1449, 1438, 1430, 1421, 1416, 1410, 1403, 1399, 1395, 1391, 1386, 1381, 1379, 1382, 1386, 1391, 1398, 1405, 1409, 1411, 1415, 1423, 1433, 1442, 1452, 1463, 1473, 1483, 1495, 1507, 1514, 1520, 1527, 1537, 1539, 1543, 1546, 1547, 1546, 1543, 1539, 1756, 1751, 1743, 1736, 1729, 1717, 1711, 1684, 1673, 1666, 1664, 1661, 1658, 1654, 1651, 1646, 1641, 1635, 1629, 1625, 1608, 1601, 1592, 1581, 1577, 1577, 1574, 1562, 1562, 1564, 1568, 1566, 1562, 1561, 1558, 1552, 1550, 1548, 1548, 1549, 1549, 1547, 1545, 1540, 1536, 1535, 1536, 1537, 1532, 1528, 1522, 1520, 1515, 1510, 1508, 1507, 1505, 1505, 1506, 1499, 1495, 1491, 1488, 1483, 1480, 1478, 1478, 1479, 1477, 1476, 1474, 1473, 1471, 1469, 1467, 1464, 1461, 1459, 1458, 1457, 1453, 1456, 1456, 1453, 1453, 1453, 1453, 1454, 1455, 1450, 1449, 1448, 1447, 1446, 1445, 1447, 1445, 1445, 1440, 1437, 1436, 1435, 1440, 1439, 1438, 1435, 1434, 1435, 1433, 1434, 1432, 1432, 1433, 1433, 1435, 1435, 1437, 1442, 1445, 1447, 1451, 1461, 1466, 1475, 1488, 1496, 1503, 1516, 1529, 1543, 1552, 1557, 1557, 1549, 1539, 1525, 1511, 1496, 1483, 1467, 1455, 1445, 1438, 1432, 1423, 1414, 1406, 1401, 1398, 1393, 1389, 1385, 1383, 1385, 1390, 1394, 1399, 1404, 1407, 1409, 1413, 1422, 1430, 1438, 1449, 1460, 1469, 1477, 1490, 1503, 1508, 1515, 1524, 1532, 1533, 1536, 1532, 1529, 1528, 1525, 1520, 1759, 1748, 1740, 1734, 1725, 1715, 1703, 1687, 1674, 1668, 1664, 1659, 1653, 1649, 1645, 1641, 1636, 1630, 1625, 1618, 1606, 1599, 1589, 1581, 1575, 1574, 1563, 1559, 1563, 1566, 1568, 1566, 1562, 1562, 1559, 1554, 1549, 1546, 1546, 1546, 1544, 1542, 1542, 1541, 1536, 1529, 1528, 1526, 1522, 1518, 1518, 1514, 1510, 1508, 1506, 1504, 1502, 1502, 1501, 1497, 1495, 1493, 1487, 1482, 1482, 1481, 1478, 1481, 1481, 1477, 1474, 1472, 1472, 1471, 1469, 1463, 1460, 1461, 1461, 1460, 1455, 1454, 1456, 1453, 1453, 1455, 1457, 1459, 1453, 1449, 1447, 1448, 1449, 1450, 1450, 1449, 1447, 1441, 1438, 1437, 1438, 1439, 1442, 1440, 1440, 1438, 1437, 1435, 1434, 1437, 1434, 1431, 1432, 1434, 1437, 1438, 1440, 1444, 1447, 1453, 1457, 1463, 1469, 1481, 1491, 1498, 1505, 1514, 1528, 1544, 1555, 1558, 1559, 1553, 1543, 1530, 1513, 1498, 1487, 1474, 1460, 1448, 1441, 1434, 1428, 1420, 1412, 1406, 1402, 1396, 1391, 1388, 1386, 1387, 1392, 1395, 1398, 1402, 1405, 1408, 1415, 1422, 1428, 1434, 1445, 1455, 1465, 1474, 1486, 1501, 1505, 1509, 1517, 1520, 1520, 1521, 1517, 1513, 1513, 1511, 1505, 1766, 1754, 1741, 1732, 1723, 1712, 1698, 1687, 1676, 1669, 1665, 1659, 1653, 1648, 1640, 1634, 1628, 1624, 1620, 1613, 1605, 1598, 1588, 1580, 1578, 1572, 1563, 1563, 1564, 1565, 1570, 1566, 1560, 1561, 1557, 1556, 1552, 1549, 1547, 1546, 1544, 1540, 1538, 1536, 1533, 1528, 1523, 1520, 1516, 1511, 1510, 1508, 1506, 1504, 1504, 1502, 1500, 1500, 1498, 1497, 1496, 1494, 1491, 1487, 1485, 1482, 1481, 1483, 1483, 1480, 1477, 1475, 1475, 1473, 1472, 1466, 1463, 1464, 1464, 1463, 1459, 1456, 1457, 1454, 1454, 1456, 1456, 1456, 1451, 1448, 1446, 1447, 1450, 1452, 1452, 1451, 1446, 1441, 1439, 1439, 1442, 1443, 1442, 1441, 1439, 1440, 1439, 1437, 1435, 1437, 1433, 1431, 1433, 1437, 1443, 1440, 1442, 1445, 1451, 1459, 1464, 1469, 1475, 1488, 1495, 1499, 1508, 1517, 1529, 1545, 1557, 1561, 1561, 1557, 1549, 1536, 1519, 1504, 1495, 1485, 1470, 1457, 1448, 1439, 1433, 1426, 1418, 1412, 1406, 1399, 1395, 1393, 1391, 1391, 1391, 1392, 1395, 1398, 1404, 1410, 1417, 1423, 1428, 1434, 1444, 1454, 1463, 1471, 1483, 1494, 1500, 1505, 1510, 1510, 1509, 1506, 1502, 1498, 1497, 1496, 1494, 1768, 1758, 1744, 1732, 1722, 1707, 1691, 1680, 1676, 1671, 1665, 1658, 1654, 1648, 1639, 1629, 1622, 1617, 1612, 1607, 1601, 1599, 1590, 1579, 1578, 1572, 1564, 1566, 1562, 1564, 1566, 1565, 1559, 1559, 1558, 1553, 1555, 1553, 1549, 1548, 1545, 1541, 1536, 1532, 1529, 1525, 1520, 1518, 1513, 1510, 1507, 1507, 1504, 1503, 1503, 1501, 1498, 1498, 1497, 1497, 1496, 1494, 1492, 1490, 1485, 1483, 1482, 1482, 1483, 1483, 1480, 1479, 1477, 1474, 1474, 1469, 1465, 1467, 1466, 1465, 1463, 1459, 1457, 1456, 1457, 1456, 1454, 1453, 1452, 1450, 1449, 1448, 1452, 1453, 1451, 1449, 1447, 1442, 1439, 1441, 1442, 1444, 1443, 1440, 1439, 1441, 1437, 1436, 1438, 1435, 1434, 1434, 1438, 1443, 1447, 1445, 1444, 1448, 1457, 1466, 1473, 1475, 1482, 1491, 1497, 1500, 1510, 1518, 1528, 1542, 1555, 1562, 1563, 1561, 1557, 1544, 1528, 1514, 1505, 1494, 1479, 1466, 1455, 1446, 1440, 1433, 1424, 1418, 1413, 1405, 1399, 1399, 1397, 1395, 1391, 1390, 1391, 1395, 1402, 1410, 1416, 1421, 1426, 1432, 1443, 1451, 1460, 1468, 1480, 1488, 1493, 1498, 1499, 1500, 1498, 1492, 1488, 1485, 1484, 1482, 1484, 1767, 1758, 1747, 1732, 1719, 1701, 1682, 1673, 1672, 1668, 1661, 1653, 1651, 1644, 1636, 1626, 1616, 1609, 1604, 1600, 1597, 1594, 1588, 1578, 1574, 1571, 1564, 1563, 1559, 1561, 1563, 1564, 1559, 1559, 1560, 1554, 1556, 1554, 1550, 1549, 1547, 1544, 1538, 1531, 1526, 1522, 1521, 1521, 1516, 1513, 1510, 1510, 1507, 1505, 1504, 1500, 1498, 1497, 1497, 1497, 1495, 1492, 1491, 1490, 1485, 1484, 1483, 1483, 1483, 1482, 1482, 1481, 1478, 1475, 1475, 1470, 1465, 1465, 1466, 1466, 1464, 1461, 1459, 1460, 1460, 1457, 1454, 1453, 1453, 1453, 1452, 1451, 1454, 1453, 1451, 1447, 1445, 1441, 1439, 1442, 1441, 1443, 1443, 1440, 1440, 1441, 1438, 1434, 1438, 1435, 1440, 1439, 1445, 1449, 1450, 1449, 1447, 1452, 1463, 1470, 1478, 1479, 1486, 1491, 1497, 1501, 1510, 1518, 1530, 1540, 1550, 1559, 1562, 1564, 1562, 1552, 1540, 1526, 1514, 1501, 1485, 1473, 1462, 1453, 1448, 1440, 1431, 1426, 1421, 1414, 1408, 1406, 1401, 1397, 1393, 1389, 1390, 1393, 1401, 1408, 1413, 1417, 1422, 1429, 1439, 1444, 1455, 1466, 1474, 1480, 1483, 1486, 1488, 1491, 1486, 1479, 1476, 1476, 1474, 1471, 1476, 1767, 1756, 1745, 1733, 1716, 1696, 1676, 1669, 1666, 1659, 1653, 1643, 1643, 1636, 1628, 1621, 1610, 1602, 1598, 1596, 1595, 1587, 1581, 1575, 1572, 1570, 1565, 1559, 1559, 1561, 1565, 1565, 1562, 1560, 1559, 1556, 1554, 1551, 1549, 1548, 1547, 1547, 1542, 1533, 1527, 1524, 1526, 1527, 1523, 1520, 1519, 1517, 1514, 1511, 1509, 1503, 1502, 1499, 1498, 1499, 1495, 1492, 1490, 1490, 1486, 1484, 1485, 1487, 1483, 1481, 1482, 1482, 1479, 1476, 1477, 1471, 1466, 1464, 1465, 1466, 1463, 1463, 1464, 1464, 1463, 1460, 1456, 1454, 1453, 1453, 1453, 1454, 1455, 1451, 1449, 1445, 1442, 1440, 1439, 1441, 1441, 1442, 1441, 1440, 1439, 1440, 1441, 1436, 1439, 1438, 1445, 1446, 1450, 1453, 1452, 1453, 1453, 1458, 1467, 1472, 1479, 1482, 1488, 1493, 1498, 1503, 1512, 1520, 1534, 1540, 1548, 1556, 1560, 1565, 1564, 1559, 1550, 1537, 1524, 1508, 1494, 1482, 1469, 1460, 1455, 1446, 1438, 1433, 1426, 1420, 1415, 1410, 1403, 1399, 1395, 1389, 1387, 1390, 1397, 1403, 1409, 1413, 1420, 1427, 1433, 1436, 1447, 1460, 1463, 1468, 1470, 1472, 1477, 1478, 1472, 1466, 1464, 1466, 1466, 1463, 1467, 1766, 1751, 1736, 1730, 1715, 1693, 1677, 1670, 1658, 1647, 1644, 1634, 1636, 1629, 1619, 1614, 1606, 1597, 1595, 1594, 1595, 1586, 1577, 1572, 1571, 1569, 1568, 1563, 1564, 1567, 1567, 1567, 1566, 1562, 1560, 1555, 1552, 1547, 1546, 1544, 1545, 1545, 1544, 1534, 1529, 1528, 1531, 1530, 1527, 1529, 1530, 1528, 1524, 1520, 1517, 1510, 1508, 1504, 1503, 1502, 1497, 1493, 1490, 1491, 1486, 1484, 1486, 1485, 1482, 1480, 1480, 1481, 1478, 1476, 1475, 1471, 1469, 1467, 1465, 1465, 1463, 1466, 1469, 1467, 1465, 1461, 1459, 1457, 1454, 1454, 1454, 1454, 1454, 1449, 1445, 1444, 1442, 1442, 1441, 1440, 1440, 1441, 1439, 1439, 1440, 1441, 1442, 1438, 1444, 1443, 1445, 1450, 1454, 1455, 1452, 1457, 1461, 1463, 1469, 1474, 1481, 1483, 1489, 1497, 1503, 1507, 1518, 1527, 1536, 1542, 1551, 1558, 1564, 1567, 1565, 1564, 1558, 1548, 1534, 1519, 1504, 1492, 1476, 1465, 1457, 1449, 1442, 1434, 1425, 1419, 1416, 1409, 1404, 1400, 1394, 1388, 1384, 1386, 1391, 1396, 1402, 1410, 1418, 1426, 1430, 1432, 1439, 1449, 1453, 1455, 1458, 1462, 1464, 1463, 1460, 1456, 1451, 1453, 1455, 1456, 1455, 1763, 1745, 1730, 1720, 1706, 1689, 1679, 1668, 1656, 1645, 1636, 1633, 1628, 1623, 1614, 1607, 1598, 1597, 1597, 1595, 1593, 1588, 1580, 1575, 1570, 1570, 1568, 1565, 1566, 1569, 1569, 1569, 1568, 1564, 1560, 1556, 1552, 1545, 1544, 1541, 1541, 1539, 1538, 1532, 1529, 1526, 1530, 1531, 1525, 1531, 1532, 1533, 1529, 1525, 1521, 1516, 1514, 1510, 1506, 1501, 1497, 1495, 1492, 1490, 1488, 1486, 1486, 1486, 1482, 1479, 1477, 1477, 1476, 1473, 1469, 1469, 1467, 1465, 1463, 1464, 1464, 1466, 1469, 1467, 1463, 1460, 1461, 1459, 1456, 1454, 1454, 1456, 1452, 1451, 1448, 1446, 1445, 1447, 1444, 1442, 1442, 1441, 1439, 1438, 1441, 1442, 1440, 1440, 1446, 1450, 1450, 1456, 1461, 1459, 1458, 1459, 1465, 1467, 1474, 1476, 1480, 1483, 1491, 1502, 1508, 1515, 1522, 1530, 1536, 1545, 1555, 1563, 1568, 1572, 1573, 1572, 1566, 1557, 1545, 1530, 1514, 1498, 1485, 1471, 1464, 1454, 1443, 1432, 1425, 1417, 1414, 1408, 1402, 1398, 1393, 1389, 1384, 1382, 1383, 1387, 1393, 1402, 1411, 1419, 1428, 1433, 1434, 1440, 1446, 1448, 1451, 1454, 1455, 1450, 1450, 1445, 1448, 1443, 1441, 1441, 1439, 1749, 1738, 1724, 1706, 1695, 1683, 1676, 1665, 1654, 1644, 1641, 1639, 1634, 1617, 1609, 1601, 1599, 1599, 1597, 1589, 1588, 1584, 1579, 1577, 1573, 1570, 1568, 1567, 1570, 1571, 1570, 1569, 1568, 1565, 1561, 1557, 1556, 1548, 1543, 1539, 1536, 1533, 1529, 1527, 1528, 1528, 1525, 1524, 1526, 1526, 1527, 1528, 1525, 1520, 1518, 1515, 1512, 1510, 1508, 1503, 1499, 1496, 1496, 1495, 1491, 1488, 1486, 1485, 1484, 1481, 1477, 1474, 1472, 1469, 1467, 1468, 1468, 1467, 1464, 1464, 1464, 1465, 1467, 1466, 1463, 1461, 1459, 1459, 1457, 1455, 1456, 1455, 1455, 1452, 1452, 1449, 1447, 1447, 1447, 1445, 1442, 1439, 1439, 1436, 1435, 1436, 1436, 1443, 1449, 1455, 1457, 1462, 1467, 1466, 1467, 1463, 1471, 1473, 1476, 1479, 1486, 1491, 1496, 1506, 1517, 1526, 1533, 1539, 1545, 1554, 1565, 1572, 1575, 1580, 1580, 1585, 1580, 1571, 1556, 1536, 1519, 1505, 1489, 1477, 1469, 1457, 1443, 1435, 1430, 1421, 1414, 1409, 1404, 1400, 1395, 1388, 1381, 1373, 1374, 1379, 1383, 1392, 1401, 1409, 1416, 1424, 1429, 1430, 1436, 1442, 1446, 1447, 1444, 1443, 1439, 1433, 1434, 1432, 1427, 1427, 1424, 1741, 1729, 1717, 1703, 1692, 1681, 1671, 1660, 1651, 1646, 1639, 1636, 1631, 1616, 1609, 1604, 1601, 1600, 1596, 1589, 1585, 1583, 1582, 1580, 1574, 1570, 1570, 1570, 1572, 1572, 1571, 1569, 1567, 1564, 1562, 1559, 1557, 1555, 1547, 1542, 1540, 1537, 1531, 1528, 1529, 1528, 1524, 1521, 1520, 1522, 1523, 1523, 1520, 1517, 1516, 1514, 1510, 1507, 1505, 1502, 1499, 1498, 1499, 1498, 1494, 1490, 1486, 1485, 1484, 1481, 1479, 1476, 1473, 1470, 1466, 1468, 1469, 1466, 1464, 1466, 1465, 1465, 1467, 1463, 1462, 1460, 1458, 1458, 1458, 1456, 1454, 1451, 1452, 1450, 1449, 1446, 1445, 1445, 1446, 1444, 1443, 1440, 1438, 1435, 1435, 1435, 1437, 1441, 1451, 1453, 1460, 1465, 1470, 1474, 1475, 1474, 1478, 1477, 1479, 1482, 1489, 1492, 1500, 1514, 1525, 1534, 1542, 1547, 1553, 1560, 1569, 1576, 1581, 1584, 1588, 1593, 1588, 1582, 1567, 1547, 1529, 1518, 1501, 1483, 1473, 1460, 1448, 1441, 1438, 1430, 1421, 1414, 1408, 1404, 1398, 1390, 1381, 1376, 1373, 1373, 1375, 1384, 1390, 1398, 1406, 1413, 1417, 1420, 1422, 1428, 1435, 1434, 1428, 1429, 1424, 1419, 1416, 1416, 1413, 1411, 1408, 1732, 1721, 1712, 1701, 1688, 1674, 1663, 1655, 1647, 1643, 1639, 1632, 1629, 1618, 1611, 1606, 1603, 1601, 1599, 1595, 1590, 1587, 1585, 1581, 1576, 1573, 1574, 1574, 1575, 1573, 1567, 1565, 1565, 1563, 1561, 1559, 1557, 1556, 1552, 1546, 1544, 1540, 1535, 1529, 1528, 1526, 1523, 1519, 1520, 1520, 1521, 1520, 1518, 1516, 1515, 1513, 1509, 1506, 1504, 1501, 1499, 1498, 1498, 1497, 1492, 1487, 1485, 1485, 1483, 1480, 1478, 1476, 1474, 1470, 1468, 1469, 1469, 1466, 1464, 1465, 1465, 1466, 1468, 1465, 1463, 1460, 1460, 1459, 1458, 1456, 1455, 1452, 1453, 1451, 1450, 1447, 1445, 1445, 1445, 1445, 1443, 1441, 1439, 1439, 1438, 1437, 1438, 1440, 1449, 1452, 1457, 1463, 1477, 1484, 1480, 1482, 1480, 1482, 1484, 1487, 1493, 1498, 1506, 1518, 1529, 1539, 1550, 1554, 1560, 1565, 1572, 1578, 1582, 1588, 1595, 1595, 1590, 1587, 1574, 1553, 1535, 1524, 1505, 1489, 1480, 1470, 1455, 1447, 1441, 1435, 1429, 1421, 1412, 1406, 1400, 1391, 1382, 1376, 1372, 1371, 1373, 1377, 1384, 1390, 1396, 1402, 1406, 1409, 1409, 1413, 1418, 1419, 1416, 1413, 1408, 1406, 1403, 1400, 1395, 1393, 1393, 1722, 1715, 1706, 1694, 1681, 1666, 1657, 1650, 1640, 1636, 1633, 1629, 1625, 1620, 1615, 1611, 1609, 1608, 1606, 1600, 1594, 1590, 1587, 1581, 1578, 1576, 1576, 1576, 1575, 1570, 1564, 1561, 1560, 1558, 1559, 1557, 1556, 1556, 1554, 1550, 1547, 1541, 1535, 1529, 1526, 1524, 1521, 1519, 1519, 1520, 1520, 1518, 1517, 1516, 1514, 1510, 1507, 1504, 1502, 1500, 1500, 1499, 1497, 1495, 1491, 1486, 1484, 1483, 1482, 1480, 1476, 1474, 1472, 1469, 1470, 1471, 1470, 1467, 1466, 1467, 1467, 1464, 1466, 1466, 1464, 1462, 1462, 1460, 1458, 1456, 1455, 1454, 1454, 1452, 1450, 1448, 1446, 1446, 1447, 1446, 1443, 1443, 1443, 1443, 1443, 1442, 1440, 1442, 1447, 1456, 1460, 1465, 1482, 1489, 1489, 1489, 1487, 1488, 1490, 1493, 1499, 1504, 1512, 1521, 1531, 1539, 1550, 1555, 1560, 1564, 1568, 1571, 1576, 1583, 1591, 1591, 1592, 1590, 1582, 1566, 1545, 1533, 1515, 1500, 1490, 1480, 1464, 1455, 1447, 1440, 1436, 1430, 1419, 1410, 1402, 1394, 1385, 1378, 1374, 1373, 1373, 1374, 1378, 1382, 1387, 1393, 1398, 1400, 1399, 1398, 1400, 1401, 1402, 1396, 1392, 1389, 1384, 1381, 1379, 1380, 1382, 1709, 1702, 1692, 1681, 1671, 1661, 1653, 1645, 1635, 1631, 1628, 1626, 1623, 1621, 1619, 1616, 1615, 1613, 1610, 1602, 1595, 1589, 1585, 1580, 1579, 1579, 1578, 1577, 1573, 1565, 1561, 1558, 1556, 1555, 1556, 1555, 1554, 1553, 1551, 1550, 1548, 1541, 1533, 1527, 1525, 1522, 1520, 1519, 1519, 1519, 1518, 1518, 1516, 1514, 1512, 1508, 1505, 1503, 1502, 1501, 1501, 1501, 1499, 1497, 1493, 1488, 1484, 1482, 1482, 1481, 1477, 1473, 1471, 1470, 1471, 1471, 1471, 1468, 1466, 1468, 1467, 1462, 1462, 1464, 1464, 1462, 1459, 1458, 1456, 1455, 1453, 1455, 1453, 1452, 1451, 1450, 1450, 1451, 1451, 1448, 1446, 1447, 1448, 1449, 1450, 1450, 1448, 1449, 1451, 1460, 1464, 1471, 1490, 1494, 1498, 1497, 1494, 1493, 1495, 1498, 1504, 1512, 1517, 1525, 1532, 1538, 1544, 1548, 1554, 1557, 1559, 1562, 1570, 1577, 1583, 1588, 1594, 1593, 1588, 1578, 1558, 1543, 1527, 1513, 1504, 1494, 1478, 1465, 1455, 1447, 1442, 1437, 1425, 1414, 1405, 1398, 1390, 1383, 1380, 1376, 1374, 1372, 1372, 1374, 1377, 1383, 1387, 1388, 1387, 1385, 1385, 1382, 1383, 1379, 1377, 1374, 1367, 1364, 1367, 1370, 1370, 1692, 1681, 1673, 1664, 1658, 1654, 1645, 1637, 1629, 1626, 1624, 1622, 1621, 1621, 1620, 1618, 1616, 1613, 1608, 1601, 1591, 1584, 1581, 1579, 1579, 1580, 1579, 1576, 1569, 1561, 1558, 1556, 1554, 1553, 1554, 1553, 1550, 1549, 1547, 1546, 1545, 1540, 1531, 1525, 1524, 1521, 1519, 1518, 1517, 1517, 1516, 1517, 1514, 1510, 1508, 1506, 1502, 1501, 1500, 1500, 1501, 1501, 1501, 1501, 1496, 1490, 1485, 1482, 1480, 1479, 1476, 1473, 1471, 1471, 1471, 1470, 1470, 1466, 1464, 1464, 1463, 1460, 1460, 1461, 1461, 1458, 1456, 1455, 1454, 1454, 1454, 1454, 1452, 1451, 1452, 1452, 1452, 1454, 1453, 1450, 1450, 1450, 1453, 1455, 1457, 1458, 1459, 1458, 1457, 1463, 1469, 1478, 1495, 1499, 1503, 1503, 1500, 1500, 1498, 1500, 1504, 1518, 1520, 1525, 1532, 1535, 1538, 1540, 1546, 1549, 1551, 1556, 1567, 1574, 1578, 1587, 1595, 1596, 1594, 1585, 1570, 1553, 1540, 1529, 1520, 1508, 1492, 1477, 1463, 1454, 1447, 1441, 1430, 1418, 1409, 1401, 1395, 1390, 1387, 1381, 1375, 1371, 1367, 1365, 1367, 1371, 1374, 1373, 1372, 1371, 1368, 1363, 1363, 1362, 1361, 1361, 1357, 1352, 1357, 1359, 1354, 1674, 1663, 1657, 1651, 1647, 1643, 1635, 1627, 1623, 1621, 1619, 1619, 1618, 1618, 1617, 1616, 1612, 1609, 1604, 1597, 1588, 1580, 1578, 1578, 1579, 1578, 1576, 1573, 1564, 1558, 1556, 1554, 1552, 1551, 1552, 1552, 1548, 1546, 1544, 1542, 1541, 1539, 1531, 1526, 1523, 1521, 1520, 1517, 1515, 1515, 1515, 1514, 1512, 1508, 1506, 1503, 1500, 1500, 1499, 1500, 1501, 1500, 1502, 1502, 1499, 1493, 1487, 1482, 1476, 1475, 1474, 1472, 1471, 1472, 1473, 1471, 1467, 1463, 1463, 1461, 1459, 1459, 1461, 1460, 1459, 1458, 1458, 1458, 1458, 1458, 1457, 1454, 1452, 1451, 1453, 1453, 1453, 1453, 1452, 1450, 1452, 1453, 1456, 1458, 1460, 1463, 1470, 1467, 1463, 1469, 1476, 1485, 1496, 1503, 1508, 1508, 1508, 1508, 1501, 1503, 1505, 1520, 1521, 1524, 1529, 1532, 1534, 1538, 1542, 1547, 1549, 1556, 1569, 1575, 1579, 1588, 1597, 1600, 1600, 1591, 1578, 1563, 1552, 1545, 1534, 1521, 1504, 1488, 1473, 1462, 1452, 1443, 1432, 1423, 1415, 1407, 1400, 1396, 1393, 1386, 1377, 1371, 1365, 1359, 1360, 1363, 1364, 1363, 1359, 1356, 1351, 1347, 1346, 1345, 1344, 1345, 1345, 1341, 1342, 1344, 1336, 1660, 1652, 1649, 1644, 1641, 1632, 1627, 1621, 1618, 1617, 1616, 1616, 1612, 1612, 1612, 1611, 1607, 1603, 1596, 1590, 1584, 1578, 1577, 1577, 1576, 1573, 1570, 1567, 1559, 1554, 1553, 1550, 1549, 1548, 1549, 1549, 1547, 1544, 1540, 1538, 1539, 1539, 1534, 1530, 1523, 1520, 1520, 1517, 1516, 1516, 1514, 1512, 1508, 1506, 1504, 1502, 1500, 1501, 1500, 1502, 1503, 1501, 1500, 1500, 1500, 1497, 1491, 1485, 1478, 1472, 1471, 1472, 1470, 1470, 1471, 1469, 1465, 1459, 1458, 1458, 1460, 1461, 1460, 1460, 1460, 1462, 1464, 1463, 1465, 1465, 1461, 1456, 1453, 1453, 1454, 1454, 1454, 1454, 1452, 1453, 1455, 1456, 1457, 1461, 1464, 1468, 1479, 1474, 1471, 1479, 1486, 1492, 1499, 1509, 1513, 1514, 1514, 1513, 1509, 1509, 1510, 1519, 1522, 1525, 1525, 1529, 1533, 1539, 1543, 1549, 1554, 1561, 1572, 1577, 1583, 1593, 1602, 1606, 1605, 1599, 1587, 1571, 1562, 1554, 1545, 1534, 1513, 1496, 1483, 1471, 1457, 1444, 1433, 1428, 1421, 1412, 1405, 1398, 1394, 1389, 1381, 1375, 1368, 1361, 1360, 1360, 1362, 1359, 1351, 1344, 1338, 1334, 1333, 1331, 1327, 1323, 1320, 1322, 1322, 1325, 1322, 1650, 1647, 1643, 1637, 1632, 1625, 1622, 1617, 1615, 1613, 1613, 1609, 1604, 1601, 1604, 1606, 1600, 1594, 1590, 1586, 1578, 1575, 1574, 1574, 1572, 1568, 1564, 1559, 1555, 1551, 1549, 1546, 1546, 1546, 1545, 1544, 1544, 1541, 1536, 1535, 1536, 1536, 1534, 1532, 1524, 1519, 1517, 1516, 1518, 1514, 1514, 1510, 1505, 1503, 1504, 1505, 1503, 1503, 1503, 1502, 1505, 1505, 1502, 1498, 1498, 1498, 1493, 1487, 1481, 1475, 1471, 1469, 1468, 1466, 1465, 1463, 1463, 1462, 1460, 1460, 1463, 1465, 1463, 1463, 1463, 1463, 1466, 1467, 1470, 1468, 1463, 1459, 1456, 1457, 1458, 1454, 1454, 1454, 1455, 1458, 1456, 1456, 1457, 1462, 1467, 1475, 1484, 1480, 1479, 1489, 1497, 1500, 1509, 1518, 1523, 1526, 1526, 1521, 1516, 1514, 1517, 1520, 1525, 1523, 1523, 1526, 1533, 1538, 1543, 1552, 1559, 1566, 1572, 1578, 1586, 1596, 1605, 1610, 1609, 1606, 1596, 1580, 1571, 1563, 1554, 1544, 1526, 1505, 1491, 1475, 1460, 1449, 1441, 1435, 1428, 1418, 1409, 1401, 1395, 1389, 1383, 1378, 1372, 1369, 1365, 1362, 1361, 1356, 1347, 1338, 1331, 1327, 1326, 1323, 1317, 1309, 1297, 1297, 1302, 1310, 1309, 1645, 1644, 1638, 1632, 1628, 1623, 1618, 1615, 1613, 1613, 1608, 1604, 1601, 1600, 1599, 1601, 1597, 1591, 1589, 1585, 1579, 1575, 1571, 1571, 1568, 1564, 1561, 1559, 1556, 1553, 1549, 1545, 1545, 1542, 1540, 1539, 1537, 1535, 1532, 1531, 1531, 1534, 1535, 1532, 1527, 1520, 1517, 1516, 1516, 1514, 1514, 1511, 1506, 1504, 1506, 1505, 1504, 1504, 1503, 1502, 1502, 1501, 1503, 1500, 1496, 1496, 1493, 1489, 1483, 1478, 1474, 1470, 1468, 1466, 1465, 1463, 1465, 1467, 1465, 1466, 1468, 1468, 1465, 1465, 1463, 1461, 1463, 1466, 1469, 1465, 1461, 1458, 1457, 1459, 1459, 1456, 1455, 1456, 1455, 1456, 1452, 1455, 1455, 1462, 1472, 1480, 1490, 1484, 1487, 1503, 1511, 1511, 1521, 1526, 1535, 1540, 1538, 1536, 1525, 1522, 1522, 1521, 1521, 1520, 1522, 1525, 1533, 1540, 1542, 1549, 1555, 1560, 1567, 1576, 1590, 1598, 1604, 1609, 1612, 1614, 1610, 1595, 1582, 1568, 1559, 1549, 1533, 1514, 1501, 1488, 1473, 1464, 1455, 1445, 1436, 1425, 1417, 1408, 1401, 1394, 1387, 1383, 1380, 1378, 1373, 1368, 1364, 1359, 1350, 1340, 1333, 1327, 1321, 1315, 1305, 1295, 1282, 1282, 1288, 1292, 1295, 1642, 1641, 1635, 1630, 1626, 1623, 1620, 1617, 1615, 1614, 1610, 1605, 1603, 1608, 1604, 1601, 1596, 1591, 1590, 1586, 1581, 1576, 1571, 1570, 1568, 1566, 1564, 1562, 1559, 1556, 1551, 1547, 1546, 1543, 1540, 1540, 1539, 1536, 1534, 1532, 1531, 1532, 1531, 1530, 1526, 1521, 1518, 1515, 1514, 1515, 1513, 1512, 1508, 1505, 1504, 1503, 1503, 1505, 1502, 1500, 1500, 1499, 1498, 1498, 1496, 1494, 1491, 1487, 1485, 1480, 1476, 1473, 1470, 1469, 1467, 1465, 1463, 1465, 1468, 1469, 1468, 1466, 1464, 1464, 1463, 1463, 1465, 1466, 1467, 1462, 1459, 1457, 1459, 1460, 1461, 1459, 1461, 1465, 1463, 1458, 1457, 1459, 1460, 1466, 1475, 1486, 1495, 1493, 1502, 1515, 1523, 1525, 1528, 1539, 1546, 1551, 1547, 1542, 1532, 1527, 1527, 1526, 1524, 1521, 1523, 1527, 1533, 1541, 1541, 1542, 1546, 1553, 1565, 1579, 1595, 1604, 1608, 1611, 1614, 1615, 1615, 1611, 1597, 1581, 1566, 1555, 1541, 1522, 1510, 1499, 1487, 1480, 1470, 1456, 1443, 1432, 1424, 1415, 1408, 1400, 1393, 1390, 1387, 1385, 1380, 1375, 1369, 1362, 1353, 1343, 1336, 1326, 1315, 1307, 1294, 1282, 1269, 1269, 1271, 1276, 1280, 1641, 1639, 1636, 1631, 1629, 1628, 1627, 1624, 1622, 1619, 1615, 1611, 1607, 1611, 1613, 1606, 1599, 1595, 1591, 1588, 1584, 1579, 1574, 1573, 1570, 1568, 1566, 1563, 1560, 1556, 1553, 1550, 1548, 1544, 1540, 1539, 1540, 1537, 1534, 1532, 1532, 1532, 1530, 1529, 1526, 1522, 1519, 1515, 1514, 1514, 1514, 1513, 1509, 1507, 1506, 1503, 1502, 1503, 1503, 1503, 1501, 1499, 1498, 1498, 1497, 1494, 1491, 1489, 1486, 1484, 1480, 1478, 1474, 1472, 1469, 1466, 1464, 1464, 1468, 1468, 1467, 1465, 1465, 1466, 1465, 1465, 1466, 1465, 1465, 1462, 1458, 1458, 1461, 1463, 1463, 1463, 1468, 1469, 1467, 1464, 1465, 1466, 1466, 1470, 1479, 1487, 1497, 1500, 1515, 1524, 1530, 1533, 1535, 1545, 1554, 1558, 1554, 1548, 1540, 1531, 1533, 1531, 1528, 1524, 1525, 1531, 1534, 1538, 1538, 1539, 1542, 1549, 1566, 1585, 1599, 1607, 1611, 1614, 1617, 1618, 1618, 1617, 1609, 1596, 1578, 1567, 1555, 1534, 1520, 1509, 1500, 1493, 1484, 1469, 1452, 1439, 1430, 1423, 1417, 1409, 1401, 1396, 1394, 1390, 1384, 1379, 1372, 1364, 1355, 1345, 1336, 1325, 1311, 1299, 1284, 1271, 1258, 1256, 1258, 1263, 1271, 1643, 1642, 1641, 1638, 1637, 1637, 1636, 1634, 1631, 1628, 1623, 1620, 1615, 1615, 1616, 1611, 1604, 1600, 1593, 1590, 1588, 1581, 1577, 1575, 1572, 1568, 1565, 1562, 1559, 1556, 1552, 1552, 1550, 1547, 1542, 1540, 1538, 1536, 1534, 1534, 1532, 1534, 1533, 1531, 1527, 1524, 1520, 1516, 1515, 1514, 1514, 1513, 1510, 1510, 1509, 1506, 1503, 1502, 1504, 1505, 1502, 1500, 1500, 1500, 1498, 1494, 1493, 1490, 1485, 1485, 1484, 1482, 1477, 1472, 1469, 1466, 1464, 1465, 1468, 1467, 1466, 1466, 1466, 1466, 1466, 1467, 1466, 1465, 1465, 1463, 1461, 1461, 1463, 1465, 1465, 1470, 1473, 1472, 1470, 1471, 1471, 1472, 1473, 1475, 1482, 1486, 1500, 1509, 1526, 1530, 1535, 1539, 1543, 1551, 1560, 1564, 1559, 1555, 1546, 1539, 1541, 1536, 1532, 1530, 1529, 1534, 1536, 1538, 1539, 1541, 1543, 1552, 1569, 1589, 1602, 1610, 1614, 1617, 1621, 1624, 1624, 1623, 1618, 1610, 1594, 1580, 1568, 1550, 1532, 1520, 1511, 1503, 1496, 1483, 1465, 1450, 1438, 1432, 1425, 1415, 1408, 1403, 1399, 1394, 1388, 1382, 1375, 1367, 1359, 1349, 1337, 1325, 1312, 1297, 1280, 1264, 1251, 1246, 1246, 1249, 1260, 1647, 1648, 1648, 1646, 1645, 1646, 1645, 1643, 1641, 1637, 1632, 1630, 1624, 1621, 1619, 1614, 1608, 1603, 1595, 1591, 1589, 1582, 1577, 1576, 1571, 1566, 1563, 1560, 1557, 1554, 1551, 1551, 1550, 1549, 1544, 1540, 1537, 1535, 1536, 1535, 1531, 1533, 1532, 1533, 1528, 1524, 1520, 1517, 1515, 1514, 1514, 1513, 1512, 1513, 1512, 1509, 1505, 1502, 1503, 1503, 1502, 1500, 1502, 1501, 1496, 1491, 1490, 1488, 1484, 1484, 1483, 1481, 1476, 1472, 1468, 1465, 1464, 1466, 1468, 1467, 1466, 1466, 1466, 1465, 1466, 1467, 1466, 1465, 1466, 1466, 1466, 1466, 1466, 1466, 1468, 1474, 1476, 1476, 1471, 1475, 1475, 1476, 1477, 1480, 1486, 1490, 1506, 1518, 1530, 1533, 1538, 1544, 1551, 1559, 1565, 1571, 1564, 1560, 1552, 1547, 1546, 1542, 1538, 1534, 1533, 1535, 1539, 1541, 1543, 1547, 1549, 1559, 1572, 1588, 1601, 1610, 1615, 1619, 1624, 1628, 1630, 1628, 1626, 1621, 1608, 1592, 1577, 1564, 1547, 1531, 1520, 1512, 1505, 1494, 1479, 1463, 1448, 1439, 1432, 1422, 1414, 1409, 1404, 1399, 1392, 1384, 1378, 1370, 1363, 1356, 1342, 1329, 1315, 1300, 1283, 1265, 1250, 1240, 1235, 1236, 1243, 1654, 1655, 1654, 1655, 1655, 1654, 1653, 1652, 1651, 1646, 1642, 1639, 1633, 1629, 1625, 1617, 1610, 1603, 1597, 1592, 1586, 1582, 1577, 1574, 1570, 1565, 1560, 1557, 1555, 1551, 1549, 1549, 1550, 1550, 1545, 1541, 1538, 1538, 1540, 1534, 1530, 1529, 1528, 1530, 1526, 1522, 1519, 1518, 1516, 1515, 1513, 1514, 1515, 1516, 1515, 1512, 1507, 1503, 1501, 1501, 1501, 1501, 1502, 1498, 1492, 1486, 1485, 1485, 1483, 1482, 1479, 1477, 1474, 1470, 1466, 1465, 1466, 1469, 1468, 1467, 1467, 1468, 1466, 1465, 1466, 1466, 1467, 1467, 1469, 1469, 1470, 1470, 1469, 1469, 1472, 1475, 1477, 1478, 1473, 1477, 1477, 1477, 1479, 1484, 1491, 1499, 1513, 1522, 1529, 1532, 1536, 1547, 1559, 1566, 1571, 1578, 1571, 1565, 1556, 1552, 1548, 1545, 1541, 1535, 1534, 1534, 1542, 1544, 1546, 1553, 1558, 1566, 1574, 1585, 1597, 1606, 1615, 1621, 1627, 1630, 1633, 1633, 1632, 1627, 1617, 1601, 1586, 1575, 1560, 1542, 1529, 1518, 1511, 1502, 1488, 1476, 1462, 1447, 1439, 1430, 1420, 1414, 1410, 1405, 1397, 1388, 1380, 1373, 1366, 1361, 1350, 1336, 1319, 1305, 1289, 1270, 1252, 1238, 1229, 1230, 1229, 1663, 1662, 1662, 1663, 1666, 1666, 1665, 1663, 1661, 1655, 1651, 1646, 1641, 1635, 1629, 1621, 1613, 1606, 1600, 1594, 1585, 1579, 1574, 1572, 1569, 1563, 1557, 1554, 1552, 1547, 1547, 1548, 1548, 1548, 1544, 1540, 1539, 1543, 1542, 1532, 1531, 1527, 1525, 1524, 1523, 1520, 1519, 1519, 1517, 1516, 1514, 1515, 1515, 1515, 1515, 1511, 1507, 1503, 1501, 1500, 1499, 1501, 1499, 1494, 1490, 1485, 1483, 1483, 1482, 1481, 1478, 1475, 1471, 1468, 1466, 1468, 1469, 1470, 1469, 1469, 1470, 1470, 1469, 1468, 1468, 1467, 1467, 1469, 1470, 1471, 1472, 1472, 1473, 1473, 1474, 1474, 1478, 1480, 1477, 1477, 1477, 1478, 1480, 1487, 1497, 1510, 1519, 1522, 1527, 1530, 1535, 1548, 1563, 1570, 1576, 1583, 1578, 1569, 1560, 1555, 1550, 1546, 1541, 1534, 1532, 1534, 1542, 1545, 1547, 1556, 1563, 1568, 1576, 1585, 1592, 1603, 1614, 1624, 1631, 1635, 1637, 1640, 1638, 1631, 1621, 1606, 1593, 1584, 1570, 1554, 1539, 1525, 1516, 1506, 1493, 1485, 1476, 1458, 1446, 1438, 1428, 1420, 1417, 1412, 1404, 1395, 1384, 1375, 1367, 1363, 1354, 1342, 1325, 1310, 1295, 1275, 1256, 1240, 1230, 1230, 1228, 1671, 1671, 1674, 1677, 1680, 1683, 1681, 1673, 1668, 1662, 1658, 1652, 1647, 1640, 1633, 1625, 1619, 1612, 1604, 1597, 1590, 1582, 1575, 1571, 1566, 1559, 1552, 1548, 1546, 1545, 1546, 1548, 1546, 1545, 1543, 1541, 1541, 1545, 1544, 1534, 1532, 1525, 1525, 1522, 1523, 1522, 1521, 1522, 1519, 1518, 1515, 1513, 1513, 1512, 1511, 1508, 1504, 1500, 1500, 1501, 1497, 1499, 1497, 1494, 1490, 1487, 1485, 1483, 1481, 1480, 1477, 1473, 1468, 1467, 1469, 1471, 1470, 1470, 1471, 1471, 1472, 1473, 1473, 1471, 1471, 1471, 1469, 1468, 1468, 1470, 1472, 1474, 1476, 1476, 1479, 1480, 1478, 1482, 1481, 1476, 1477, 1479, 1483, 1490, 1506, 1519, 1523, 1525, 1529, 1532, 1538, 1550, 1560, 1568, 1576, 1581, 1580, 1574, 1565, 1558, 1554, 1548, 1541, 1535, 1532, 1537, 1544, 1545, 1546, 1555, 1561, 1566, 1578, 1586, 1592, 1601, 1612, 1625, 1634, 1640, 1644, 1645, 1646, 1637, 1625, 1613, 1603, 1592, 1579, 1566, 1550, 1536, 1522, 1510, 1501, 1495, 1486, 1469, 1454, 1446, 1435, 1428, 1423, 1419, 1413, 1401, 1389, 1377, 1369, 1365, 1359, 1348, 1334, 1318, 1302, 1281, 1262, 1249, 1237, 1230, 1227, 1677, 1682, 1687, 1691, 1693, 1691, 1686, 1680, 1672, 1666, 1662, 1657, 1651, 1644, 1637, 1629, 1623, 1616, 1608, 1601, 1594, 1587, 1579, 1571, 1562, 1554, 1547, 1543, 1542, 1542, 1544, 1546, 1542, 1541, 1541, 1542, 1544, 1548, 1542, 1536, 1531, 1523, 1525, 1522, 1522, 1523, 1523, 1523, 1522, 1519, 1517, 1513, 1511, 1510, 1509, 1508, 1505, 1501, 1501, 1500, 1495, 1495, 1496, 1496, 1494, 1491, 1489, 1487, 1485, 1483, 1480, 1474, 1470, 1469, 1470, 1470, 1471, 1472, 1473, 1473, 1476, 1477, 1476, 1475, 1473, 1473, 1470, 1467, 1466, 1467, 1471, 1475, 1476, 1478, 1483, 1486, 1484, 1482, 1479, 1475, 1481, 1482, 1487, 1496, 1510, 1523, 1524, 1532, 1532, 1536, 1545, 1553, 1556, 1562, 1571, 1577, 1580, 1576, 1569, 1564, 1560, 1554, 1544, 1538, 1537, 1540, 1547, 1549, 1550, 1553, 1561, 1564, 1576, 1584, 1589, 1596, 1608, 1624, 1635, 1643, 1647, 1647, 1651, 1647, 1636, 1625, 1612, 1599, 1589, 1579, 1564, 1550, 1532, 1516, 1508, 1504, 1497, 1482, 1471, 1459, 1442, 1432, 1428, 1425, 1419, 1404, 1390, 1377, 1368, 1367, 1363, 1355, 1344, 1330, 1313, 1292, 1273, 1261, 1250, 1237, 1227, 1687, 1691, 1695, 1697, 1697, 1694, 1687, 1683, 1674, 1669, 1665, 1660, 1654, 1646, 1639, 1630, 1624, 1618, 1611, 1604, 1598, 1592, 1584, 1574, 1562, 1553, 1544, 1543, 1542, 1540, 1541, 1540, 1539, 1538, 1538, 1539, 1541, 1548, 1542, 1538, 1529, 1525, 1526, 1523, 1521, 1522, 1522, 1523, 1523, 1520, 1518, 1516, 1514, 1511, 1511, 1508, 1506, 1505, 1503, 1499, 1496, 1498, 1498, 1495, 1495, 1494, 1492, 1489, 1486, 1480, 1478, 1475, 1473, 1472, 1471, 1472, 1474, 1476, 1476, 1476, 1477, 1476, 1475, 1475, 1474, 1471, 1467, 1469, 1466, 1465, 1470, 1475, 1479, 1481, 1486, 1489, 1487, 1485, 1480, 1478, 1484, 1485, 1490, 1500, 1513, 1525, 1531, 1539, 1538, 1543, 1551, 1556, 1559, 1559, 1566, 1572, 1578, 1577, 1572, 1567, 1562, 1554, 1544, 1539, 1539, 1543, 1548, 1552, 1555, 1558, 1561, 1563, 1569, 1578, 1586, 1594, 1608, 1622, 1634, 1644, 1649, 1653, 1656, 1653, 1647, 1640, 1627, 1612, 1599, 1590, 1580, 1566, 1548, 1532, 1518, 1512, 1504, 1492, 1482, 1472, 1454, 1436, 1431, 1428, 1419, 1407, 1392, 1378, 1367, 1365, 1362, 1355, 1345, 1333, 1317, 1296, 1281, 1269, 1261, 1250, 1237, 1697, 1702, 1701, 1701, 1699, 1696, 1691, 1685, 1679, 1675, 1671, 1665, 1659, 1649, 1640, 1631, 1623, 1618, 1612, 1607, 1603, 1595, 1585, 1577, 1566, 1555, 1544, 1544, 1540, 1539, 1537, 1537, 1537, 1536, 1537, 1537, 1538, 1543, 1539, 1535, 1528, 1524, 1523, 1522, 1520, 1521, 1521, 1520, 1522, 1520, 1519, 1517, 1516, 1512, 1513, 1510, 1506, 1505, 1504, 1501, 1497, 1499, 1497, 1495, 1494, 1494, 1490, 1486, 1481, 1475, 1474, 1474, 1475, 1474, 1472, 1472, 1474, 1478, 1479, 1478, 1476, 1476, 1475, 1475, 1475, 1473, 1469, 1468, 1465, 1464, 1472, 1479, 1485, 1484, 1487, 1490, 1489, 1487, 1484, 1483, 1484, 1488, 1496, 1501, 1512, 1526, 1535, 1539, 1542, 1547, 1551, 1554, 1558, 1558, 1565, 1570, 1576, 1577, 1573, 1567, 1562, 1552, 1543, 1541, 1541, 1544, 1548, 1553, 1559, 1561, 1563, 1565, 1570, 1579, 1588, 1599, 1613, 1625, 1636, 1648, 1653, 1656, 1658, 1657, 1655, 1651, 1640, 1626, 1609, 1597, 1589, 1578, 1563, 1547, 1533, 1521, 1508, 1496, 1488, 1478, 1467, 1452, 1438, 1429, 1423, 1413, 1398, 1383, 1372, 1367, 1363, 1354, 1344, 1332, 1318, 1303, 1290, 1278, 1271, 1260, 1248, 1705, 1710, 1708, 1707, 1704, 1699, 1696, 1689, 1686, 1683, 1679, 1670, 1666, 1655, 1643, 1634, 1627, 1621, 1613, 1609, 1606, 1596, 1587, 1578, 1567, 1555, 1543, 1542, 1536, 1535, 1534, 1535, 1536, 1535, 1534, 1533, 1534, 1538, 1537, 1531, 1526, 1521, 1520, 1522, 1519, 1518, 1519, 1517, 1517, 1516, 1513, 1512, 1513, 1513, 1512, 1509, 1505, 1502, 1502, 1500, 1497, 1497, 1495, 1493, 1491, 1490, 1487, 1482, 1476, 1472, 1472, 1472, 1474, 1475, 1473, 1472, 1473, 1477, 1478, 1479, 1478, 1477, 1476, 1475, 1474, 1472, 1471, 1469, 1468, 1468, 1475, 1481, 1484, 1483, 1487, 1490, 1489, 1488, 1488, 1487, 1487, 1493, 1499, 1502, 1510, 1523, 1532, 1537, 1543, 1547, 1549, 1552, 1554, 1560, 1569, 1573, 1579, 1580, 1575, 1568, 1561, 1552, 1548, 1545, 1545, 1547, 1549, 1556, 1560, 1562, 1566, 1568, 1574, 1585, 1595, 1607, 1618, 1627, 1637, 1650, 1656, 1656, 1657, 1659, 1659, 1655, 1648, 1637, 1618, 1601, 1594, 1585, 1572, 1559, 1546, 1533, 1518, 1504, 1495, 1485, 1477, 1467, 1453, 1437, 1430, 1422, 1405, 1387, 1375, 1369, 1365, 1357, 1346, 1333, 1322, 1313, 1302, 1289, 1279, 1269, 1257, 1714, 1716, 1716, 1716, 1713, 1706, 1703, 1698, 1694, 1689, 1686, 1678, 1672, 1662, 1650, 1640, 1632, 1623, 1612, 1608, 1605, 1594, 1586, 1574, 1562, 1553, 1542, 1538, 1534, 1532, 1532, 1533, 1534, 1534, 1531, 1531, 1532, 1535, 1535, 1529, 1526, 1522, 1520, 1523, 1518, 1516, 1515, 1515, 1513, 1510, 1507, 1506, 1507, 1508, 1506, 1504, 1501, 1498, 1498, 1496, 1495, 1493, 1491, 1490, 1487, 1486, 1484, 1478, 1474, 1472, 1474, 1473, 1472, 1474, 1475, 1473, 1473, 1475, 1477, 1477, 1477, 1477, 1475, 1473, 1472, 1470, 1471, 1472, 1472, 1473, 1478, 1480, 1482, 1481, 1486, 1489, 1490, 1489, 1491, 1492, 1492, 1497, 1501, 1505, 1510, 1519, 1526, 1534, 1540, 1543, 1546, 1550, 1553, 1562, 1574, 1581, 1586, 1586, 1579, 1572, 1562, 1557, 1555, 1551, 1550, 1551, 1553, 1560, 1561, 1565, 1570, 1572, 1580, 1591, 1602, 1615, 1625, 1630, 1639, 1650, 1658, 1658, 1659, 1662, 1661, 1657, 1652, 1643, 1626, 1608, 1599, 1590, 1579, 1569, 1557, 1546, 1531, 1515, 1503, 1494, 1485, 1478, 1467, 1452, 1438, 1427, 1411, 1394, 1381, 1373, 1369, 1361, 1351, 1338, 1327, 1319, 1311, 1299, 1288, 1275, 1263, 1722, 1722, 1725, 1725, 1721, 1714, 1710, 1706, 1699, 1694, 1690, 1686, 1678, 1669, 1658, 1647, 1636, 1622, 1610, 1604, 1600, 1589, 1581, 1568, 1556, 1551, 1542, 1535, 1533, 1531, 1532, 1532, 1532, 1531, 1529, 1530, 1531, 1533, 1532, 1529, 1528, 1525, 1522, 1522, 1517, 1514, 1512, 1513, 1509, 1506, 1503, 1502, 1500, 1500, 1500, 1499, 1495, 1492, 1494, 1492, 1491, 1488, 1487, 1486, 1485, 1484, 1481, 1477, 1475, 1475, 1476, 1474, 1471, 1473, 1475, 1474, 1473, 1474, 1474, 1474, 1475, 1474, 1473, 1472, 1471, 1470, 1471, 1473, 1474, 1475, 1479, 1480, 1481, 1481, 1486, 1489, 1491, 1492, 1495, 1496, 1499, 1502, 1505, 1507, 1511, 1515, 1522, 1531, 1536, 1539, 1542, 1547, 1553, 1563, 1576, 1585, 1591, 1590, 1584, 1575, 1566, 1562, 1558, 1555, 1556, 1556, 1558, 1562, 1563, 1569, 1574, 1576, 1583, 1594, 1607, 1619, 1629, 1633, 1641, 1652, 1660, 1662, 1664, 1665, 1664, 1659, 1652, 1645, 1632, 1616, 1605, 1596, 1586, 1578, 1567, 1555, 1542, 1527, 1514, 1503, 1493, 1487, 1478, 1465, 1447, 1429, 1414, 1404, 1392, 1381, 1374, 1365, 1357, 1345, 1331, 1320, 1315, 1306, 1294, 1279, 1265, 1730, 1730, 1733, 1731, 1728, 1721, 1716, 1711, 1703, 1698, 1694, 1692, 1687, 1676, 1664, 1653, 1638, 1622, 1609, 1599, 1593, 1583, 1575, 1563, 1555, 1550, 1545, 1535, 1535, 1532, 1531, 1530, 1529, 1528, 1527, 1527, 1528, 1529, 1527, 1527, 1528, 1527, 1525, 1521, 1516, 1512, 1510, 1511, 1506, 1502, 1499, 1496, 1494, 1494, 1495, 1495, 1491, 1487, 1489, 1488, 1488, 1485, 1485, 1483, 1483, 1483, 1480, 1478, 1477, 1477, 1478, 1475, 1473, 1473, 1474, 1474, 1473, 1473, 1472, 1472, 1473, 1472, 1471, 1471, 1473, 1474, 1475, 1473, 1474, 1475, 1481, 1482, 1482, 1483, 1487, 1490, 1494, 1495, 1498, 1501, 1507, 1510, 1511, 1510, 1513, 1515, 1522, 1530, 1535, 1537, 1541, 1545, 1552, 1564, 1576, 1585, 1591, 1590, 1586, 1578, 1571, 1564, 1558, 1559, 1561, 1561, 1561, 1562, 1566, 1570, 1575, 1577, 1584, 1594, 1606, 1618, 1629, 1636, 1644, 1655, 1663, 1666, 1670, 1670, 1668, 1662, 1653, 1647, 1637, 1625, 1616, 1605, 1594, 1586, 1576, 1562, 1550, 1540, 1526, 1512, 1503, 1498, 1488, 1476, 1457, 1434, 1418, 1414, 1405, 1392, 1379, 1370, 1362, 1350, 1335, 1321, 1316, 1310, 1299, 1282, 1268, 1737, 1738, 1740, 1738, 1735, 1728, 1723, 1714, 1707, 1703, 1701, 1696, 1691, 1681, 1669, 1657, 1643, 1626, 1609, 1596, 1588, 1580, 1574, 1565, 1560, 1553, 1548, 1539, 1537, 1534, 1529, 1528, 1526, 1526, 1524, 1523, 1522, 1523, 1524, 1524, 1526, 1527, 1526, 1521, 1516, 1511, 1509, 1508, 1502, 1498, 1495, 1492, 1493, 1495, 1494, 1494, 1491, 1485, 1485, 1483, 1483, 1482, 1483, 1481, 1480, 1481, 1479, 1478, 1476, 1478, 1478, 1476, 1475, 1474, 1473, 1473, 1472, 1472, 1471, 1472, 1472, 1471, 1470, 1472, 1475, 1478, 1480, 1475, 1475, 1477, 1483, 1483, 1485, 1488, 1492, 1495, 1496, 1497, 1501, 1509, 1516, 1519, 1516, 1514, 1515, 1520, 1525, 1532, 1537, 1539, 1545, 1547, 1552, 1563, 1575, 1582, 1586, 1588, 1585, 1580, 1574, 1567, 1559, 1564, 1565, 1568, 1565, 1564, 1566, 1569, 1573, 1576, 1584, 1591, 1600, 1613, 1625, 1635, 1645, 1654, 1662, 1670, 1674, 1676, 1673, 1665, 1657, 1650, 1642, 1632, 1627, 1618, 1605, 1596, 1583, 1568, 1558, 1551, 1539, 1524, 1514, 1507, 1499, 1487, 1468, 1444, 1425, 1422, 1412, 1401, 1385, 1375, 1363, 1351, 1337, 1323, 1318, 1312, 1302, 1287, 1270, 1745, 1745, 1746, 1744, 1739, 1732, 1726, 1718, 1712, 1709, 1706, 1703, 1692, 1682, 1671, 1661, 1647, 1630, 1611, 1597, 1588, 1581, 1576, 1568, 1563, 1556, 1552, 1544, 1539, 1533, 1526, 1524, 1523, 1522, 1521, 1519, 1519, 1519, 1522, 1524, 1525, 1525, 1524, 1520, 1515, 1510, 1506, 1505, 1501, 1497, 1495, 1494, 1495, 1496, 1495, 1493, 1489, 1486, 1482, 1480, 1479, 1479, 1480, 1478, 1477, 1477, 1476, 1475, 1475, 1477, 1477, 1478, 1476, 1473, 1471, 1470, 1470, 1471, 1473, 1473, 1474, 1472, 1472, 1474, 1477, 1478, 1478, 1476, 1477, 1479, 1483, 1485, 1489, 1493, 1499, 1502, 1502, 1504, 1509, 1521, 1529, 1531, 1522, 1522, 1523, 1526, 1528, 1534, 1539, 1543, 1549, 1549, 1554, 1563, 1573, 1579, 1583, 1585, 1583, 1581, 1576, 1571, 1566, 1568, 1570, 1573, 1568, 1570, 1567, 1567, 1568, 1573, 1582, 1588, 1596, 1607, 1618, 1626, 1637, 1647, 1658, 1667, 1670, 1678, 1676, 1668, 1660, 1655, 1648, 1638, 1632, 1626, 1617, 1605, 1591, 1576, 1566, 1557, 1547, 1536, 1524, 1515, 1509, 1497, 1481, 1457, 1436, 1430, 1418, 1411, 1395, 1378, 1364, 1352, 1340, 1331, 1325, 1314, 1304, 1289, 1272, 1756, 1754, 1751, 1747, 1739, 1739, 1729, 1723, 1718, 1715, 1711, 1708, 1694, 1680, 1670, 1665, 1649, 1635, 1617, 1604, 1592, 1588, 1579, 1574, 1563, 1558, 1554, 1547, 1540, 1530, 1524, 1522, 1520, 1517, 1518, 1518, 1517, 1518, 1518, 1521, 1523, 1523, 1522, 1518, 1514, 1509, 1508, 1505, 1502, 1499, 1498, 1498, 1497, 1497, 1495, 1492, 1489, 1487, 1483, 1479, 1478, 1477, 1478, 1478, 1477, 1476, 1476, 1473, 1471, 1475, 1479, 1477, 1474, 1472, 1470, 1470, 1472, 1474, 1476, 1477, 1476, 1475, 1474, 1475, 1474, 1474, 1474, 1477, 1478, 1481, 1487, 1493, 1498, 1502, 1506, 1510, 1512, 1516, 1526, 1532, 1539, 1543, 1538, 1534, 1535, 1534, 1533, 1536, 1542, 1545, 1549, 1550, 1554, 1563, 1568, 1573, 1576, 1581, 1583, 1584, 1582, 1578, 1576, 1574, 1578, 1579, 1577, 1578, 1574, 1570, 1570, 1573, 1580, 1588, 1594, 1601, 1609, 1613, 1621, 1632, 1644, 1653, 1661, 1672, 1676, 1674, 1669, 1664, 1660, 1652, 1644, 1637, 1629, 1614, 1600, 1588, 1577, 1565, 1556, 1548, 1540, 1531, 1519, 1504, 1488, 1466, 1451, 1440, 1426, 1416, 1402, 1386, 1370, 1356, 1347, 1340, 1331, 1316, 1303, 1292, 1279, 1765, 1760, 1756, 1751, 1745, 1741, 1733, 1727, 1721, 1718, 1714, 1709, 1696, 1680, 1668, 1663, 1650, 1634, 1615, 1607, 1593, 1589, 1579, 1574, 1563, 1558, 1557, 1549, 1541, 1531, 1525, 1522, 1520, 1517, 1516, 1517, 1517, 1517, 1517, 1518, 1520, 1521, 1521, 1519, 1513, 1508, 1507, 1503, 1500, 1499, 1499, 1499, 1499, 1497, 1493, 1490, 1490, 1486, 1481, 1478, 1479, 1479, 1479, 1479, 1479, 1477, 1476, 1473, 1472, 1473, 1477, 1476, 1474, 1474, 1473, 1475, 1478, 1479, 1479, 1479, 1477, 1476, 1474, 1474, 1474, 1475, 1475, 1480, 1484, 1488, 1494, 1503, 1509, 1513, 1516, 1519, 1522, 1526, 1536, 1540, 1546, 1551, 1551, 1547, 1547, 1545, 1545, 1544, 1548, 1547, 1550, 1551, 1555, 1561, 1564, 1567, 1570, 1574, 1580, 1586, 1586, 1584, 1583, 1582, 1582, 1581, 1583, 1586, 1582, 1577, 1576, 1577, 1582, 1589, 1594, 1598, 1600, 1605, 1610, 1619, 1628, 1637, 1649, 1664, 1676, 1676, 1673, 1672, 1668, 1664, 1657, 1649, 1638, 1625, 1615, 1606, 1592, 1577, 1568, 1560, 1552, 1545, 1536, 1515, 1492, 1475, 1463, 1452, 1434, 1416, 1405, 1394, 1379, 1365, 1353, 1344, 1331, 1316, 1306, 1298, 1282, 1769, 1764, 1760, 1756, 1751, 1743, 1737, 1731, 1725, 1720, 1716, 1708, 1697, 1678, 1666, 1660, 1648, 1628, 1610, 1602, 1590, 1584, 1575, 1571, 1560, 1557, 1557, 1548, 1540, 1531, 1526, 1521, 1521, 1518, 1516, 1517, 1517, 1517, 1519, 1518, 1518, 1517, 1519, 1514, 1511, 1506, 1504, 1502, 1499, 1498, 1497, 1497, 1497, 1496, 1491, 1488, 1484, 1480, 1476, 1475, 1478, 1479, 1478, 1479, 1478, 1477, 1475, 1474, 1476, 1475, 1476, 1477, 1476, 1477, 1477, 1479, 1480, 1480, 1479, 1478, 1474, 1475, 1477, 1479, 1479, 1479, 1481, 1486, 1493, 1499, 1505, 1515, 1521, 1525, 1528, 1529, 1534, 1540, 1549, 1554, 1560, 1563, 1562, 1559, 1562, 1561, 1561, 1556, 1556, 1553, 1553, 1555, 1558, 1562, 1563, 1565, 1568, 1571, 1578, 1588, 1589, 1588, 1587, 1587, 1586, 1585, 1585, 1590, 1590, 1585, 1583, 1583, 1586, 1590, 1594, 1597, 1598, 1603, 1609, 1615, 1622, 1630, 1640, 1655, 1670, 1674, 1672, 1673, 1673, 1672, 1666, 1658, 1647, 1635, 1628, 1621, 1607, 1593, 1581, 1569, 1562, 1556, 1546, 1527, 1503, 1485, 1473, 1462, 1440, 1422, 1412, 1403, 1391, 1376, 1362, 1348, 1336, 1321, 1311, 1301, 1286, 1768, 1764, 1761, 1757, 1752, 1744, 1737, 1733, 1727, 1722, 1714, 1703, 1689, 1672, 1662, 1653, 1640, 1623, 1605, 1596, 1585, 1580, 1569, 1565, 1556, 1551, 1548, 1541, 1536, 1529, 1524, 1520, 1520, 1519, 1519, 1518, 1518, 1517, 1519, 1516, 1514, 1514, 1513, 1507, 1508, 1503, 1501, 1500, 1500, 1499, 1498, 1497, 1498, 1498, 1492, 1489, 1480, 1474, 1470, 1470, 1474, 1478, 1480, 1480, 1478, 1478, 1477, 1476, 1479, 1478, 1478, 1478, 1478, 1480, 1480, 1480, 1479, 1478, 1476, 1475, 1474, 1480, 1484, 1485, 1486, 1487, 1491, 1494, 1503, 1511, 1519, 1528, 1532, 1536, 1541, 1542, 1550, 1556, 1566, 1572, 1576, 1576, 1577, 1576, 1579, 1578, 1576, 1570, 1566, 1561, 1560, 1561, 1562, 1566, 1565, 1566, 1568, 1571, 1578, 1587, 1591, 1591, 1590, 1589, 1588, 1589, 1588, 1592, 1593, 1591, 1589, 1589, 1591, 1593, 1595, 1598, 1599, 1604, 1610, 1615, 1620, 1626, 1634, 1646, 1658, 1663, 1665, 1668, 1673, 1677, 1674, 1668, 1658, 1645, 1637, 1631, 1619, 1604, 1590, 1576, 1568, 1561, 1551, 1536, 1515, 1497, 1482, 1469, 1448, 1432, 1419, 1411, 1401, 1386, 1371, 1355, 1341, 1328, 1317, 1304, 1292, 1765, 1763, 1760, 1755, 1748, 1742, 1732, 1729, 1723, 1716, 1704, 1693, 1677, 1664, 1653, 1644, 1630, 1617, 1601, 1590, 1581, 1577, 1565, 1559, 1551, 1544, 1537, 1533, 1531, 1527, 1523, 1520, 1520, 1519, 1521, 1519, 1517, 1515, 1514, 1511, 1509, 1510, 1507, 1504, 1504, 1501, 1499, 1499, 1498, 1500, 1499, 1497, 1499, 1499, 1494, 1490, 1480, 1470, 1466, 1467, 1471, 1479, 1482, 1481, 1479, 1478, 1478, 1477, 1478, 1479, 1480, 1479, 1479, 1481, 1481, 1480, 1479, 1478, 1477, 1477, 1481, 1487, 1492, 1494, 1497, 1499, 1504, 1504, 1512, 1522, 1530, 1538, 1543, 1548, 1557, 1560, 1566, 1571, 1580, 1587, 1590, 1589, 1591, 1592, 1595, 1595, 1590, 1585, 1579, 1572, 1569, 1567, 1567, 1569, 1567, 1568, 1568, 1571, 1576, 1583, 1589, 1589, 1590, 1589, 1588, 1589, 1589, 1590, 1591, 1591, 1592, 1592, 1593, 1594, 1595, 1597, 1599, 1603, 1606, 1610, 1614, 1619, 1627, 1638, 1648, 1649, 1653, 1660, 1669, 1675, 1676, 1674, 1667, 1656, 1647, 1638, 1628, 1614, 1596, 1582, 1573, 1565, 1555, 1541, 1525, 1510, 1496, 1479, 1460, 1444, 1431, 1421, 1411, 1395, 1380, 1363, 1347, 1334, 1320, 1306, 1295, 1760, 1758, 1753, 1748, 1741, 1734, 1723, 1719, 1712, 1704, 1693, 1683, 1668, 1656, 1644, 1634, 1621, 1610, 1598, 1585, 1577, 1573, 1563, 1554, 1547, 1539, 1531, 1528, 1526, 1525, 1522, 1519, 1517, 1515, 1517, 1516, 1513, 1510, 1508, 1506, 1505, 1506, 1504, 1502, 1501, 1500, 1498, 1497, 1495, 1497, 1499, 1497, 1496, 1497, 1495, 1489, 1478, 1467, 1465, 1465, 1469, 1478, 1482, 1482, 1481, 1478, 1478, 1479, 1479, 1481, 1483, 1483, 1482, 1482, 1483, 1483, 1483, 1483, 1484, 1486, 1492, 1495, 1500, 1505, 1509, 1512, 1517, 1516, 1522, 1530, 1537, 1545, 1557, 1565, 1576, 1581, 1581, 1585, 1592, 1598, 1602, 1601, 1601, 1604, 1606, 1610, 1605, 1600, 1593, 1583, 1578, 1575, 1572, 1571, 1570, 1569, 1569, 1569, 1572, 1575, 1580, 1583, 1586, 1588, 1587, 1585, 1584, 1584, 1585, 1586, 1588, 1590, 1591, 1591, 1592, 1595, 1596, 1597, 1598, 1600, 1603, 1608, 1617, 1628, 1639, 1637, 1644, 1653, 1662, 1668, 1671, 1673, 1670, 1664, 1657, 1647, 1638, 1627, 1608, 1590, 1579, 1571, 1562, 1547, 1533, 1521, 1509, 1494, 1473, 1458, 1445, 1432, 1419, 1402, 1388, 1371, 1354, 1338, 1323, 1308, 1295, 1753, 1749, 1743, 1739, 1731, 1722, 1714, 1710, 1702, 1692, 1684, 1675, 1662, 1649, 1639, 1627, 1616, 1605, 1595, 1584, 1574, 1569, 1562, 1552, 1545, 1538, 1532, 1527, 1523, 1522, 1520, 1515, 1512, 1510, 1510, 1509, 1506, 1503, 1502, 1503, 1502, 1502, 1502, 1500, 1499, 1499, 1499, 1496, 1492, 1491, 1495, 1495, 1493, 1493, 1491, 1487, 1477, 1467, 1466, 1465, 1468, 1474, 1480, 1483, 1482, 1480, 1480, 1482, 1483, 1485, 1486, 1488, 1488, 1486, 1488, 1489, 1491, 1494, 1495, 1498, 1503, 1505, 1509, 1515, 1520, 1525, 1531, 1531, 1536, 1543, 1549, 1556, 1573, 1583, 1591, 1598, 1594, 1601, 1606, 1611, 1615, 1613, 1612, 1613, 1618, 1623, 1620, 1615, 1604, 1593, 1587, 1583, 1579, 1575, 1572, 1569, 1568, 1567, 1567, 1563, 1566, 1573, 1578, 1581, 1582, 1580, 1575, 1574, 1575, 1580, 1581, 1583, 1584, 1584, 1587, 1589, 1589, 1588, 1588, 1590, 1593, 1597, 1604, 1616, 1629, 1631, 1639, 1646, 1653, 1659, 1664, 1668, 1670, 1669, 1663, 1656, 1650, 1642, 1625, 1604, 1591, 1581, 1571, 1556, 1543, 1528, 1516, 1509, 1489, 1474, 1458, 1441, 1425, 1406, 1396, 1378, 1363, 1345, 1330, 1313, 1299, 1747, 1742, 1739, 1732, 1725, 1717, 1711, 1707, 1698, 1688, 1679, 1670, 1661, 1649, 1639, 1628, 1618, 1607, 1597, 1588, 1576, 1566, 1562, 1553, 1545, 1539, 1533, 1528, 1521, 1519, 1517, 1511, 1507, 1508, 1504, 1502, 1499, 1497, 1496, 1497, 1499, 1500, 1500, 1498, 1499, 1500, 1496, 1496, 1492, 1489, 1490, 1493, 1494, 1492, 1486, 1484, 1479, 1469, 1465, 1465, 1469, 1472, 1477, 1484, 1484, 1483, 1484, 1486, 1488, 1490, 1493, 1496, 1497, 1495, 1496, 1498, 1502, 1507, 1511, 1509, 1514, 1518, 1516, 1521, 1529, 1536, 1543, 1547, 1554, 1562, 1567, 1575, 1585, 1591, 1597, 1604, 1608, 1618, 1623, 1627, 1629, 1628, 1628, 1630, 1633, 1634, 1632, 1625, 1613, 1601, 1595, 1591, 1585, 1581, 1576, 1571, 1568, 1565, 1563, 1555, 1557, 1561, 1566, 1571, 1572, 1570, 1565, 1562, 1565, 1571, 1575, 1577, 1576, 1575, 1577, 1579, 1576, 1575, 1576, 1580, 1583, 1585, 1592, 1605, 1620, 1625, 1633, 1639, 1645, 1652, 1658, 1663, 1668, 1669, 1665, 1661, 1658, 1653, 1641, 1625, 1610, 1595, 1578, 1564, 1552, 1536, 1523, 1514, 1504, 1488, 1468, 1449, 1429, 1412, 1400, 1383, 1370, 1352, 1338, 1321, 1307, 1751, 1748, 1744, 1736, 1731, 1724, 1717, 1712, 1703, 1693, 1683, 1674, 1667, 1656, 1642, 1633, 1621, 1609, 1598, 1587, 1574, 1565, 1561, 1553, 1544, 1537, 1531, 1525, 1520, 1515, 1513, 1507, 1503, 1504, 1499, 1496, 1493, 1492, 1492, 1493, 1495, 1497, 1500, 1499, 1498, 1499, 1497, 1496, 1492, 1488, 1488, 1489, 1490, 1490, 1485, 1482, 1479, 1470, 1465, 1465, 1468, 1474, 1477, 1481, 1483, 1485, 1488, 1490, 1493, 1496, 1499, 1502, 1504, 1505, 1510, 1516, 1520, 1522, 1524, 1525, 1526, 1529, 1528, 1529, 1535, 1540, 1547, 1556, 1565, 1573, 1578, 1586, 1594, 1603, 1608, 1623, 1629, 1638, 1640, 1642, 1643, 1643, 1643, 1641, 1643, 1641, 1640, 1632, 1619, 1609, 1604, 1599, 1591, 1589, 1587, 1575, 1572, 1569, 1561, 1555, 1553, 1550, 1552, 1556, 1557, 1554, 1548, 1548, 1554, 1562, 1570, 1571, 1567, 1564, 1564, 1566, 1565, 1564, 1566, 1569, 1570, 1574, 1586, 1595, 1604, 1613, 1623, 1631, 1636, 1645, 1654, 1661, 1665, 1667, 1668, 1665, 1661, 1655, 1646, 1641, 1629, 1614, 1594, 1575, 1563, 1550, 1536, 1523, 1513, 1498, 1479, 1460, 1435, 1420, 1407, 1393, 1376, 1359, 1342, 1328, 1315}; - -} // namespace elevationmap \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.h b/src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.h deleted file mode 100644 index 3ed0a11d4e49d8f00598b53a42d8759bbdb728c6..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.h +++ /dev/null @@ -1,53 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.790836 - -#pragma once - -namespace elevationmap -{ - -static constexpr int MAP_SIZE = 32761; -static constexpr int RESOLUTION = 181; - -extern const int elevations[MAP_SIZE]; - -static constexpr double NORTH = 41.829348969; -static constexpr double EAST = 14.079935099; -static constexpr double SOUTH = 41.785137031; -static constexpr double WEST = 14.029930901; - -static constexpr double LAT_DELTA = 0.044211938000004; -static constexpr double LON_DELTA = 0.050004198; - -static constexpr int INVALID_ELEVATION = -10000; - -} // namespace elevationmap diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.cpp b/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.cpp deleted file mode 100644 index baff13d6db94d43b282078d743f68b2e899bfbd7..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.803849 - -#include "elevation_map_test_data.h" - -namespace elevationmap -{ -namespace test -{ - -const double test_latitudes[] = {41.82708234979836, 41.821291587208876, 41.81622659104515, 41.8157014338408, 41.82870583508878, 41.80107110081455, 41.82829865364242, 41.787064340054364, 41.796289270749185, 41.81002309344475, 41.8079927840481, 41.82362734720108, 41.79208534166083, 41.813576479597806, 41.79673020276218, 41.8279795159602, 41.808938145674055, 41.811153179646, 41.794064487584166, 41.80754173983661, 41.80601563231319, 41.82693833141129, 41.81445631515033, 41.826450981141704, 41.79849688390758, 41.818692403892385, 41.79499103831984, 41.7864549173588, 41.804008962028696, 41.80162235659861, 41.792011873182474, 41.81888275270903, 41.81756871815552, 41.81975106448033, 41.7994304765352, 41.82051559009884, 41.79220966808061, 41.815722206595844, 41.8198355980766, 41.82320726530367, 41.82790921139985, 41.82813514954995, 41.790293087215204, 41.80373728900156, 41.78972757698609, 41.81608094758319, 41.82926269893544, 41.819961765062864, 41.80400526726692, 41.78725878213593, 41.826369025075536, 41.80737545493757, 41.81555530876627, 41.81024298472925, 41.796006533683624, 41.79852663483084, 41.78528288216411, 41.79347496775014, 41.82703923649528, 41.80464317193785, 41.81501067894883, 41.81017756462398, 41.78742643741276, 41.804976704822636, 41.80573542340919, 41.78893977667544, 41.81550477888124, 41.81034397977335, 41.819038276599954, 41.82043709067041, 41.799637405233284, 41.803315169191514, 41.81117102351575, 41.82536648342734, 41.825884833500844, 41.791507625374805, 41.81599859047873, 41.815264813445076, 41.82810968627914, 41.787532375343524, 41.791748319226485, 41.79417300021781, 41.790312464752105, 41.817022907959995, 41.82104090610301, 41.80822683412401, 41.817538072360996, 41.81248961530259, 41.79511500595239, 41.78966031102617, 41.79901551681279, 41.807004835677745, 41.8028176328367, 41.80642576340565, 41.81049225106247, 41.80387837882641, 41.80459135386265, 41.799162758122705, 41.82366003623922, 41.81190359213679, 41.81602918988551, 41.81780230614857, 41.79985677805766, 41.799120849184774, 41.79562793657795, 41.79499999522934, 41.8202178618958, 41.79213421802276, 41.818202351415046, 41.82295491142914, 41.81040397444149, 41.8028326918664, 41.80075856760539, 41.80122455595223, 41.78714094183182, 41.81212377117246, 41.794946672799284, 41.82498908698345, 41.786731693117666, 41.79947255540831, 41.82894669915881, 41.82491669082763, 41.81929168060149, 41.805184250420524, 41.787174490448024, 41.80068850913464, 41.81489671643056, 41.80359769726767, 41.79359921192111, 41.82457173037957, 41.80245837271749, 41.82895479142485, 41.79286320223182, 41.82847385162532, 41.81174893503581, 41.82028496084967, 41.80791404017505, 41.82562012766585, 41.80855759647145, 41.82207377294201, 41.79468137512678, 41.80887489557865, 41.78600500894037, 41.809910513327175, 41.79192037726362, 41.81308638675384, 41.79315998107903, 41.79110026195912, 41.80339235519549, 41.80542210746179, 41.810497322691916, 41.81003480138001, 41.82435800186916, 41.82281178286366, 41.793645298012805, 41.8055529870875, 41.80145283721646, 41.78767935936966, 41.8147064682381, 41.808581204126604, 41.82827967493166, 41.7894366506153, 41.800386216290775, 41.80158450222138, 41.79545322435889, 41.78736939848283, 41.82907494401777, 41.797926639356064, 41.82725555934539, 41.7974383197433, 41.815812448626794, 41.8195789389162, 41.80784427994019, 41.79360284664005, 41.803112081559405, 41.78696536674973, 41.82146776019325, 41.82045635954891, 41.80846842893757, 41.812116635787525, 41.81304000008999, 41.813162533300996, 41.81167117973781, 41.81118126630085, 41.8052528356911, 41.8171798401996, 41.797188662988376, 41.80576522375628, 41.7884665214562, 41.79579663476566, 41.79948398472155, 41.81029821103301, 41.80128132445547, 41.79661470823312, 41.81464199109952, 41.799815892268356, 41.82649178146265, 41.80534067489791, 41.807576313756755, 41.80325229280602, 41.81841167996479, 41.81611109823366, 41.826976090334256, 41.79775516941842, 41.792800110948995, 41.79738317900435, 41.81956007288644, 41.78995580256388, 41.81719074296708, 41.80401621753635, 41.79029306037748, 41.82495903570796, 41.78664843409368, 41.79846807286977, 41.81030942162368, 41.79966108822262, 41.791356536430655, 41.828539918746316, 41.817930111171925, 41.822568889659294, 41.81264239337827, 41.79269201165448, 41.82410633618125, 41.81545315111207, 41.82605826304627, 41.8084437895293, 41.80837475283449, 41.81371363366126, 41.822462068957776, 41.823691443914726, 41.8132821877682, 41.8128701492982, 41.798753157470685, 41.82218835727688, 41.796865622024185, 41.79898347008589, 41.78936622906462, 41.80079187093953, 41.80363868481272, 41.82021237055085, 41.80147899673317, 41.78608611307781, 41.81831826360513, 41.78805733101757, 41.824277540205365, 41.791555261315814, 41.82837910403205, 41.827596267779455, 41.81910323207861, 41.818930768795475, 41.82014329147635, 41.786846005050734, 41.787652815693335, 41.81117468527427, 41.80370621241168, 41.80972066246537, 41.823274580024375, 41.79432775067166, 41.790917406611285, 41.81664058167814, 41.802720187518716, 41.79660545574593, 41.790409264006236, 41.808128336745206, 41.79455588245276, 41.82177794009934, 41.79524776161091, 41.81751591053669, 41.78744826279467, 41.81392143798948, 41.824550690194926, 41.802327478894505, 41.78871603788085, 41.81498393684539, 41.81035974698955, 41.79969672034178, 41.81789076620953, 41.80424794831275, 41.80812671719063, 41.81548154639344, 41.81848154884688, 41.78818781295562, 41.81746092635656, 41.799894402435434, 41.82148560602005, 41.82800250843458, 41.7857975246217, 41.80075208515957, 41.797051979347145, 41.81362990228457, 41.81416797784151, 41.79959713893544, 41.79084643113588, 41.815009434820624, 41.816468075137756, 41.80657423010421, 41.797378596993205, 41.81804193477951, 41.80214778664095, 41.785140744056704, 41.785137031, 41.785137031, 41.807243, 41.804541159, 41.826155884, 41.787102006, 41.809453597, 41.811664194, 41.793733797, 41.823454044, 41.828366481, 41.808225488, 41.813629169, 41.808716731, 41.826647128, 41.808471109, 41.815594144, 41.820997825, 41.806014891, 41.824682153, 41.813629169, 41.802084941, 41.812155438, 41.825419019, 41.799628722, 41.807734244, 41.789312603, 41.795207528, 41.822225934, 41.785382653, 41.825173397, 41.803558672, 41.788084494, 41.797172503, 41.810436084, 41.798400612, 41.810681706, 41.818787228, 41.806014891, 41.806751756, 41.79545315, 41.81903285, 41.816576631, 41.822471556, 41.825173397, 41.800856831, 41.808471109, 41.785137031, 41.792751309, 41.792996931, 41.799874344, 41.803804294, 41.804541159, 41.83721881322334, 41.83845951337057, 41.84691203728648, 41.84682513261238, 41.83523360348267, 41.83396437490874, 41.83555718547031, 41.83814562890976, 41.84668981096064, 41.832800044931545, 41.84183683990193, 41.849845701248164, 41.84011677059559, 41.835840083173544, 41.84276161700376, 41.8332844123594, 41.839206886246444, 41.83617740289866, 41.830166719547165, 41.84331316269042, 41.84059731715814, 41.851097675498295, 41.83300417357647, 41.8461443084529, 41.84036050368289, 41.83312878037554, 41.84633959845679, 41.84357111017135, 41.85059785185801, 41.848214835883695, 41.841251037572206, 41.83593576628918, 41.842316535572095, 41.83513834566402, 41.833546008829096, 41.83730453633941, 41.83275036231747, 41.83629757475488, 41.8509861591525, 41.842796659587144, 41.832276364695716, 41.83529701524999, 41.84276398350953, 41.83023022735609, 41.84318103105883, 41.84286929225523, 41.83820949880378, 41.833083809810006, 41.84364111272128, 41.841347776873384, 41.847568490891824, 41.83791500608981, 41.831397619328065, 41.8481382240157, 41.830593941381906, 41.830013118953026, 41.83626505960847, 41.8340448679517, 41.83095084324889, 41.84762522392701, 41.83454900556674, 41.8308556730467, 41.834616005693356, 41.838900245259794, 41.84899129766321, 41.83803093174601, 41.83368980773846, 41.846799585575475, 41.82987258569012, 41.84832732722259, 41.8450491466847, 41.84871117317088, 41.84956666194104, 41.8411273243975, 41.84604520291779, 41.84493473293011, 41.83036719801577, 41.84528703133368, 41.837327422666036, 41.83338121034787, 41.8481958143508, 41.82949119937888, 41.83117201459085, 41.84458133749265, 41.83481673742607, 41.849968471743075, 41.834681440591176, 41.83474032197834, 41.84282545159287, 41.84998506182762, 41.84206523305124, 41.8418897495065, 41.83296130737865, 41.843624382097076, 41.842934569592884, 41.840744594403965, 41.84457013498628, 41.83694254588932, 41.84884210320843, 41.835940525010884, 41.77162481006172, 41.7648483482748, 41.7761783977614, 41.77412999144142, 41.76677318272391, 41.76931078484994, 41.78414176747095, 41.77445026250166, 41.765293625517224, 41.784120095835256, 41.78081425543265, 41.77186820099382, 41.77373756544763, 41.77206075046624, 41.78424128444527, 41.78168215620172, 41.7699854351203, 41.7721534812427, 41.78037989340452, 41.771948745502094, 41.77489948849641, 41.77678111521066, 41.76337581565808, 41.76541500603111, 41.778580963781444, 41.78107213153558, 41.77058812851974, 41.767006976545126, 41.76570695335872, 41.77110124278643, 41.782987048928725, 41.77952228800069, 41.78361685933519, 41.763753983672785, 41.77492908315567, 41.77173202331327, 41.784691035190534, 41.78274029785501, 41.76982984726282, 41.763455276035515, 41.76905952858188, 41.78089001516991, 41.777501143367886, 41.78284748634242, 41.779310057713765, 41.78058506566497, 41.76899719707465, 41.76434578368621, 41.775149814899144, 41.77106808861477, 41.77405195080402, 41.77078367929031, 41.78036442822638, 41.76509300638822, 41.78349963438745, 41.77394990953858, 41.771395086366645, 41.775508150437474, 41.77966684947055, 41.772896798727736, 41.76992628319818, 41.77288335409914, 41.77348624597124, 41.77563017310786, 41.7786004216051, 41.77855040529373, 41.77659878111, 41.78076632684257, 41.76607377541678, 41.78258719510769, 41.76892818646068, 41.765471839024144, 41.78170221854487, 41.766746173777044, 41.77603808543134, 41.76770384612731, 41.76421488482499, 41.77852220108903, 41.76885813535075, 41.77084771190817, 41.779150574956674, 41.780845189814485, 41.77213404207111, 41.78494039284739, 41.772677733897865, 41.77117959637102, 41.771189504295954, 41.76520449797681, 41.77725986233693, 41.78378015966453, 41.77815758794603, 41.77235425346949, 41.78174089840301, 41.77514469253452, 41.77864160328462, 41.78426320606634, 41.78222895218705, 41.772595235333775, 41.78037991234499, 41.76343543538439, 41.82034797302362, 41.79696984610409, 41.80721232329438, 41.801536940449196, 41.789124204347004, 41.80272520460889, 41.80183276750684, 41.82245849616037, 41.79572436517866, 41.82772430494197, 41.817321932806124, 41.787844877630384, 41.81753252259322, 41.8265766322171, 41.795503424699255, 41.82350493740739, 41.79437644181642, 41.79469308838371, 41.82640782248337, 41.81589075136037, 41.795703844308626, 41.828751276969676, 41.81099633781871, 41.81664856449581, 41.81899375647674, 41.785350409407066, 41.82415896840287, 41.79351706937229, 41.78680335026969, 41.80033641701671, 41.81575991578718, 41.82307519639466, 41.82048011740881, 41.798165485982366, 41.79653518037043, 41.82740606972039, 41.828786672575944, 41.802479021637545, 41.824899536475804, 41.797341736138556, 41.81668486624567, 41.78847546765862, 41.78526776754682, 41.813506933364884, 41.8090019919661, 41.81216247845918, 41.81992883632366, 41.82292517669763, 41.807800695950355, 41.800775090546395, 41.8236317043099, 41.791336111173614, 41.79217385309076, 41.7946009803901, 41.824641552557445, 41.8118573606971, 41.81956935455188, 41.80360581704668, 41.80634079924277, 41.80632788797058, 41.79015517381882, 41.81243545593792, 41.789387626337245, 41.82405269517602, 41.82259107774191, 41.82508632782959, 41.787774068936, 41.815538099196075, 41.82494382446849, 41.794934138652, 41.80676846984443, 41.82851322571915, 41.797232778498916, 41.791815945631384, 41.78622040096297, 41.82797279710989, 41.81961867375226, 41.81857293932406, 41.81635836360326, 41.793110036912985, 41.8282322452757, 41.82175639577298, 41.827656698362475, 41.825837669941436, 41.82877206171785, 41.79366300149143, 41.807262349998204, 41.79617222378779, 41.801121837874085, 41.820389220127815, 41.828955283275214, 41.8172042323775, 41.79624691155235, 41.79189055091775, 41.790921588623604, 41.810183530240785, 41.793044112321844, 41.81676599065734, 41.82066132359837, 41.80914357400164, 41.829348969, 41.829348969, 41.829348969, 41.807243, 41.785137031, 0, 0, 0, 0, 0, 90, -90, 100, -100}; -const double test_longitudes[] = {14.057942896983715, 14.03827697918792, 14.04409890622163, 14.052961901815785, 14.072125837637198, 14.031579642700516, 14.075697515668194, 14.077468475635524, 14.07116236455456, 14.070895789308741, 14.03792831169845, 14.051851252408616, 14.037405397230621, 14.052654642379895, 14.040376965925246, 14.051895992679183, 14.051462865523476, 14.039491490887816, 14.039960173841523, 14.051585760801194, 14.05377253392513, 14.035203597040873, 14.0779648464303, 14.033725776196446, 14.068420415535675, 14.054123642355897, 14.035303057269449, 14.049535868534354, 14.063083496500466, 14.03641847427883, 14.063603154492087, 14.068420453553744, 14.079620826636752, 14.047509192997659, 14.045797246350407, 14.048512740730887, 14.032921666234735, 14.049796295110069, 14.054280432105887, 14.078803515090843, 14.041815398030236, 14.068622316570085, 14.059125316113413, 14.06630497990524, 14.045708752390405, 14.042645544908197, 14.060907166284386, 14.068063686104896, 14.038698740809217, 14.055502492541777, 14.056742368859803, 14.064713013757226, 14.068968430374785, 14.069119862066477, 14.035597512504483, 14.048884543171974, 14.06465210085702, 14.076126828690075, 14.070709408769089, 14.053188648763845, 14.079747140165926, 14.051359708804517, 14.045482816190473, 14.049564254664258, 14.064438422535543, 14.041098591240983, 14.034061855192034, 14.072989447360435, 14.065977020854728, 14.068467965438398, 14.052793084839916, 14.072013030046008, 14.06406668921375, 14.04774980003663, 14.033739743580725, 14.078355630645081, 14.049357133293775, 14.03760477712425, 14.067470348298542, 14.034962470976886, 14.079578442333236, 14.070143304656927, 14.076295475671786, 14.05765788372138, 14.070409404786773, 14.034237516593976, 14.03276446180832, 14.077553484481319, 14.069459001025017, 14.047624662709822, 14.057303624786808, 14.078713270344107, 14.054191342531748, 14.06419874386501, 14.060758865543137, 14.033790642864231, 14.037628868698553, 14.068178572148119, 14.038036345446498, 14.051934311283407, 14.034686641811255, 14.060602006702807, 14.047423908662115, 14.078740737579214, 14.069156515402012, 14.06607025614793, 14.062987397652694, 14.041271290573544, 14.072504999376841, 14.062695963065181, 14.050799364113828, 14.03854627254873, 14.037434532757393, 14.060519985871037, 14.031768488747518, 14.064186011019142, 14.047979555148968, 14.05075109132226, 14.037639532373028, 14.065355255944418, 14.054193614138127, 14.05347919975185, 14.034037157639142, 14.070650631904357, 14.069126512973043, 14.055909134818986, 14.03991983260316, 14.042800553501369, 14.037490164944623, 14.05706117205332, 14.077665401775509, 14.044625680839573, 14.054775330682027, 14.049641527404583, 14.036608544795717, 14.041905742396258, 14.04381750041341, 14.056440182761069, 14.062244981205252, 14.057498161896133, 14.069111111664512, 14.042446185956784, 14.068506198999764, 14.03669974583123, 14.078854621208096, 14.069617274245083, 14.045726977902442, 14.074884409919916, 14.033797243550714, 14.053761969307073, 14.049442213064385, 14.059506534103958, 14.031443084255935, 14.06856832301331, 14.065754354977376, 14.06077656153078, 14.062149175112719, 14.057757939659524, 14.041473479003695, 14.041689137125509, 14.071576876898082, 14.073685522711145, 14.07384260042293, 14.075159718081842, 14.059117316642993, 14.060666354009236, 14.034702180460403, 14.05436118518531, 14.074074931363748, 14.071495846858374, 14.060840359689104, 14.064607831253717, 14.074320434932273, 14.04348872046365, 14.041566937141843, 14.058287795056689, 14.073391404648453, 14.078677021551595, 14.05720892922648, 14.045320905194776, 14.031772861876185, 14.07641025541773, 14.065111072012353, 14.079161157391288, 14.062193914748718, 14.07709654276426, 14.037705331673719, 14.078891099602954, 14.032544744884664, 14.076966576434426, 14.053556326683584, 14.067883810174695, 14.037130925515951, 14.072523625197451, 14.047629638391996, 14.040453465602358, 14.05753776513554, 14.072618652317846, 14.070836427154154, 14.075812828024004, 14.074331214329572, 14.060528854204696, 14.070292737773885, 14.046242692641243, 14.061199080715467, 14.06755263594605, 14.041730309843398, 14.075355011823634, 14.047357253059088, 14.050346175948949, 14.03083609489711, 14.066227725851004, 14.053527087411567, 14.068561601192568, 14.055448344762288, 14.039443186286125, 14.051756264762307, 14.07472264498942, 14.061593093348822, 14.066909043340925, 14.079427055542887, 14.067368230926679, 14.03389735973976, 14.061622348887305, 14.061017755884619, 14.072729702452387, 14.054773577165676, 14.05720206375146, 14.03556452388179, 14.035008824258622, 14.04582288110551, 14.05741931088713, 14.030636036821305, 14.061344691906266, 14.039207577385566, 14.037761901730656, 14.048105153485372, 14.055321649820474, 14.074558183490925, 14.043296643081579, 14.068861836160783, 14.072077046056911, 14.045477645899734, 14.050791634554324, 14.046865632845774, 14.042541595171913, 14.035697386288412, 14.05507571386681, 14.065076002861398, 14.070839316780575, 14.069195094746972, 14.055696013294913, 14.050999267215204, 14.047095172497592, 14.038365176267671, 14.049723919969836, 14.054669547133516, 14.045950875079715, 14.033136833906664, 14.070519394587338, 14.032005386061009, 14.071035905250946, 14.03485216075261, 14.048736044588194, 14.040888787535364, 14.058778997316885, 14.078595615425666, 14.036980774680398, 14.077011888328316, 14.071884608542385, 14.075816456922034, 14.050114350444847, 14.032548506628991, 14.059926230535192, 14.041974640377566, 14.03658119941922, 14.036561969604506, 14.05853885747977, 14.06759335047709, 14.031373263908936, 14.032498315535953, 14.034599257418176, 14.036420219739002, 14.050329968922549, 14.055832438357161, 14.079654864937137, 14.043830798632722, 14.060058595926327, 14.068250460345409, 14.055832238877088, 14.040277245216, 14.077834872318505, 14.04317388032014, 14.076676861106463, 14.048147544369234, 14.048598488397026, 14.031442672983914, 14.048321880101726, 14.038551635884145, 14.058624369699668, 14.054933, 14.029930901, 14.029930901, 14.035486923, 14.070767663, 14.071323265, 14.076323685, 14.055766403, 14.064378238, 14.035486923, 14.077712691, 14.068267453, 14.031319906, 14.069100856, 14.034653519, 14.045209961, 14.043265354, 14.033264514, 14.055766403, 14.079657298, 14.059655619, 14.079657298, 14.070489862, 14.073267873, 14.051321586, 14.061044624, 14.034097917, 14.053266193, 14.060489022, 14.041320746, 14.07243447, 14.076323685, 14.068823055, 14.043543155, 14.038542735, 14.034653519, 14.07243447, 14.064656039, 14.058266613, 14.052154989, 14.078546094, 14.070212061, 14.030764304, 14.060489022, 14.070489862, 14.065767243, 14.036598127, 14.056322006, 14.060489022, 14.037987133, 14.050765983, 14.03743153, 14.079101696, 14.034169390791428, 14.013711948424152, 14.019673788184592, 14.070737365850247, 14.08290402517295, 14.024841924132831, 14.038352319021405, 14.031431241570884, 14.030448759252671, 14.101898377597742, 14.08712614803729, 14.039758398785207, 14.060251088586051, 14.006884994972264, 14.091305605395926, 14.07155854205553, 14.087615252668524, 14.10333101375195, 14.014898867193242, 14.079575178960583, 14.055699407516187, 14.064637085916917, 14.021065323352884, 14.08369621996299, 14.047090798119989, 14.102934107875948, 14.096285149639012, 14.103472462849188, 14.092373254652182, 14.032305480227345, 14.057404695149696, 14.071493841003294, 14.009146223502663, 14.090577641306414, 14.008564674030364, 14.066879393808014, 14.102831084787926, 14.098093309299449, 14.101001671387046, 14.092609604527148, 14.014754274968105, 14.085919330057182, 14.102117173609946, 14.05619017226132, 14.050176599151026, 14.041843336672297, 14.0066029978151, 14.054066756867885, 14.05460900423939, 14.081241460189043, 14.032022074603518, 14.025789595996875, 14.069115960712052, 14.018440154639531, 14.04943469408366, 14.050135623244003, 14.065807669408407, 14.043732582634798, 14.026177068694706, 14.098246865872447, 14.053644597599847, 14.017104537247686, 14.01109330386927, 14.05362419771267, 14.010118301064127, 14.049275259973346, 14.058513546380826, 14.084693295328709, 14.034842182390872, 14.101446797979126, 14.041059212519626, 14.04015450124807, 14.08164572846626, 14.061816453118935, 14.051444976385785, 14.07880075946775, 14.073464519672735, 14.041889730375823, 14.035639430202446, 14.046920789742497, 14.061419273467017, 14.011076360991199, 14.074039713559191, 14.005066958638373, 14.08763658573893, 14.012752546616971, 14.104905617096536, 14.0368158393896, 14.04201936540039, 14.024025071602209, 14.005630631975393, 14.034731043363033, 14.054770306173058, 14.013652438387387, 14.077560920252562, 14.068001578783738, 14.05061815713018, 14.03874973910959, 14.088657680106182, 14.07109051396466, 14.087160848538096, 14.059906653600192, 14.063315061467778, 14.0150512678653, 14.06496603323626, 14.09533192047974, 14.084900556274148, 14.034536255535098, 14.050379699412586, 14.061973247203104, 14.070971133195991, 14.02680307391102, 14.077097185031198, 14.083596090878762, 14.068228573413165, 14.085909446107175, 14.08636199737916, 14.104708309050219, 14.100746100299876, 14.058968931431172, 14.090379059684423, 14.07323627130891, 14.035416163562353, 14.02287150807912, 14.051593030902756, 14.074669989677975, 14.029221836745473, 14.02454583006859, 14.075112686101988, 14.050424056028099, 14.035550459439374, 14.045261858600643, 14.079370455640353, 14.02766804858213, 14.044078990874125, 14.031359893853473, 14.02828189344456, 14.065047360933809, 14.032185921560696, 14.075416027617317, 14.054148832426023, 14.038473115561713, 14.058948151005456, 14.061721678436774, 14.096307037566445, 14.03701156934895, 14.093209303364153, 14.051555727998617, 14.095698959118945, 14.049672305389743, 14.097372171643277, 14.079078735122781, 14.067129829261459, 14.073365176857731, 14.08906833200303, 14.02095543663082, 14.094972014048212, 14.0973598432155, 14.050548683172254, 14.009992953649313, 14.098041282936963, 14.01290777399211, 14.015105137781498, 14.023444214154376, 14.083032345543234, 14.048368540494385, 14.03407140298398, 14.102895474255254, 14.079261582750807, 14.057513658788897, 14.057580240050186, 14.069084935170194, 14.026536210833099, 14.040452998718191, 14.085791372470988, 14.070828702198167, 14.046940984382537, 14.024881376974308, 14.094659736032389, 14.06703132368698, 14.098573015401325, 14.070087532263132, 14.083451524237732, 14.020631481876618, 14.09601594222841, 14.059720256944795, 14.083157982120929, 14.10132776338662, 14.036861916304796, 14.02986385590702, 14.06601156887932, 14.010734209188175, 14.102824160722006, 14.02601984632984, 14.085550612404742, 14.008480704417481, 14.075500338438369, 14.00756434632568, 14.079554475838114, 14.07637188895545, 14.01610702385042, 14.019853545571205, 14.028162371923974, 13.98465891340882, 14.021926115328151, 14.00532933801604, 13.993954650736432, 13.980886878517437, 14.005429245186262, 14.015046481302873, 13.984608258936678, 13.994692816698246, 13.998926893745654, 13.989601167547297, 13.992764615161807, 14.023510416471604, 14.029251927797898, 13.98316789623261, 13.987111806105824, 14.020781193559225, 14.011129573359003, 13.997781579392322, 13.980057679894774, 14.015082822741613, 14.01475429311345, 13.983292470337686, 13.98742395110344, 14.00390848341868, 14.021700028716884, 14.00422165262, 13.989389383706168, 14.009545428773864, 13.98148368023919, 13.983000357485997, 14.019866244143005, 14.016727708735308, 13.99445368832483, 14.006458338306784, 13.986526744084133, 14.000676884624813, 14.022663207045237, 14.016250812821758, 14.00506754403289, 14.027920506513492, 13.989984184611309, 13.987201721545162, 13.996136794365448, 14.007644665814942, 13.993478776007487, 13.97995691746596, 14.095190764790033, 14.102830141946212, 14.114768076684488, 14.101918699240352, 14.085029356597778, 14.091574536785057, 14.102440763696745, 14.11811166775762, 14.124804242528853, 14.115482102375246, 14.120514644015332, 14.105087423647229, 14.112431936596234, 14.084027667365316, 14.12346863535685, 14.127903535949745, 14.098993341045523, 14.081191464234074, 14.082935887712877, 14.092542374807092, 14.119338954222119, 14.109418959950228, 14.103561968579795, 14.09829501267685, 14.122458704042886, 14.082863877180086, 14.089832284967146, 14.106300941097835, 14.089173775602408, 14.109385244031344, 14.097558762545757, 14.125430757060196, 14.09264742083766, 14.08828489406975, 14.086037334506576, 14.08869095175331, 14.126153541761038, 14.102570014003762, 14.121631020857494, 14.102619999692413, 14.098329011865417, 14.081506297711172, 14.119721434415565, 14.101150730121278, 14.094324146301599, 14.129387026976843, 14.093519204003718, 14.080503499362427, 14.089412808953401, 14.107511972590759, 14.029930901, 14.054933, 14.079935099, 14.079935099, 14.079935099, 0, 180, -180, 200, -200, 0, 0, 0, 0}; -const int test_elevations[] = {1531, 1571, 1524, 1443, 1670, 1858, 1510, 1327, 1661, 1653, 1765, 1468, 1797, 1427, 1898, 1478, 1414, 1664, 1824, 1429, 1418, 1596, 1676, 1658, 1752, 1449, 1940, 1593, 1482, 1834, 1535, 1462, 1586, 1490, 1754, 1485, 1845, 1450, 1452, 1282, 1514, 1615, 1458, 1587, 1711, 1553, 1591, 1526, 1786, 1532, 1492, 1503, 1411, 1655, 1972, 1693, 1652, 1574, 1655, 1422, 1679, 1421, 1690, 1444, 1527, 1706, 1634, 1643, 1552, 1519, 1489, 1546, 1431, 1495, 1643, 1471, 1452, 1626, 1598, 1763, 1419, 1751, 1509, 1430, 1443, 1796, 1681, 1694, 1754, 1659, 1402, 1533, 1413, 1499, 1423, 1966, 1811, 1739, 1556, 1422, 1627, 1429, 1695, 1244, 1754, 1589, 1472, 1759, 1382, 1516, 1422, 1829, 1829, 1440, 1795, 1429, 1736, 1471, 1694, 1586, 1531, 1471, 1641, 1676, 1657, 1406, 1598, 1644, 1829, 1477, 1288, 1493, 1542, 1479, 1687, 1533, 1573, 1490, 1415, 1448, 1744, 1616, 1624, 1715, 1460, 1503, 1720, 1600, 1941, 1415, 1436, 1416, 1654, 1599, 1580, 1415, 1491, 1522, 1579, 1635, 1676, 1545, 1446, 1370, 1416, 1488, 1607, 1494, 1536, 1619, 1423, 1499, 1643, 1752, 1707, 1543, 1382, 1456, 1408, 1513, 1709, 1632, 1483, 1788, 1446, 1573, 1948, 1455, 1813, 1463, 1475, 1624, 1827, 1570, 1434, 1838, 1496, 1561, 1731, 1379, 1424, 1422, 1665, 1813, 1456, 1720, 1538, 1521, 1460, 1450, 1812, 1566, 1536, 1752, 1409, 1861, 1590, 1571, 1429, 1575, 1764, 1692, 1619, 1426, 1553, 1741, 1406, 1426, 1588, 1591, 1472, 1426, 1868, 1499, 1926, 1899, 1652, 1410, 1417, 1523, 1738, 1579, 1503, 1562, 1500, 1724, 1577, 1503, 1516, 1401, 1482, 1558, 1565, 1474, 1807, 1441, 1465, 1728, 1834, 1391, 1912, 1657, 1820, 1449, 1832, 1443, 1417, 1578, 1369, 1476, 1388, 1473, 1816, 1421, 1605, 1883, 1586, 1400, 1602, 1684, 1673, 1771, 1592, 1596, 1451, 1304, 1657, 1414, 1755, 1419, 1606, 1264, 1700, 1594, 1451, 1461, 1861, 1469, 1817, 1623, 1607, 1755, 1990, 1919, 1643, 1617, 1698, 1415, 1518, 1581, 1411, 1630, 1701, 1682, 1622, 1539, 1545, 1634, 1410, 1230, 1420, 1248, 1537, 1504, 1566, 1414, 1812, 1609, 1469, 1673, 1516, 1354, 1723, 1856, 1670, 1930, 1620, 1496, 1398, 1429, 1407, 1414, 1718, 1476, 1612, 1611, 1757, 1624, 1440, 1802, 1574, 1838, 1381, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000, -10000}; - -} -} // namespace elevationmap \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.h b/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.h deleted file mode 100644 index 5e7721cabe3e622cfb19ee258cc7564b11d70351..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/elevation_map_test_data.h +++ /dev/null @@ -1,49 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.803938 - -#pragma once - -#include <cstddef> - -namespace elevationmap -{ -namespace test -{ - -static constexpr size_t TEST_DATA_SIZE = 667; - -extern const double test_latitudes[TEST_DATA_SIZE]; -extern const double test_longitudes[TEST_DATA_SIZE]; -extern const int test_elevations[TEST_DATA_SIZE]; - -} -} // namespace elevationmap \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/lh_circles_test_data.h b/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/lh_circles_test_data.h deleted file mode 100644 index 9459331d1d097ca6f792697a3389a0dabda065ed..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/lh_circles_test_data.h +++ /dev/null @@ -1,54 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.804010 - -#pragma once - -#include <cstddef> - -namespace launchhazard -{ -namespace test -{ - -static constexpr size_t TEST_DATA_SIZE = 0; - -extern const double test_latitudes[TEST_DATA_SIZE]; -extern const double test_longitudes[TEST_DATA_SIZE]; - - - - -static const double* const test_distances[] = {}; -static const bool* const test_inside[] = {}; - -} -} // namespace elevationmap \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/lh_circles.h b/src/boards/DeathStack/ADA/DeploymentUtils/lh_circles.h deleted file mode 100644 index c79aee6e330f1c989c358d380599bf044f84864f..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/ADA/DeploymentUtils/lh_circles.h +++ /dev/null @@ -1,84 +0,0 @@ -/** - * lha_circles.h - * - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <cmath> - -namespace launchhazard -{ -double DEG2RAD = M_PI / 180.0; -double EARTH_RADIUS_SQUARED = 40560046730329; -/** - * Launch Hazard Circle - */ -struct LHCircle -{ - double center_lat; - double center_lon; - double radius2; - - /** - * Calculates the square of the distance between the provided coordinates - * and the center of the circle. - * Approximates the curvature of the Earth with a equirectangular - * projection: Works well only for distances up to a few km. - * - * @param lat Latitude of the point - * @param lon Longitude of the point - * @return Square of the distance to the center in meters - */ - double distance2(double lat, double lon) - { - lat = lat * DEG2RAD; - lon = lon * DEG2RAD; - - double lat0 = center_lat * DEG2RAD; - double lon0 = center_lon * DEG2RAD; - - double x = (lon - lon0)*cos((lat0+lat)/2); - double y = (lat - lat0); - - return (pow(x, 2) + pow(y,2))*EARTH_RADIUS_SQUARED; - } - - /** - * Checks wheter the provided coordinates fall inside the launch hazard - * circle. - * - * @param lat Latitude of the point - * @param lon Longitude of the point - * @return True if the provided coordinates are inside the circle - */ - bool isInside(double lat, double lon) - { - return distance2(lat, lon) < radius2; - } -}; - -// Load circles definitions -#include "generated/lh_circles_data.h" - -} // namespace launchhazard diff --git a/src/boards/DeathStack/AirBrakes/AirBrakes.scxml b/src/boards/DeathStack/AirBrakes/AirBrakes.scxml new file mode 100644 index 0000000000000000000000000000000000000000..714e798147460bc9a1f4b880730649d7453bc665 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/AirBrakes.scxml @@ -0,0 +1,38 @@ +<?xml version="1.0" encoding="UTF-8"?> +<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="idle" version="1.0"> + <state id="idle"> + <onentry>initServo()</onentry> + <transition event="FLIGHT_EVENTS.EV_LIFTOFF" target="shadowMode"/> + <transition event="ABK.EV_WIGGLE_SERVO" target="idle"> + wiggleServo() + </transition> + <transition event="ABK.EV_RESET_SERVO" target="idle"> + resetServo() + </transition> + <transition event="ABK.EV_TEST_ABK" target="testAirbrakes"/> + </state> + <state id="shadowMode"> + <onentry>postD(EV_SHADOW_MODE_TIMEOUT)</onentry> + <transition event="ABK.EV_SHADOW_MODE_TIMEOUT" target="enabled"/> + <transition event="ABK.EV_DISABLE_ABK" target="disabled"/> + </state> + <state id="enabled"> + <onentry>enableAirbrakes()</onentry> + <transition event="FLIGHT_EVENTS.EV_APOGEE" target="end"/> + <transition event="ABK.EV_DISABLE_ABK" target="disabled"/> + </state> + <state id="end"> + <onentry>setFullyClosed()</onentry> + </state> + <state id="disabled"> + <onentry>setFullyClosed()</onentry> + </state> + <state id="testAirbrakes"> + <onentry>enableAirbrakes()</onentry> + <onentry>incrementallyOpen()</onentry> + <onentry>postD(ABK.EV_TEST_TIMEOUT)</onentry> + <onexit>incrementallyCloe()</onexit> + <onexit>disableAirbrakes()</onexit> + <transition event="ABK.EV_TEST_TIMEOUT" target="idle"/> + </state> +</scxml> diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..db89e7b1c458f5524a11b023b4352b9b232a4453 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h @@ -0,0 +1,511 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <AirBrakes/AirBrakesData.h> +#include <AirBrakes/PID.h> +#include <AirBrakes/trajectories/Trajectory.h> +#include <Algorithm.h> +#include <LoggerService/LoggerService.h> +#include <ServoInterface.h> +#include <TimestampTimer.h> +#include <configs/AirBrakesConfig.h> +#include <diagnostic/PrintLogger.h> +#include <sensors/Sensor.h> + +#include <algorithm> +#include <type_traits> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL.h> +#endif + +/** + * Error and timing benchmark results: + * + * | float | double + * ========================================================== + * Max setpoint error: | 0.000122 | 0.000001 + * Max u error: | 0.001045 | 0.000052 + * Max deltas error: | 0.000000 | 0.000000 + * Max alpha error: | 0 | 0 + * Init time: | 0.179810 ms | 2.037333 ms + * Avg iter time: | 0.151057 | 1.065261 ms + * Total computation time: | 25.557320 ms | 181.001143 ms + */ + +using namespace DeathStackBoard::AirBrakesConfigs; + +namespace DeathStackBoard +{ + +template <class T> +class AirBrakesControlAlgorithm : public Algorithm +{ + +public: + AirBrakesControlAlgorithm(Sensor<T>& sensor, ServoInterface* actuator); + + float getEstimatedCd() { return ab_data.estimated_cd; } + + bool init() override { return true; } + + /** + * @brief This method starts the algorithm and chooses the trajectory the + * rocket will follow for reaching apogee. + */ + void begin(); + + /** + * @brief This method does a step if the algorithm is running, or sets the + * actuator to 0 otherwise + */ + void update() + { + if (running) + { + step(); + } +#ifdef HARDWARE_IN_THE_LOOP + else + { + HIL::getInstance()->send(0.0); + } +#endif + } + + /** + * @brief This method looks for nearest point in the current chosen + * trajectory and sends to the servoInterface the airbrakes degree + * according to the current rocket speed and the one in the prediction. + */ + void step() override; + + /** + * @brief Computes the alpha degree (in radiants) to be sent to the + * servoInterface in order to follow the choosen trajectory. If + * firstIteration is true then the nearest trajectory to the current + * rocket's coordinates is chosen. + * + * @param input Input data struct, containing z, vz, vMod and timestamp. + * @param firstIteration The flag indicating wheter the trajectory has + * already been choosen or not. + * + * @returns The alpha in radiants to be sent to the servoInterface + */ + float computeAlpha(T input, bool firstIteration); + + /** + * @brief Chooses the nearest point on the trajectory, considering all the + * trajectories. Given the rocket initial input, stores the nearest + * trajectory. + * + * @param z The altitude of the rocket + * @param vz The z component of the rocket's speed + */ + TrajectoryPoint chooseTrajectory(float z, float vz); + + /** + * @brief Starting from the second iteration selects the nearest point on + * the choosen trajectory. This method does not consider the Euclidean + * distance, but only the altitude (z) distance. + * + * @param z The altitude of the rocket + * @param vz The z component of the rocket's speed + */ + TrajectoryPoint getSetpoint(float z, float vz); + + /** + * @brief Update Pid internal input. The Pid output represents represents + * the drag force of the wind. + * + * @param z Current rocket's altitude + * @param vz The z component of the rocket's speed + * @param vMod Modulo of the rocket's speed + * @param rho The density of the air + * @param setpoint Nearest point to the current rocket input in the chosen + * trajectory + */ + float pidStep(float z, float vz, float vMod, float rho, + TrajectoryPoint setpoint); + + /** + * @brief Compute the necessary airbrakes surface to match the + * given the drag force from the Pid. The possible surface values are + * discretized in (S_MIN, S_MAX) with a + * step of S_STEP. For every possible deltaS the + * correspondig drag force is computed with @see getDrag method and the one + * that gives lowest error with respect to Pid output is returned. + * + * @param z The current altitude of the rocket + * @param vz The z component of the rocket's speed + * @param vMod Modulo of the rocket's speed + * @param rho The density of the air + * @param u The current pid value + */ + float getSurface(float z, float vz, float vMod, float rho, float u); + + /** + * @brief Maps the exposed airbrakes surface to servoInterface angle in + * degrees. + * + * @param s Airbrakes surface + */ + float getAlpha(float s); + + /** + * @param h The current altitude + * @return The density of air according to current altitude. + */ + float getRho(float h); + + /** + * @param vMod Modulo of the current rocket speed. + * @param z The z component of the rocket's speed + * @return The speed in Mach unit. + */ + float getMach(float vMod, float z); + + /** + * @param s Airbrakes surface + * @return The radial distance of the current airbrakes extension. + */ + float getExtension(float s); + + /** + * @param v Rocket's velocity module + * @param h The current altitude of the rocket + * @param s Airbrakes surface + * @return The drag force coefficient + */ + float getDrag(float v, float h, float s); + + /** + * @brief Log algorithm data structure + * + */ + void logAlgorithmData(T input, uint64_t t); + + /** + * @brief Log airbrakes data structure + */ + void logAirbrakesData(uint64_t t); + +private: + int indexMinVal = 0; + float alpha = 0; + uint64_t ts = 0; + Trajectory chosenTrajectory; + ServoInterface* actuator; + Sensor<T>& sensor; + PIController pid; + + AirBrakesAlgorithmData algo_data; + AirBrakesData ab_data; + + LoggerService& logger; + + std::is_same<float, decltype(std::declval<T>().z)> checkz; + std::is_same<float, decltype(std::declval<T>().vz)> checkvz; + std::is_same<float, decltype(std::declval<T>().vMod)> checkvMod; + std::is_same<uint64_t, decltype(std::declval<T>().timestamp)> + checktimestamp; +}; + +template <class T> +AirBrakesControlAlgorithm<T>::AirBrakesControlAlgorithm( + Sensor<T>& sensor, ServoInterface* actuator) + : actuator(actuator), sensor(sensor), pid(Kp, Ki), + logger(*(LoggerService::getInstance())) +{ +} + +template <class T> +void AirBrakesControlAlgorithm<T>::begin() +{ + if (running) + { + return; + } + + running = true; + + ts = (sensor.getLastSample()).timestamp; + + alpha = computeAlpha(sensor.getLastSample(), true); + actuator->set(alpha, true); +} + +template <class T> +void AirBrakesControlAlgorithm<T>::step() +{ + T input = sensor.getLastSample(); + + if (input.timestamp > ts) + { + ts = input.timestamp; + alpha = computeAlpha(input, false); + } + + actuator->set(alpha, true); + + uint64_t t = TimestampTimer::getTimestamp(); + logAlgorithmData(input, t); + logAirbrakesData(t); +} + +template <class T> +float AirBrakesControlAlgorithm<T>::computeAlpha(T input, bool firstIteration) +{ + float z = input.z; + float vz = input.vz; + float vMod = input.vMod; + float rho = getRho(z); + + TrajectoryPoint setpoint; + + if (firstIteration) + { + setpoint = chooseTrajectory(z, vz); + } + else + { + setpoint = getSetpoint(z, vz); + } + + float u = pidStep(z, vz, vMod, rho, setpoint); + float s = getSurface(z, vz, vMod, rho, u); + float alpha = getAlpha(s); + + return alpha; +} + +template <class T> +TrajectoryPoint AirBrakesControlAlgorithm<T>::chooseTrajectory(float z, + float vz) +{ + TrajectoryPoint currentPoint(z, vz); + + float bestMin = INFINITY; + + for (uint8_t trajectoryIndex = 0; trajectoryIndex < TOT_TRAJECTORIES; + trajectoryIndex++) + { + Trajectory trajectory(trajectoryIndex, S_MAX); + + for (uint32_t pointIndex = 0; pointIndex < trajectory.length(); + pointIndex++) + { + TrajectoryPoint ref = trajectory.get(pointIndex); + float distanceFromCurrentinput = + TrajectoryPoint::distance(ref, currentPoint); + + if (distanceFromCurrentinput < bestMin) + { + bestMin = distanceFromCurrentinput; + indexMinVal = pointIndex; + chosenTrajectory = trajectory; + } + } + } + + logger.log( + AirBrakesChosenTrajectory{chosenTrajectory.getTrajectoryIndex()}); + + PrintLogger log = Logging::getLogger("deathstack.fsm.abk"); + LOG_INFO(log, "Chosen trajectory : {:d} \n", + chosenTrajectory.getTrajectoryIndex()); + + TrajectoryPoint setpoint = chosenTrajectory.get(indexMinVal); + + return setpoint; +} + +template <class T> +TrajectoryPoint AirBrakesControlAlgorithm<T>::getSetpoint(float z, float vz) +{ + TrajectoryPoint currentPoint(z, vz); + float minDistance = INFINITY; + + uint32_t start = std::max(indexMinVal + START_INDEX_OFFSET, 0); + uint32_t end = chosenTrajectory.length(); + + for (uint32_t pointIndex = start; pointIndex < end; pointIndex++) + { + TrajectoryPoint ref = chosenTrajectory.get(pointIndex); + float distanceFromCurrentinput = + TrajectoryPoint::distance(ref, currentPoint); + if (distanceFromCurrentinput < minDistance) + { + minDistance = distanceFromCurrentinput; + indexMinVal = pointIndex; + } + } + + TrajectoryPoint setpoint = chosenTrajectory.get(indexMinVal); + + ab_data.setpoint_z = setpoint.getZ(); + ab_data.setpoint_vz = setpoint.getVz(); + + return setpoint; +} + +template <class T> +float AirBrakesControlAlgorithm<T>::pidStep(float z, float vz, float vMod, + float rho, TrajectoryPoint setpoint) +{ + // cd minimum if abk surface is 0 + float cd_min = getDrag(vMod, z, 0); + // cd maximum if abk surface is maximum + float cd_max = getDrag(vMod, z, S_MAX); + + float u_min = 0.5 * rho * cd_min * S0 * vz * vMod; + float u_max = 0.5 * rho * cd_max * S0 * vz * vMod; + + // get reference CD and control action, according to the chosen trajectory + float cd_ref = getDrag(vMod, z, chosenTrajectory.getRefSurface()); + float u_ref = 0.5 * rho * cd_ref * S0 * vz * vMod; + + float error = vz - setpoint.getVz(); + ab_data.pid_error = error; + + // update PI controller + float u = pid.update(error); + u = u + u_ref; + u = pid.antiWindUp(u, u_min, u_max); + + return u; +} + +template <class T> +float AirBrakesControlAlgorithm<T>::getSurface(float z, float vz, float vMod, + float rho, float u) +{ + float estimatedCd = 0; + float selectedS = 0; + float bestDu = INFINITY; + + for (float s = S_MIN; s < S_MAX + S_STEP; s += S_STEP) + { + float cd = getDrag(vMod, z, s); + float du = abs(u - (0.5 * rho * S0 * cd * vz * vMod)); + + if (du < bestDu) + { + bestDu = du; + selectedS = s; + estimatedCd = cd; + } + } + + ab_data.estimated_cd = estimatedCd; + + return selectedS; +} + +template <class T> +float AirBrakesControlAlgorithm<T>::getAlpha(float s) +{ + float alphaRadiants = + (-B_DELTAS + sqrt(powf(B_DELTAS, 2) + 4 * A_DELTAS * s)) / + (2 * A_DELTAS); + + float alphaDegrees = alphaRadiants * 180.0f / PI; + + return alphaDegrees; +} + +template <class T> +float AirBrakesControlAlgorithm<T>::getRho(float h) +{ + return RHO * expf(-h / Hn); +} + +template <class T> +float AirBrakesControlAlgorithm<T>::getMach(float vMod, float z) +{ + float c = Co + ALPHA * z; + return vMod / c; +} + +template <class T> +float AirBrakesControlAlgorithm<T>::getExtension(float s) +{ + return (-B + sqrtf(powf(B, 2) + 4 * A * s)) / (2 * A); +} + +template <class T> +float AirBrakesControlAlgorithm<T>::getDrag(float v, float h, float s) +{ + float x = getExtension(s); + + float mach = getMach(v, h); + + float powMach[7] = {1, + mach, + powf(mach, 2), + powf(mach, 3), + powf(mach, 4), + powf(mach, 5), + powf(mach, 6)}; + + return coeffs.n000 + coeffs.n100 * powMach[1] + coeffs.n200 * powMach[2] + + coeffs.n300 * powMach[3] + coeffs.n400 * powMach[4] + + coeffs.n500 * powMach[5] + coeffs.n600 * powMach[6] + + coeffs.n010 * x + coeffs.n020 * powf(x, 2) + + coeffs.n110 * x * powMach[1] + + coeffs.n120 * powf(x, 2) * powMach[1] + + coeffs.n210 * x * powMach[2] + + coeffs.n220 * powf(x, 2) * powMach[2] + + coeffs.n310 * x * powMach[3] + + coeffs.n320 * powf(x, 2) * powMach[3] + + coeffs.n410 * x * powMach[4] + + coeffs.n420 * powf(x, 2) * powMach[4] + + coeffs.n510 * x * powMach[5] + + coeffs.n520 * powf(x, 2) * powMach[5] + coeffs.n001 * h; +} + +template <class T> +void AirBrakesControlAlgorithm<T>::logAlgorithmData(T input, uint64_t t) +{ + AirBrakesAlgorithmData d; + d.timestamp = t; + d.z = input.z; + d.vz = input.vz; + d.vMod = input.vMod; + logger.log(d); +} + +template <class T> +void AirBrakesControlAlgorithm<T>::logAirbrakesData(uint64_t t) +{ + ab_data.timestamp = t; + ab_data.servo_position = actuator->getCurrentPosition(); + // estimated_cd set when computing the new alpha (in getDrag()) + // pid_error inserted when computing the new alpha (in pidStep()) + // setpoint z and vz inserted when computing the new setpoint (in + // getSetpoint()) + logger.log(ab_data); +} + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesController.h b/src/boards/DeathStack/AirBrakes/AirBrakesController.h new file mode 100644 index 0000000000000000000000000000000000000000..281e1644ea8cb91eff147bf68cdf1a177935e183 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/AirBrakesController.h @@ -0,0 +1,409 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <AirBrakes/AirBrakesAlgorithm.h> +#include <AirBrakes/AirBrakesData.h> +#include <AirBrakes/AirBrakesServo.h> +#include <System/StackLogger.h> +#include <TimestampTimer.h> +#include <configs/AirBrakesConfig.h> +#include <diagnostic/PrintLogger.h> +#include <events/EventBroker.h> +#include <events/Events.h> +#include <events/FSM.h> +#include <miosix.h> + +namespace DeathStackBoard +{ + +/** + * @brief AirBrakes state machine + */ +template <class T> +class AirBrakesController : public FSM<AirBrakesController<T>> +{ +public: + AirBrakesController(Sensor<T>& sensor, + ServoInterface* servo = new AirBrakesServo()); + ~AirBrakesController(); + + // Airbrakes FSM states + void state_initialization(const Event& ev); + void state_idle(const Event& ev); + void state_shadowMode(const Event& ev); + void state_enabled(const Event& ev); + void state_end(const Event& ev); + void state_disabled(const Event& ev); + void state_testAirbrakes(const Event& ev); + + /** + * @brief Update the airbrakes control algorithm + */ + void update(); + + void setAirBrakesPosition(float pos); + + float getEstimatedCd() { return algorithm.getEstimatedCd(); } + +private: + /** + * @brief Incrementally opens the servo with steps of 10° + */ + void incrementallyOpen(); + + /** + * @brief Incrementally closes the servo with steps of 10° + */ + void incrementallyClose(); + + void logStatus(AirBrakesControllerState state); + + AirBrakesControllerStatus status; + ServoInterface* servo; + AirBrakesControlAlgorithm<T> algorithm; + uint16_t ev_shadow_mode_timeout_id; + + PrintLogger log = Logging::getLogger("deathstack.fsm.arb"); +}; + +template <class T> +AirBrakesController<T>::AirBrakesController(Sensor<T>& sensor, + ServoInterface* servo) + : FSM<AirBrakesController<T>>( + &AirBrakesController<T>::state_initialization), + servo(servo), algorithm(sensor, servo) +{ + memset(&status, 0, sizeof(AirBrakesControllerStatus)); + sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker->subscribe(this, TOPIC_ABK); +} + +template <class T> +AirBrakesController<T>::~AirBrakesController() +{ + sEventBroker->unsubscribe(this); +} + +template <class T> +void AirBrakesController<T>::update() +{ + algorithm.update(); +} + +template <class T> +void AirBrakesController<T>::state_initialization(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + logStatus(AirBrakesControllerState::INITIALIZATION); + + servo->enable(); + servo->reset(); + + this->transition(&AirBrakesController<T>::state_idle); + break; + } + case EV_EXIT: + { + break; + } + + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::state_idle(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + logStatus(AirBrakesControllerState::IDLE); + + LOG_DEBUG(log, "Eentering state idle"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state idle"); + break; + } + case EV_LIFTOFF: + { + this->transition(&AirBrakesController<T>::state_shadowMode); + break; + } + case EV_WIGGLE_SERVO: + { + servo->selfTest(); + break; + } + case EV_RESET_SERVO: + { + servo->reset(); + break; + } + case EV_TEST_ABK: + { + this->transition(&AirBrakesController<T>::state_testAirbrakes); + break; + } + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::state_shadowMode(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + ev_shadow_mode_timeout_id = + sEventBroker->postDelayed<SHADOW_MODE_DURATION>( + Event{EV_SHADOW_MODE_TIMEOUT}, TOPIC_ABK); + + logStatus(AirBrakesControllerState::SHADOW_MODE); + + LOG_DEBUG(log, "Entering state shadow_mode"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state shadow_mode"); + break; + } + case EV_SHADOW_MODE_TIMEOUT: + { + this->transition(&AirBrakesController<T>::state_enabled); + break; + } + case EV_DISABLE_ABK: + { + this->transition(&AirBrakesController<T>::state_disabled); + break; + } + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::state_enabled(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + algorithm.begin(); + + logStatus(AirBrakesControllerState::ENABLED); + + LOG_DEBUG(log, "Entering state enabled"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state enabled"); + break; + } + case EV_APOGEE: + { + this->transition(&AirBrakesController<T>::state_end); + break; + } + case EV_DISABLE_ABK: + { + this->transition(&AirBrakesController<T>::state_disabled); + break; + } + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::state_end(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + algorithm.end(); + servo->reset(); + + logStatus(AirBrakesControllerState::END); + + LOG_DEBUG(log, "Entering state end"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state end"); + break; + } + + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::state_disabled(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + algorithm.end(); + servo->reset(); + + logStatus(AirBrakesControllerState::DISABLED); + + LOG_DEBUG(log, "Entering state disabled"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state disabled"); + break; + } + + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::state_testAirbrakes(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state test_airbrakes"); + + incrementallyOpen(); + miosix::Thread::sleep(1000); + incrementallyClose(); + miosix::Thread::sleep(1000); + servo->reset(); + + logStatus(AirBrakesControllerState::TEST_AEROBRAKES); + + sEventBroker->post(Event{EV_TEST_TIMEOUT}, TOPIC_ABK); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state test_airbrakes"); + break; + } + case EV_TEST_TIMEOUT: + { + this->transition(&AirBrakesController<T>::state_idle); + break; + } + default: + { + break; + } + } +} + +template <class T> +void AirBrakesController<T>::incrementallyOpen() +{ + // Equal steps of about 5° + const int STEPS_NUM = (servo->MAX_POS - servo->MIN_POS) / 10.0f; + const float INCREMENT_STEP = (servo->MAX_POS - servo->MIN_POS) / STEPS_NUM; + + float currentStep = servo->MIN_POS; + + for (auto i = 0; i < STEPS_NUM; i++) + { + LOG_DEBUG(log, "Servo position : {:.2f}", currentStep); + servo->set(currentStep); + currentStep += INCREMENT_STEP; + miosix::Thread::sleep(1000); + } + + servo->set(servo->MAX_POS); +} + +template <class T> +void AirBrakesController<T>::incrementallyClose() +{ + // Equal steps of about 5° + const int STEPS_NUM = (servo->MAX_POS - servo->MIN_POS) / 10.0f; + const float INCREMENT_STEP = (servo->MAX_POS - servo->MIN_POS) / STEPS_NUM; + + float currentStep = servo->MAX_POS; + + for (auto i = 0; i < STEPS_NUM; i++) + { + LOG_DEBUG(log, "Servo position : {:.2f}", currentStep); + servo->set(currentStep); + currentStep -= INCREMENT_STEP; + miosix::Thread::sleep(1000); + } + + servo->set(servo->MIN_POS); +} + +template <class T> +void AirBrakesController<T>::logStatus(AirBrakesControllerState state) +{ + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + + LoggerService::getInstance()->log(status); + + StackLogger::getInstance()->updateStack(THID_ABK_FSM); +} + +template <class T> +void AirBrakesController<T>::setAirBrakesPosition(float pos) +{ + servo->set(pos); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesData.h b/src/boards/DeathStack/AirBrakes/AirBrakesData.h new file mode 100644 index 0000000000000000000000000000000000000000..e3457b340797cd0579567cd92ce6739b3b4a41a0 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/AirBrakesData.h @@ -0,0 +1,115 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <stdint.h> + +#include <iostream> +#include <string> + +namespace DeathStackBoard +{ + +/** + * Enum defining the possibile FSM states. + */ +enum class AirBrakesControllerState : uint8_t +{ + INITIALIZATION = 0, + IDLE, + TEST_AEROBRAKES, + SHADOW_MODE, + ENABLED, + DISABLED, + END, +}; + +/** + * Structure defining the output of the control algorithm. + */ +struct AirBrakesData +{ + uint64_t timestamp; + float servo_position; + float estimated_cd; + float pid_error; + float setpoint_z; + float setpoint_vz; + + static std::string header() + { + return "timestamp,servo_position,estimated_cd,pid_error,setpoint_z,setpoint_vz\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << servo_position << "," << estimated_cd << "," + << pid_error << "," << setpoint_z << "," << setpoint_vz << "\n"; + } +}; + +/** + * Structure defining the overall controller status. + */ +struct AirBrakesControllerStatus +{ + uint64_t timestamp; + AirBrakesControllerState state; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +/** + * Structure to log the data relative to the airbrakes algorithm, including the + * chosen trajectory (when the airbrakes are enabled for the first time). + */ +struct AirBrakesAlgorithmData +{ + uint64_t timestamp; + float z; + float vz; + float vMod; + + static std::string header() { return "timestamp,z,vz,vMod\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << z << "," << vz << "," << vMod << "\n"; + } +}; + +struct AirBrakesChosenTrajectory +{ + uint8_t trajectory; + + static std::string header() { return "trajectory\n"; } + + void print(std::ostream& os) const { os << (int)trajectory << "\n"; } +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp b/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cfd5cb472d4092e859edf622b9ac7bbd1eaa0494 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp @@ -0,0 +1,111 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <AirBrakes/AirBrakesServo.h> + +namespace DeathStackBoard +{ + +AirBrakesServo::AirBrakesServo() + : ServoInterface(AB_SERVO_MIN_POS, AB_SERVO_MAX_POS) +{ +} + +AirBrakesServo::AirBrakesServo(float minPosition, float maxPosition) + : ServoInterface(minPosition, maxPosition) +{ +} + +AirBrakesServo::AirBrakesServo(float minPosition, float maxPosition, + float resetPosition) + : ServoInterface(minPosition, maxPosition, resetPosition) +{ +} + +AirBrakesServo::~AirBrakesServo() {} + +void AirBrakesServo::enable() +{ + servo.setMaxPulseWidth(2500); + servo.setMinPulseWidth(500); + servo.enable(AB_SERVO_PWM_CH); + servo.start(); +} + +void AirBrakesServo::disable() +{ + servo.stop(); + servo.disable(AB_SERVO_PWM_CH); +} + +void AirBrakesServo::selfTest() +{ + float base = (MAX_POS + RESET_POS) / 2; + float maxpos = base + AB_SERVO_WIGGLE_AMPLITUDE / 2; + float minpos = base - AB_SERVO_WIGGLE_AMPLITUDE / 2; + + set(base, true); + + for (int i = 0; i < 3; i++) + { + miosix::Thread::sleep(ABK_UPDATE_PERIOD + 100); + set(maxpos, true); + miosix::Thread::sleep(ABK_UPDATE_PERIOD + 100); + set(minpos, true); + } + + miosix::Thread::sleep(ABK_UPDATE_PERIOD); + reset(); +} + +void AirBrakesServo::setPosition(float angle) +{ + currentPosition = angle; + // map position to [0;1] interval for the servo driver + servo.setPosition(AirBrakesConfigs::AB_SERVO_PWM_CH, angle / 180.0f); + +#ifdef HARDWARE_IN_THE_LOOP + simulator->send(angle); +#endif +} + +float AirBrakesServo::preprocessPosition(float angle) +{ + angle = ServoInterface::preprocessPosition(angle); + + float rate = (angle - currentPosition) / ABK_UPDATE_PERIOD_SECONDS; + + if (rate > AB_SERVO_MAX_RATE) + { + angle = ABK_UPDATE_PERIOD_SECONDS * AB_SERVO_MAX_RATE + currentPosition; + } + else if (rate < AB_SERVO_MIN_RATE) + { + angle = ABK_UPDATE_PERIOD_SECONDS * AB_SERVO_MIN_RATE + currentPosition; + } + + angle = FILTER_COEFF * angle + (1 - FILTER_COEFF) * getCurrentPosition(); + + return angle; +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h new file mode 100644 index 0000000000000000000000000000000000000000..9e2a91c0dc2ccc898f37bf9011514bf3566138d7 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h @@ -0,0 +1,79 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <AirBrakes/AirBrakesData.h> +#include <LoggerService/LoggerService.h> +#include <ServoInterface.h> +#include <configs/AirBrakesConfig.h> +#include <drivers/servo/servo.h> +#include <miosix.h> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL.h> +#endif + +namespace DeathStackBoard +{ + +using namespace AirBrakesConfigs; + +class AirBrakesServo : public ServoInterface +{ +public: + AirBrakesServo(); + + AirBrakesServo(float minPosition, float maxPosition); + + AirBrakesServo(float minPosition, float maxPosition, float resetPosition); + + virtual ~AirBrakesServo(); + + void enable() override; + + void disable() override; + + /** + * @brief Perform wiggle around the middle point. + */ + void selfTest() override; + +private: + Servo servo{AirBrakesConfigs::AB_SERVO_TIMER}; + +#ifdef HARDWARE_IN_THE_LOOP + HIL *simulator = HIL::getInstance(); +#endif + +protected: + /** + * @brief Set servo position. + * + * @param angle servo position (in degrees) + */ + void setPosition(float angle) override; + + float preprocessPosition(float angle) override; +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/AirBrakes/PID.h b/src/boards/DeathStack/AirBrakes/PID.h new file mode 100644 index 0000000000000000000000000000000000000000..cefa076087394f0542f598483ff548d40d2c4c5d --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/PID.h @@ -0,0 +1,86 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace DeathStackBoard +{ + +/** + * @brief Proportional and integral controller with saturation. + * */ +class PIController +{ + +public: + PIController(float Kp, float Ki) : Kp(Kp), Ki(Ki) {} + ~PIController() {} + + /** + * @brief Update the PI internal state. + * */ + float update(float error) + { + + float p = Kp * error; + + if (!saturation) + { + i = i + Ki * error; + } + + float u = p + i; + + return u; + } + + /** + * @brief Anti-windup mechanism. + * */ + float antiWindUp(float u, float umin, float umax) + { + if (u < umin) + { + u = umin; + saturation = true; + } + else if (u > umax) + { + u = umax; + saturation = true; + } + else + { + saturation = false; + } + + return u; + } + +private: + const float Kp; // proportional factor + const float Ki; // integral factor + float i = 0; // integral contribution + bool saturation = false; +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/readme.md b/src/boards/DeathStack/AirBrakes/readme.md new file mode 100644 index 0000000000000000000000000000000000000000..ef63c80a1a898154e6d6b7e618b677c581064c7f --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/readme.md @@ -0,0 +1,12 @@ +```mermaid +stateDiagram-v2 + [*] --> initialization + initialization --> idle + idle --> shadow_mode : EV_LIFTOFF + idle --> test_airbrakes : EV_TEST_ABK + shadow_mode --> enabled : EV_SHADOW_MODE_TIMEOUT + shadow_mode --> disabled : EV_DISABLE_ABK + enabled --> end : EV_APOGEE + enabled --> disabled : EV_DISABLE_ABK + test_airbrakes --> idle : EV_TEST_TIMEOUT +``` \ No newline at end of file diff --git a/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h new file mode 100644 index 0000000000000000000000000000000000000000..62e6333180d413ecf8a5688ccc2ba916604dc54f --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h @@ -0,0 +1,1481 @@ +/* + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace DeathStackBoard +{ + +static const unsigned int TOT_TRAJECTORIES = 9; +static const unsigned int TRAJECTORY_MAX_LENGTH = 326; + +struct point_t +{ + float z; + float vz; +}; + +struct trajectory_t +{ + unsigned int length; + point_t data[TRAJECTORY_MAX_LENGTH]; +}; + +const trajectory_t TRAJECTORIES_DATA[TOT_TRAJECTORIES] = { + {326, + { + {1593.049160, 194.060201}, {1602.730437, 193.190887}, + {1612.368334, 192.324990}, {1621.963021, 191.462478}, + {1631.514666, 190.603319}, {1641.023436, 189.747482}, + {1650.489496, 188.894938}, {1659.913011, 188.045654}, + {1669.294142, 187.199601}, {1678.633051, 186.356751}, + {1687.929897, 185.517073}, {1697.184837, 184.680538}, + {1706.398029, 183.847118}, {1715.569626, 183.016786}, + {1724.699784, 182.189512}, {1733.788653, 181.365269}, + {1742.836386, 180.544031}, {1751.843131, 179.725770}, + {1760.809036, 178.910459}, {1769.734250, 178.098072}, + {1778.618916, 177.288583}, {1787.463180, 176.481966}, + {1796.267184, 175.678196}, {1805.031070, 174.877247}, + {1813.754978, 174.079094}, {1822.439048, 173.283713}, + {1831.083418, 172.491079}, {1839.688224, 171.701168}, + {1848.253603, 170.913956}, {1856.779687, 170.129419}, + {1865.266611, 169.347534}, {1873.714506, 168.568277}, + {1882.123504, 167.791625}, {1890.493733, 167.017556}, + {1898.825323, 166.246048}, {1907.118401, 165.477077}, + {1915.373094, 164.710621}, {1923.589526, 163.946660}, + {1931.767822, 163.185171}, {1939.908104, 162.426133}, + {1948.010496, 161.669524}, {1956.075117, 160.915324}, + {1964.102088, 160.163511}, {1972.091527, 159.414066}, + {1980.043553, 158.666967}, {1987.958282, 157.922195}, + {1995.835830, 157.179730}, {2003.676312, 156.439551}, + {2011.479842, 155.701640}, {2019.246532, 154.965976}, + {2026.976495, 154.232541}, {2034.669842, 153.501316}, + {2042.326682, 152.772281}, {2049.947124, 152.045419}, + {2057.531277, 151.320710}, {2065.079248, 150.598136}, + {2072.591144, 149.877680}, {2080.067069, 149.159322}, + {2087.507128, 148.443047}, {2094.911425, 147.728835}, + {2102.280063, 147.016669}, {2109.613143, 146.306533}, + {2116.910766, 145.598409}, {2124.173034, 144.892280}, + {2131.400044, 144.188130}, {2138.591896, 143.485941}, + {2145.748687, 142.785698}, {2152.870514, 142.087384}, + {2159.957473, 141.390983}, {2167.009659, 140.696479}, + {2174.027168, 140.003856}, {2181.010092, 139.313099}, + {2187.958524, 138.624191}, {2194.872557, 137.937119}, + {2201.752281, 137.251865}, {2208.597788, 136.568416}, + {2215.409168, 135.886757}, {2222.186508, 135.206872}, + {2228.929899, 134.528746}, {2235.639427, 133.852367}, + {2242.315179, 133.177718}, {2248.957241, 132.504786}, + {2255.565700, 131.833556}, {2262.140639, 131.164016}, + {2268.682143, 130.496150}, {2275.190296, 129.829945}, + {2281.665179, 129.165387}, {2288.106875, 128.502464}, + {2294.515466, 127.841161}, {2300.891032, 127.181465}, + {2307.233652, 126.523364}, {2313.543407, 125.866845}, + {2319.820376, 125.211893}, {2326.064636, 124.558498}, + {2332.276264, 123.906645}, {2338.455338, 123.256323}, + {2344.601935, 122.607519}, {2350.716128, 121.960221}, + {2356.797994, 121.314417}, {2362.847607, 120.670095}, + {2368.865040, 120.027242}, {2374.850367, 119.385847}, + {2380.803661, 118.745898}, {2386.724993, 118.107383}, + {2392.614435, 117.470291}, {2398.472057, 116.834611}, + {2404.297931, 116.200331}, {2410.092125, 115.567440}, + {2415.854709, 114.935926}, {2421.585752, 114.305779}, + {2427.285321, 113.676988}, {2432.953484, 113.049542}, + {2438.590309, 112.423430}, {2444.195861, 111.798642}, + {2449.770206, 111.175166}, {2455.313410, 110.552993}, + {2460.825537, 109.932112}, {2466.306653, 109.312513}, + {2471.756820, 108.694186}, {2477.176103, 108.077119}, + {2482.564564, 107.461305}, {2487.922265, 106.846732}, + {2493.249268, 106.233390}, {2498.545634, 105.621270}, + {2503.811425, 105.010363}, {2509.046701, 104.400658}, + {2514.251521, 103.792146}, {2519.425945, 103.184817}, + {2524.570032, 102.578663}, {2529.683840, 101.973673}, + {2534.767428, 101.369839}, {2539.820853, 100.767151}, + {2544.844171, 100.165601}, {2549.837441, 99.565179}, + {2554.800717, 98.965877}, {2559.734056, 98.367685}, + {2564.637513, 97.770595}, {2569.511143, 97.174599}, + {2574.355000, 96.579686}, {2579.169139, 95.985850}, + {2583.953612, 95.393081}, {2588.708473, 94.801370}, + {2593.433775, 94.210711}, {2598.129570, 93.621094}, + {2602.795911, 93.032510}, {2607.432847, 92.444953}, + {2612.040431, 91.858413}, {2616.618714, 91.272883}, + {2621.167745, 90.688355}, {2625.687574, 90.104821}, + {2630.178251, 89.522272}, {2634.639826, 88.940702}, + {2639.072346, 88.360102}, {2643.475860, 87.780465}, + {2647.850416, 87.201783}, {2652.196062, 86.624048}, + {2656.512844, 86.047254}, {2660.800811, 85.471392}, + {2665.060007, 84.896455}, {2669.290479, 84.322437}, + {2673.492273, 83.749328}, {2677.665434, 83.177124}, + {2681.810008, 82.605815}, {2685.926038, 82.035396}, + {2690.013570, 81.465859}, {2694.072646, 80.897197}, + {2698.103311, 80.329402}, {2702.105608, 79.762470}, + {2706.079579, 79.196391}, {2710.025268, 78.631160}, + {2713.942716, 78.066770}, {2717.831966, 77.503214}, + {2721.693058, 76.940486}, {2725.526035, 76.378578}, + {2729.330937, 75.817485}, {2733.107804, 75.257199}, + {2736.856676, 74.697715}, {2740.577595, 74.139026}, + {2744.270599, 73.581125}, {2747.935727, 73.024007}, + {2751.573019, 72.467665}, {2755.182513, 71.912093}, + {2758.764247, 71.357284}, {2762.318260, 70.803233}, + {2765.844589, 70.249934}, {2769.343272, 69.697379}, + {2772.814346, 69.145565}, {2776.257847, 68.594483}, + {2779.673812, 68.044130}, {2783.062278, 67.494498}, + {2786.423280, 66.945582}, {2789.756854, 66.397376}, + {2793.063035, 65.849875}, {2796.341859, 65.303072}, + {2799.593360, 64.756962}, {2802.817572, 64.211540}, + {2806.014531, 63.666800}, {2809.184269, 63.122737}, + {2812.326821, 62.579344}, {2815.442220, 62.036617}, + {2818.530500, 61.494550}, {2821.591692, 60.953138}, + {2824.625829, 60.412375}, {2827.632945, 59.872256}, + {2830.613071, 59.332776}, {2833.566239, 58.793931}, + {2836.492480, 58.255713}, {2839.391826, 57.718120}, + {2842.264307, 57.181144}, {2845.109955, 56.644782}, + {2847.928801, 56.109029}, {2850.720873, 55.573879}, + {2853.486204, 55.039327}, {2856.224821, 54.505370}, + {2858.936755, 53.972001}, {2861.622036, 53.439216}, + {2864.280691, 52.907010}, {2866.912751, 52.375379}, + {2869.518243, 51.844318}, {2872.097197, 51.313822}, + {2874.649640, 50.783887}, {2877.175600, 50.254507}, + {2879.675104, 49.725679}, {2882.148181, 49.197399}, + {2884.594858, 48.669661}, {2887.015161, 48.142461}, + {2889.409117, 47.615794}, {2891.776753, 47.089658}, + {2894.118096, 46.564046}, {2896.433171, 46.038955}, + {2898.722004, 45.514381}, {2900.984622, 44.990320}, + {2903.221049, 44.466766}, {2905.431311, 43.943717}, + {2907.615433, 43.421168}, {2909.773440, 42.899115}, + {2911.905357, 42.377554}, {2914.011208, 41.856482}, + {2916.091017, 41.335893}, {2918.144809, 40.815785}, + {2920.172608, 40.296153}, {2922.174436, 39.776994}, + {2924.150319, 39.258303}, {2926.100278, 38.740078}, + {2928.024338, 38.222314}, {2929.922521, 37.705008}, + {2931.794850, 37.188156}, {2933.641348, 36.671755}, + {2935.462037, 36.155800}, {2937.256939, 35.640289}, + {2939.026077, 35.125219}, {2940.769472, 34.610584}, + {2942.487146, 34.096383}, {2944.179121, 33.582612}, + {2945.845418, 33.069267}, {2947.486058, 32.556346}, + {2949.101063, 32.043844}, {2950.690453, 31.531760}, + {2952.254249, 31.020088}, {2953.792472, 30.508828}, + {2955.305142, 29.997975}, {2956.792280, 29.487526}, + {2958.253905, 28.977479}, {2959.690038, 28.467830}, + {2961.100698, 27.958577}, {2962.485905, 27.449716}, + {2963.845679, 26.941245}, {2965.180039, 26.433161}, + {2966.489005, 25.925461}, {2967.772595, 25.418142}, + {2969.030829, 24.911203}, {2970.263725, 24.404639}, + {2971.471302, 23.898449}, {2972.653579, 23.392630}, + {2973.810574, 22.887180}, {2974.942306, 22.382095}, + {2976.048793, 21.877374}, {2977.130052, 21.373014}, + {2978.186103, 20.869013}, {2979.216963, 20.365368}, + {2980.222649, 19.862078}, {2981.203179, 19.359139}, + {2982.158571, 18.856550}, {2983.088843, 18.354309}, + {2983.994011, 17.852413}, {2984.874093, 17.350861}, + {2985.729106, 16.849650}, {2986.559066, 16.348778}, + {2987.363992, 15.848244}, {2988.143899, 15.348046}, + {2988.898805, 14.848181}, {2989.628725, 14.348648}, + {2990.333678, 13.849446}, {2991.013678, 13.350571}, + {2991.668743, 12.852024}, {2992.298889, 12.353802}, + {2992.904131, 11.855903}, {2993.484487, 11.358326}, + {2994.039972, 10.861069}, {2994.570602, 10.364132}, + {2995.076393, 9.867512}, {2995.557361, 9.371208}, + {2996.013522, 8.875220}, {2996.444891, 8.379544}, + {2996.851484, 7.884181}, {2997.233317, 7.389129}, + {2997.590405, 6.894388}, {2997.922763, 6.399955}, + {2998.230408, 5.905829}, {2998.513354, 5.412011}, + {2998.771617, 4.918498}, {2999.005211, 4.425290}, + {2999.214153, 3.932386}, {2999.398457, 3.439785}, + {2999.558139, 2.947486}, {2999.693214, 2.455489}, + {2999.803696, 1.963792}, {2999.889600, 1.472395}, + {2999.950943, 0.981298}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, + {326, + { + {1571.427664, 200.447502}, {1581.426025, 199.486921}, + {1591.376478, 198.531210}, {1601.279266, 197.580306}, + {1611.134627, 196.634152}, {1620.942798, 195.692687}, + {1630.704012, 194.755855}, {1640.418498, 193.823600}, + {1650.086485, 192.895866}, {1659.708196, 191.972599}, + {1669.283855, 191.053745}, {1678.813680, 190.139253}, + {1688.297888, 189.229069}, {1697.736693, 188.323145}, + {1707.130308, 187.421430}, {1716.478940, 186.523875}, + {1725.782798, 185.630432}, {1735.042085, 184.741053}, + {1744.257004, 183.855692}, {1753.427754, 182.974302}, + {1762.554532, 182.096840}, {1771.637535, 181.223260}, + {1780.676954, 180.353518}, {1789.672981, 179.487571}, + {1798.625805, 178.625378}, {1807.535612, 177.766896}, + {1816.402586, 176.912084}, {1825.226911, 176.060901}, + {1834.008766, 175.213308}, {1842.748330, 174.369265}, + {1851.445780, 173.528733}, {1860.101291, 172.691675}, + {1868.715034, 171.858053}, {1877.287181, 171.027829}, + {1885.817901, 170.200968}, {1894.307361, 169.377432}, + {1902.755726, 168.557187}, {1911.163161, 167.740197}, + {1919.529826, 166.926428}, {1927.855883, 166.115846}, + {1936.141490, 165.308418}, {1944.386803, 164.504110}, + {1952.591978, 163.702889}, {1960.757168, 162.904724}, + {1968.882526, 162.109583}, {1976.968202, 161.317434}, + {1985.014344, 160.528247}, {1993.021099, 159.741991}, + {2000.988615, 158.958635}, {2008.917035, 158.178151}, + {2016.806501, 157.400509}, {2024.657156, 156.625681}, + {2032.469139, 155.853637}, {2040.242589, 155.084350}, + {2047.977642, 154.317791}, {2055.674435, 153.553934}, + {2063.333102, 152.792752}, {2070.953777, 152.034217}, + {2078.536590, 151.278304}, {2086.081672, 150.524986}, + {2093.589153, 149.774238}, {2101.059159, 149.026035}, + {2108.491819, 148.280351}, {2115.887257, 147.537161}, + {2123.245597, 146.796443}, {2130.566962, 146.058170}, + {2137.851475, 145.322320}, {2145.099254, 144.588870}, + {2152.310421, 143.857795}, {2159.485093, 143.129073}, + {2166.623386, 142.402682}, {2173.725418, 141.678598}, + {2180.791303, 140.956801}, {2187.821155, 140.237268}, + {2194.815086, 139.519978}, {2201.773209, 138.804910}, + {2208.695632, 138.092041}, {2215.582467, 137.381353}, + {2222.433822, 136.672824}, {2229.249803, 135.966433}, + {2236.030518, 135.262162}, {2242.776072, 134.559990}, + {2249.486569, 133.859898}, {2256.162113, 133.161865}, + {2262.802806, 132.465875}, {2269.408751, 131.771906}, + {2275.980047, 131.079941}, {2282.516795, 130.389962}, + {2289.019092, 129.701949}, {2295.487038, 129.015886}, + {2301.920729, 128.331753}, {2308.320262, 127.649535}, + {2314.685730, 126.969213}, {2321.017230, 126.290769}, + {2327.314854, 125.614188}, {2333.578695, 124.939452}, + {2339.808845, 124.266545}, {2346.005395, 123.595450}, + {2352.168435, 122.926150}, {2358.298054, 122.258631}, + {2364.394342, 121.592875}, {2370.457385, 120.928867}, + {2376.487272, 120.266592}, {2382.484087, 119.606034}, + {2388.447918, 118.947178}, {2394.378847, 118.290009}, + {2400.276960, 117.634511}, {2406.142340, 116.980671}, + {2411.975069, 116.328474}, {2417.775228, 115.677905}, + {2423.542899, 115.028949}, {2429.278163, 114.381594}, + {2434.981098, 113.735824}, {2440.651785, 113.091626}, + {2446.290300, 112.448986}, {2451.896722, 111.807890}, + {2457.471127, 111.168326}, {2463.013592, 110.530280}, + {2468.524193, 109.893739}, {2474.003004, 109.258689}, + {2479.450099, 108.625119}, {2484.865552, 107.993014}, + {2490.249436, 107.362363}, {2495.601824, 106.733152}, + {2500.922787, 106.105371}, {2506.212397, 105.479005}, + {2511.470723, 104.854044}, {2516.697836, 104.230475}, + {2521.893805, 103.608286}, {2527.058699, 102.987466}, + {2532.192586, 102.368002}, {2537.295533, 101.749884}, + {2542.367607, 101.133099}, {2547.408876, 100.517636}, + {2552.419404, 99.903485}, {2557.399257, 99.290634}, + {2562.348499, 98.679071}, {2567.267196, 98.068787}, + {2572.155410, 97.459770}, {2577.013204, 96.852009}, + {2581.840642, 96.245495}, {2586.637784, 95.640215}, + {2591.404694, 95.036160}, {2596.141431, 94.433320}, + {2600.848056, 93.831684}, {2605.524629, 93.231243}, + {2610.171210, 92.631985}, {2614.787857, 92.033901}, + {2619.374629, 91.436982}, {2623.931584, 90.841217}, + {2628.458779, 90.246597}, {2632.956272, 89.653112}, + {2637.424119, 89.060752}, {2641.862375, 88.469508}, + {2646.271097, 87.879371}, {2650.650340, 87.290331}, + {2655.000158, 86.702380}, {2659.320605, 86.115508}, + {2663.611735, 85.529705}, {2667.873602, 84.944964}, + {2672.106258, 84.361274}, {2676.309755, 83.778628}, + {2680.484146, 83.197017}, {2684.629483, 82.616431}, + {2688.745815, 82.036863}, {2692.833194, 81.458303}, + {2696.891670, 80.880743}, {2700.921293, 80.304176}, + {2704.922112, 79.728592}, {2708.894177, 79.153983}, + {2712.837535, 78.580341}, {2716.752235, 78.007658}, + {2720.638324, 77.435926}, {2724.495851, 76.865137}, + {2728.324862, 76.295283}, {2732.125403, 75.726357}, + {2735.897520, 75.158349}, {2739.641260, 74.591253}, + {2743.356668, 74.025062}, {2747.043789, 73.459766}, + {2750.702667, 72.895360}, {2754.333347, 72.331835}, + {2757.935872, 71.769184}, {2761.510287, 71.207400}, + {2765.056634, 70.646474}, {2768.574956, 70.086401}, + {2772.065295, 69.527173}, {2775.527694, 68.968783}, + {2778.962194, 68.411223}, {2782.368837, 67.854487}, + {2785.747663, 67.298568}, {2789.098714, 66.743459}, + {2792.422029, 66.189153}, {2795.717649, 65.635643}, + {2798.985613, 65.082923}, {2802.225961, 64.530985}, + {2805.438731, 63.979824}, {2808.623963, 63.429432}, + {2811.781694, 62.879804}, {2814.911962, 62.330932}, + {2818.014805, 61.782810}, {2821.090262, 61.235433}, + {2824.138367, 60.688793}, {2827.159159, 60.142885}, + {2830.152674, 59.597701}, {2833.118947, 59.053237}, + {2836.058015, 58.509486}, {2838.969913, 57.966441}, + {2841.854677, 57.424098}, {2844.712341, 56.882449}, + {2847.542939, 56.341490}, {2850.346507, 55.801213}, + {2853.123077, 55.261614}, {2855.872685, 54.722687}, + {2858.595363, 54.184425}, {2861.291144, 53.646824}, + {2863.960061, 53.109878}, {2866.602148, 52.573581}, + {2869.217436, 52.037927}, {2871.805957, 51.502911}, + {2874.367743, 50.968528}, {2876.902825, 50.434773}, + {2879.411235, 49.901639}, {2881.893004, 49.369123}, + {2884.348163, 48.837218}, {2886.776741, 48.305919}, + {2889.178770, 47.775221}, {2891.554278, 47.245120}, + {2893.903297, 46.715609}, {2896.225854, 46.186685}, + {2898.521980, 45.658342}, {2900.791703, 45.130575}, + {2903.035051, 44.603379}, {2905.252055, 44.076750}, + {2907.442740, 43.550683}, {2909.607137, 43.025172}, + {2911.745271, 42.500215}, {2913.857172, 41.975805}, + {2915.942866, 41.451938}, {2918.002379, 40.928610}, + {2920.035740, 40.405815}, {2922.042974, 39.883551}, + {2924.024108, 39.361812}, {2925.979168, 38.840594}, + {2927.908180, 38.319892}, {2929.811170, 37.799703}, + {2931.688163, 37.280022}, {2933.539185, 36.760845}, + {2935.364260, 36.242168}, {2937.163414, 35.723987}, + {2938.936671, 35.206297}, {2940.684056, 34.689095}, + {2942.405593, 34.172377}, {2944.101306, 33.656139}, + {2945.771219, 33.140377}, {2947.415355, 32.625087}, + {2949.033739, 32.110265}, {2950.626393, 31.595908}, + {2952.193342, 31.082013}, {2953.734606, 30.568575}, + {2955.250210, 30.055590}, {2956.740176, 29.543056}, + {2958.204527, 29.030969}, {2959.643284, 28.519325}, + {2961.056471, 28.008122}, {2962.444108, 27.497355}, + {2963.806217, 26.987021}, {2965.142820, 26.477118}, + {2966.453939, 25.967641}, {2967.739595, 25.458588}, + {2968.999809, 24.949956}, {2970.234601, 24.441742}, + {2971.443993, 23.933942}, {2972.628006, 23.426553}, + {2973.786659, 22.919574}, {2974.919973, 22.413000}, + {2976.027969, 21.906829}, {2977.110666, 21.401058}, + {2978.168085, 20.895685}, {2979.200244, 20.390707}, + {2980.207165, 19.886120}, {2981.188866, 19.381924}, + {2982.145367, 18.878114}, {2983.076687, 18.374689}, + {2983.982846, 17.871646}, {2984.863861, 17.368983}, + {2985.719753, 16.866697}, {2986.550540, 16.364786}, + {2987.356241, 15.863248}, {2988.136874, 15.362080}, + {2988.892458, 14.861281}, {2989.623012, 14.360848}, + {2990.328552, 13.860780}, {2991.009099, 13.361074}, + {2991.664669, 12.861728}, {2992.295280, 12.362741}, + {2992.900952, 11.864110}, {2993.481700, 11.365834}, + {2994.037544, 10.867911}, {2994.568500, 10.370340}, + {2995.074587, 9.873118}, {2995.555821, 9.376244}, + {2996.012220, 8.879717}, {2996.443801, 8.383535}, + {2996.850582, 7.887696}, {2997.232579, 7.392199}, + {2997.589810, 6.897044}, {2997.922292, 6.402227}, + {2998.230042, 5.907749}, {2998.513075, 5.413608}, + {2998.771411, 4.919803}, {2999.005064, 4.426332}, + {2999.214052, 3.933195}, {2999.398392, 3.440391}, + {2999.558100, 2.947918}, {2999.693192, 2.455776}, + {2999.803685, 1.963964}, {2999.889597, 1.472481}, + {2999.950942, 0.981327}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, + {326, + { + {1545.999607, 208.312373}, {1556.388129, 207.228482}, + {1566.722635, 206.151756}, {1577.003480, 205.082076}, + {1587.231016, 204.019330}, {1597.405584, 202.963407}, + {1607.527524, 201.914199}, {1617.597169, 200.871601}, + {1627.614847, 199.835511}, {1637.580880, 198.805827}, + {1647.495587, 197.782452}, {1657.359281, 196.765289}, + {1667.172269, 195.754246}, {1676.934856, 194.749231}, + {1686.647341, 193.750154}, {1696.310018, 192.756927}, + {1705.923178, 191.769466}, {1715.487106, 190.787685}, + {1725.002086, 189.811503}, {1734.468395, 188.840841}, + {1743.886306, 187.875618}, {1753.256090, 186.915758}, + {1762.578014, 185.961186}, {1771.852339, 185.011827}, + {1781.079325, 184.067610}, {1790.259227, 183.128463}, + {1799.392297, 182.194316}, {1808.478782, 181.265102}, + {1817.518928, 180.340753}, {1826.512977, 179.421204}, + {1835.461167, 178.506391}, {1844.363733, 177.596250}, + {1853.220907, 176.690719}, {1862.032919, 175.789738}, + {1870.799993, 174.893247}, {1879.522354, 174.001188}, + {1888.200222, 173.113502}, {1896.833813, 172.230134}, + {1905.423342, 171.351029}, {1913.969021, 170.476131}, + {1922.471059, 169.605387}, {1930.929662, 168.738746}, + {1939.345034, 167.876154}, {1947.717377, 167.017563}, + {1956.046889, 166.162921}, {1964.333767, 165.312181}, + {1972.578204, 164.465294}, {1980.780391, 163.622212}, + {1988.940519, 162.782890}, {1997.058773, 161.947282}, + {2005.135339, 161.115342}, {2013.170398, 160.287028}, + {2021.164131, 159.462295}, {2029.116716, 158.641101}, + {2037.028329, 157.823404}, {2044.899143, 157.009162}, + {2052.729330, 156.198336}, {2060.519061, 155.390885}, + {2068.268502, 154.586770}, {2075.977820, 153.785952}, + {2083.647179, 152.988393}, {2091.276740, 152.194057}, + {2098.866664, 151.402905}, {2106.417109, 150.614902}, + {2113.928232, 149.830013}, {2121.400188, 149.048201}, + {2128.833128, 148.269433}, {2136.227206, 147.493675}, + {2143.582570, 146.720892}, {2150.899369, 145.951053}, + {2158.177748, 145.184124}, {2165.417853, 144.420073}, + {2172.619827, 143.658869}, {2179.783811, 142.900482}, + {2186.909945, 142.144879}, {2193.998367, 141.392032}, + {2201.049216, 140.641910}, {2208.062626, 139.894485}, + {2215.038731, 139.149727}, {2221.977665, 138.407608}, + {2228.879557, 137.668100}, {2235.744539, 136.931176}, + {2242.572739, 136.196807}, {2249.364283, 135.464968}, + {2256.119298, 134.735632}, {2262.837908, 134.008774}, + {2269.520237, 133.284366}, {2276.166405, 132.562384}, + {2282.776535, 131.842803}, {2289.350745, 131.125599}, + {2295.889154, 130.410746}, {2302.391878, 129.698221}, + {2308.859034, 128.988001}, {2315.290735, 128.280062}, + {2321.687096, 127.574381}, {2328.048229, 126.870935}, + {2334.374245, 126.169702}, {2340.665254, 125.470659}, + {2346.921365, 124.773786}, {2353.142686, 124.079060}, + {2359.329324, 123.386461}, {2365.481385, 122.695966}, + {2371.598973, 122.007556}, {2377.682192, 121.321210}, + {2383.731145, 120.636908}, {2389.745934, 119.954630}, + {2395.726658, 119.274356}, {2401.673419, 118.596067}, + {2407.586314, 117.919743}, {2413.465442, 117.245367}, + {2419.310899, 116.572918}, {2425.122781, 115.902378}, + {2430.901184, 115.233730}, {2436.646201, 114.566955}, + {2442.357926, 113.902035}, {2448.036451, 113.238953}, + {2453.681867, 112.577692}, {2459.294265, 111.918233}, + {2464.873735, 111.260561}, {2470.420365, 110.604657}, + {2475.934244, 109.950507}, {2481.415459, 109.298093}, + {2486.864097, 108.647398}, {2492.280242, 107.998408}, + {2497.663980, 107.351106}, {2503.015394, 106.705477}, + {2508.334569, 106.061505}, {2513.621586, 105.419174}, + {2518.876527, 104.778470}, {2524.099473, 104.139377}, + {2529.290504, 103.501881}, {2534.449701, 102.865968}, + {2539.577140, 102.231621}, {2544.672902, 101.598828}, + {2549.737062, 100.967574}, {2554.769697, 100.337846}, + {2559.770884, 99.709628}, {2564.740697, 99.082907}, + {2569.679212, 98.457671}, {2574.586501, 97.833905}, + {2579.462639, 97.211596}, {2584.307697, 96.590730}, + {2589.121748, 95.971296}, {2593.904862, 95.353280}, + {2598.657111, 94.736668}, {2603.378564, 94.121450}, + {2608.069290, 93.507611}, {2612.729359, 92.895140}, + {2617.358838, 92.284024}, {2621.957795, 91.674252}, + {2626.526297, 91.065811}, {2631.064409, 90.458689}, + {2635.572198, 89.852875}, {2640.049729, 89.248357}, + {2644.497066, 88.645123}, {2648.914273, 88.043162}, + {2653.301414, 87.442462}, {2657.658551, 86.843013}, + {2661.985746, 86.244803}, {2666.283062, 85.647821}, + {2670.550558, 85.052057}, {2674.788297, 84.457499}, + {2678.996338, 83.864137}, {2683.174741, 83.271961}, + {2687.323564, 82.680959}, {2691.442866, 82.091121}, + {2695.532705, 81.502438}, {2699.593138, 80.914898}, + {2703.624223, 80.328492}, {2707.626015, 79.743210}, + {2711.598572, 79.159041}, {2715.541947, 78.575976}, + {2719.456197, 77.994006}, {2723.341375, 77.413120}, + {2727.197536, 76.833309}, {2731.024732, 76.254563}, + {2734.823018, 75.676874}, {2738.592446, 75.100231}, + {2742.333067, 74.524626}, {2746.044934, 73.950049}, + {2749.728098, 73.376491}, {2753.382609, 72.803944}, + {2757.008517, 72.232398}, {2760.605873, 71.661845}, + {2764.174726, 71.092275}, {2767.715125, 70.523680}, + {2771.227118, 69.956052}, {2774.710754, 69.389382}, + {2778.166080, 68.823662}, {2781.593144, 68.258882}, + {2784.991992, 67.695035}, {2788.362671, 67.132113}, + {2791.705226, 66.570108}, {2795.019704, 66.009010}, + {2798.306150, 65.448814}, {2801.564608, 64.889509}, + {2804.795123, 64.331089}, {2807.997739, 63.773546}, + {2811.172499, 63.216871}, {2814.319447, 62.661058}, + {2817.438626, 62.106099}, {2820.530078, 61.551985}, + {2823.593846, 60.998711}, {2826.629970, 60.446267}, + {2829.638493, 59.894647}, {2832.619455, 59.343845}, + {2835.572898, 58.793851}, {2838.498860, 58.244660}, + {2841.397383, 57.696263}, {2844.268506, 57.148655}, + {2847.112269, 56.601828}, {2849.928709, 56.055776}, + {2852.717865, 55.510490}, {2855.479777, 54.965965}, + {2858.214481, 54.422195}, {2860.922015, 53.879171}, + {2863.602416, 53.336888}, {2866.255722, 52.795339}, + {2868.881968, 52.254518}, {2871.481192, 51.714418}, + {2874.053428, 51.175033}, {2876.598713, 50.636356}, + {2879.117081, 50.098381}, {2881.608568, 49.561103}, + {2884.073209, 49.024514}, {2886.511037, 48.488610}, + {2888.922087, 47.953383}, {2891.306392, 47.418828}, + {2893.663986, 46.884939}, {2895.994902, 46.351710}, + {2898.299173, 45.819135}, {2900.576832, 45.287209}, + {2902.827910, 44.755926}, {2905.052441, 44.225281}, + {2907.250454, 43.695266}, {2909.421983, 43.165879}, + {2911.567058, 42.637112}, {2913.685709, 42.108960}, + {2915.777969, 41.581418}, {2917.843866, 41.054481}, + {2919.883432, 40.528143}, {2921.896695, 40.002400}, + {2923.883687, 39.477246}, {2925.844435, 38.952676}, + {2927.778969, 38.428685}, {2929.687318, 37.905268}, + {2931.569510, 37.382421}, {2933.425574, 36.860137}, + {2935.255538, 36.338413}, {2937.059429, 35.817244}, + {2938.837276, 35.296626}, {2940.589105, 34.776552}, + {2942.314944, 34.257019}, {2944.014820, 33.738023}, + {2945.688760, 33.219559}, {2947.336790, 32.701622}, + {2948.958935, 32.184208}, {2950.555223, 31.667313}, + {2952.125680, 31.150933}, {2953.670329, 30.635062}, + {2955.189198, 30.119698}, {2956.682312, 29.604836}, + {2958.149695, 29.090473}, {2959.591371, 28.576603}, + {2961.007367, 28.063223}, {2962.397706, 27.550329}, + {2963.762412, 27.037918}, {2965.101510, 26.525986}, + {2966.415022, 26.014528}, {2967.702974, 25.503542}, + {2968.965388, 24.993023}, {2970.202288, 24.482969}, + {2971.413697, 23.973375}, {2972.599637, 23.464238}, + {2973.760132, 22.955556}, {2974.895204, 22.447323}, + {2976.004875, 21.939538}, {2977.089169, 21.432197}, + {2978.148106, 20.925297}, {2979.181709, 20.418834}, + {2980.190000, 19.912806}, {2981.173001, 19.407210}, + {2982.130732, 18.902042}, {2983.063216, 18.397301}, + {2983.970473, 17.892982}, {2984.852524, 17.389084}, + {2985.709392, 16.885603}, {2986.541095, 16.382537}, + {2987.347656, 15.879883}, {2988.129094, 15.377639}, + {2988.885430, 14.875803}, {2989.616684, 14.374371}, + {2990.322877, 13.873341}, {2991.004028, 13.372712}, + {2991.660158, 12.872480}, {2992.291286, 12.372644}, + {2992.897432, 11.873202}, {2993.478616, 11.374150}, + {2994.034857, 10.875488}, {2994.566175, 10.377214}, + {2995.072588, 9.879325}, {2995.554117, 9.381819}, + {2996.010780, 8.884695}, {2996.442596, 8.387951}, + {2996.849584, 7.891585}, {2997.231764, 7.395596}, + {2997.589153, 6.899982}, {2997.921771, 6.404741}, + {2998.229637, 5.909872}, {2998.512768, 5.415374}, + {2998.771183, 4.921245}, {2999.004901, 4.427484}, + {2999.213941, 3.934090}, {2999.398319, 3.441061}, + {2999.558056, 2.948396}, {2999.693168, 2.456094}, + {2999.803674, 1.964155}, {2999.889593, 1.472577}, + {2999.950941, 0.981359}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, + {326, + { + {1515.719825, 218.256161}, {1526.601153, 216.996985}, + {1537.419800, 215.748884}, {1548.176313, 214.511622}, + {1558.871227, 213.284974}, {1569.505070, 212.068722}, + {1580.078354, 210.862653}, {1590.591585, 209.666562}, + {1601.045255, 208.480250}, {1611.439849, 207.303523}, + {1621.775842, 206.136195}, {1632.053699, 204.978082}, + {1642.273876, 203.829010}, {1652.436822, 202.688805}, + {1662.542975, 201.557302}, {1672.592766, 200.434339}, + {1682.586618, 199.319757}, {1692.524947, 198.213403}, + {1702.408160, 197.115129}, {1712.236658, 196.024788}, + {1722.010834, 194.942240}, {1731.731074, 193.867348}, + {1741.397757, 192.799976}, {1751.011256, 191.739994}, + {1760.571938, 190.687275}, {1770.080162, 189.641694}, + {1779.536282, 188.603131}, {1788.940647, 187.571468}, + {1798.293599, 186.546588}, {1807.595473, 185.528381}, + {1816.846601, 184.516735}, {1826.047308, 183.511544}, + {1835.197914, 182.512704}, {1844.298734, 181.520111}, + {1853.350079, 180.533667}, {1862.352252, 179.553274}, + {1871.305555, 178.578836}, {1880.210283, 177.610261}, + {1889.066726, 176.647458}, {1897.875171, 175.690337}, + {1906.635899, 174.738812}, {1915.349189, 173.792797}, + {1924.015315, 172.852209}, {1932.634544, 171.916968}, + {1941.207143, 170.986992}, {1949.733373, 170.062205}, + {1958.213491, 169.142530}, {1966.647752, 168.227892}, + {1975.036405, 167.318219}, {1983.379696, 166.413438}, + {1991.677869, 165.513480}, {1999.931163, 164.618276}, + {2008.139814, 163.727758}, {2016.304054, 162.841861}, + {2024.424114, 161.960520}, {2032.500219, 161.083673}, + {2040.532592, 160.211256}, {2048.521453, 159.343209}, + {2056.467020, 158.479473}, {2064.369507, 157.619989}, + {2072.229124, 156.764700}, {2080.046080, 155.913549}, + {2087.820581, 155.066483}, {2095.552829, 154.223445}, + {2103.243025, 153.384384}, {2110.891366, 152.549248}, + {2118.498047, 151.717985}, {2126.063260, 150.890546}, + {2133.587196, 150.066880}, {2141.070041, 149.246941}, + {2148.511982, 148.430680}, {2155.913200, 147.618051}, + {2163.273877, 146.809008}, {2170.594189, 146.003507}, + {2177.874315, 145.201503}, {2185.114426, 144.402953}, + {2192.314695, 143.607815}, {2199.475292, 142.816047}, + {2206.596383, 142.027607}, {2213.678135, 141.242455}, + {2220.720710, 140.460552}, {2227.724270, 139.681859}, + {2234.688975, 138.906338}, {2241.614982, 138.133950}, + {2248.502448, 137.364659}, {2255.351525, 136.598428}, + {2262.162366, 135.835222}, {2268.935122, 135.075005}, + {2275.669940, 134.317742}, {2282.366969, 133.563400}, + {2289.026353, 132.811945}, {2295.648235, 132.063345}, + {2302.232758, 131.317565}, {2308.780061, 130.574576}, + {2315.290284, 129.834344}, {2321.763564, 129.096840}, + {2328.200035, 128.362032}, {2334.599834, 127.629891}, + {2340.963091, 126.900388}, {2347.289938, 126.173492}, + {2353.580504, 125.449176}, {2359.834919, 124.727411}, + {2366.053308, 124.008169}, {2372.235798, 123.291424}, + {2378.382512, 122.577147}, {2384.493574, 121.865313}, + {2390.569104, 121.155896}, {2396.609223, 120.448869}, + {2402.614050, 119.744207}, {2408.583702, 119.041885}, + {2414.518297, 118.341878}, {2420.417948, 117.644162}, + {2426.282769, 116.948713}, {2432.112875, 116.255507}, + {2437.908376, 115.564522}, {2443.669382, 114.875733}, + {2449.396003, 114.189118}, {2455.088348, 113.504654}, + {2460.746522, 112.822321}, {2466.370632, 112.142095}, + {2471.960784, 111.463955}, {2477.517079, 110.787880}, + {2483.039623, 110.113850}, {2488.528515, 109.441842}, + {2493.983857, 108.771838}, {2499.405748, 108.103817}, + {2504.794288, 107.437758}, {2510.149573, 106.773643}, + {2515.471700, 106.111452}, {2520.760766, 105.451166}, + {2526.016864, 104.792765}, {2531.240089, 104.136232}, + {2536.430533, 103.481547}, {2541.588289, 102.828693}, + {2546.713448, 102.177652}, {2551.806099, 101.528405}, + {2556.866333, 100.880936}, {2561.894237, 100.235227}, + {2566.889899, 99.591261}, {2571.853406, 98.949020}, + {2576.784844, 98.308489}, {2581.684297, 97.669650}, + {2586.551851, 97.032488}, {2591.387588, 96.396985}, + {2596.191591, 95.763127}, {2600.963941, 95.130898}, + {2605.704721, 94.500281}, {2610.414009, 93.871261}, + {2615.091886, 93.243823}, {2619.738431, 92.617953}, + {2624.353720, 91.993634}, {2628.937833, 91.370853}, + {2633.490844, 90.749595}, {2638.012830, 90.129845}, + {2642.503866, 89.511589}, {2646.964026, 88.894813}, + {2651.393384, 88.279503}, {2655.792012, 87.665645}, + {2660.159984, 87.053225}, {2664.497370, 86.442231}, + {2668.804242, 85.832648}, {2673.080670, 85.224463}, + {2677.326723, 84.617663}, {2681.542471, 84.012236}, + {2685.727981, 83.408168}, {2689.883321, 82.805447}, + {2694.008559, 82.204060}, {2698.103760, 81.603994}, + {2702.168991, 81.005238}, {2706.204316, 80.407779}, + {2710.209801, 79.811605}, {2714.185509, 79.216704}, + {2718.131503, 78.623065}, {2722.047846, 78.030675}, + {2725.934601, 77.439523}, {2729.791829, 76.849597}, + {2733.619591, 76.260886}, {2737.417948, 75.673380}, + {2741.186959, 75.087066}, {2744.926684, 74.501934}, + {2748.637182, 73.917972}, {2752.318510, 73.335170}, + {2755.970728, 72.753518}, {2759.593891, 72.173004}, + {2763.188056, 71.593618}, {2766.753280, 71.015349}, + {2770.289619, 70.438188}, {2773.797127, 69.862124}, + {2777.275858, 69.287146}, {2780.725868, 68.713246}, + {2784.147210, 68.140413}, {2787.539936, 67.568636}, + {2790.904099, 66.997907}, {2794.239753, 66.428216}, + {2797.546947, 65.859553}, {2800.825733, 65.291908}, + {2804.076163, 64.725273}, {2807.298286, 64.159637}, + {2810.492151, 63.594992}, {2813.657809, 63.031329}, + {2816.795308, 62.468639}, {2819.904697, 61.906912}, + {2822.986024, 61.346139}, {2826.039335, 60.786313}, + {2829.064678, 60.227424}, {2832.062100, 59.669464}, + {2835.031648, 59.112423}, {2837.973366, 58.556294}, + {2840.887300, 58.001068}, {2843.773495, 57.446738}, + {2846.631996, 56.893293}, {2849.462846, 56.340728}, + {2852.266090, 55.789032}, {2855.041771, 55.238200}, + {2857.789931, 54.688221}, {2860.510614, 54.139089}, + {2863.203861, 53.590796}, {2865.869715, 53.043335}, + {2868.508215, 52.496696}, {2871.119405, 51.950874}, + {2873.703323, 51.405860}, {2876.260011, 50.861647}, + {2878.789508, 50.318228}, {2881.291853, 49.775596}, + {2883.767087, 49.233743}, {2886.215247, 48.692661}, + {2888.636372, 48.152345}, {2891.030500, 47.612788}, + {2893.397669, 47.073981}, {2895.737917, 46.535918}, + {2898.051280, 45.998594}, {2900.337795, 45.462000}, + {2902.597498, 44.926130}, {2904.830425, 44.390978}, + {2907.036613, 43.856537}, {2909.216097, 43.322800}, + {2911.368911, 42.789762}, {2913.495090, 42.257416}, + {2915.594670, 41.725756}, {2917.667683, 41.194775}, + {2919.714164, 40.664468}, {2921.734146, 40.134829}, + {2923.727663, 39.605850}, {2925.694748, 39.077528}, + {2927.635432, 38.549855}, {2929.549749, 38.022826}, + {2931.437731, 37.496435}, {2933.299409, 36.970677}, + {2935.134814, 36.445546}, {2936.943979, 35.921036}, + {2938.726933, 35.397143}, {2940.483708, 34.873860}, + {2942.214334, 34.351182}, {2943.918842, 33.829105}, + {2945.597260, 33.307622}, {2947.249619, 32.786729}, + {2948.875947, 32.266421}, {2950.476275, 31.746693}, + {2952.050631, 31.227539}, {2953.599043, 30.708955}, + {2955.121541, 30.190936}, {2956.618151, 29.673477}, + {2958.088902, 29.156574}, {2959.533822, 28.640222}, + {2960.952938, 28.124417}, {2962.346277, 27.609153}, + {2963.713867, 27.094427}, {2965.055733, 26.580234}, + {2966.371904, 26.066570}, {2967.662404, 25.553431}, + {2968.927260, 25.040812}, {2970.166498, 24.528709}, + {2971.380143, 24.017119}, {2972.568222, 23.506037}, + {2973.730760, 22.995460}, {2974.867781, 22.485383}, + {2975.979310, 21.975803}, {2977.065373, 21.466715}, + {2978.125994, 20.958118}, {2979.161197, 20.450006}, + {2980.171007, 19.942376}, {2981.155447, 19.435225}, + {2982.114541, 18.928549}, {2983.048314, 18.422346}, + {2983.956788, 17.916611}, {2984.839986, 17.411342}, + {2985.697933, 16.906535}, {2986.530651, 16.402188}, + {2987.338163, 15.898296}, {2988.120492, 15.394859}, + {2988.877661, 14.891871}, {2989.609691, 14.389331}, + {2990.316605, 13.887237}, {2990.998425, 13.385584}, + {2991.655174, 12.884371}, {2992.286873, 12.383595}, + {2992.893545, 11.883253}, {2993.475209, 11.383343}, + {2994.031890, 10.883863}, {2994.563606, 10.384810}, + {2995.070381, 9.886183}, {2995.552235, 9.387978}, + {2996.009190, 8.890194}, {2996.441265, 8.392829}, + {2996.848483, 7.895880}, {2997.230864, 7.399347}, + {2997.588428, 6.903226}, {2997.921196, 6.407516}, + {2998.229190, 5.912216}, {2998.512428, 5.417323}, + {2998.770932, 4.922837}, {2999.004722, 4.428755}, + {2999.213818, 3.935076}, {2999.398240, 3.441799}, + {2999.558008, 2.948923}, {2999.693142, 2.456445}, + {2999.803662, 1.964365}, {2999.889588, 1.472681}, + {2999.950940, 0.981393}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, + {320, + { + {1566.011690, 220.000000}, {1566.011690, 220.000000}, + {1569.150545, 219.597939}, {1580.095631, 218.205494}, + {1590.971467, 216.827938}, {1601.778788, 215.464900}, + {1612.518311, 214.116021}, {1623.190735, 212.780959}, + {1633.796744, 211.459381}, {1644.337002, 210.150968}, + {1654.812162, 208.855409}, {1665.222857, 207.572408}, + {1675.569709, 206.301675}, {1685.853325, 205.042934}, + {1696.074296, 203.795913}, {1706.233202, 202.560353}, + {1716.330611, 201.336002}, {1726.367077, 200.122617}, + {1736.343141, 198.919960}, {1746.259335, 197.727803}, + {1756.116178, 196.545924}, {1765.914179, 195.374110}, + {1775.653836, 194.212150}, {1785.335636, 193.059843}, + {1794.960056, 191.916993}, {1804.527566, 190.783409}, + {1814.038624, 189.658907}, {1823.493680, 188.543307}, + {1832.893173, 187.436435}, {1842.237537, 186.338121}, + {1851.527195, 185.248201}, {1860.762563, 184.166515}, + {1869.944049, 183.092906}, {1879.072052, 182.027224}, + {1888.146965, 180.969321}, {1897.169175, 179.919055}, + {1906.139058, 178.876284}, {1915.056987, 177.840874}, + {1923.923327, 176.812693}, {1932.738434, 175.791611}, + {1941.502662, 174.777503}, {1950.216356, 173.770248}, + {1958.879855, 172.769724}, {1967.493494, 171.775818}, + {1976.057599, 170.788414}, {1984.572495, 169.807403}, + {1993.038497, 168.832678}, {2001.455917, 167.864133}, + {2009.825062, 166.901665}, {2018.146233, 165.945175}, + {2026.419727, 164.994565}, {2034.645834, 164.049739}, + {2042.824843, 163.110605}, {2050.957035, 162.177072}, + {2059.042688, 161.249051}, {2067.082075, 160.326455}, + {2075.075467, 159.409199}, {2083.023127, 158.497202}, + {2090.925316, 157.590381}, {2098.782292, 156.688658}, + {2106.594308, 155.791955}, {2114.361611, 154.900197}, + {2122.084449, 154.013310}, {2129.763062, 153.131221}, + {2137.397689, 152.253861}, {2144.988565, 151.381158}, + {2152.535920, 150.513047}, {2160.039983, 149.649459}, + {2167.500978, 148.790332}, {2174.919126, 147.935600}, + {2182.294646, 147.085202}, {2189.627753, 146.239077}, + {2196.918659, 145.397165}, {2204.167573, 144.559407}, + {2211.374702, 143.725747}, {2218.540249, 142.896128}, + {2225.664415, 142.070495}, {2232.747397, 141.248794}, + {2239.789391, 140.430973}, {2246.790590, 139.616979}, + {2253.751183, 138.806762}, {2260.671359, 138.000272}, + {2267.551302, 137.197459}, {2274.391196, 136.398277}, + {2281.191220, 135.602677}, {2287.951552, 134.810615}, + {2294.672368, 134.022043}, {2301.353842, 133.236919}, + {2307.996145, 132.455198}, {2314.599446, 131.676837}, + {2321.163912, 130.901795}, {2327.689708, 130.130030}, + {2334.176996, 129.361501}, {2340.625938, 128.596169}, + {2347.036692, 127.833995}, {2353.409415, 127.074939}, + {2359.744263, 126.318964}, {2366.041388, 125.566033}, + {2372.300941, 124.816110}, {2378.523073, 124.069158}, + {2384.707930, 123.325142}, {2390.855660, 122.584028}, + {2396.966405, 121.845781}, {2403.040309, 121.110368}, + {2409.077512, 120.377756}, {2415.078153, 119.647912}, + {2421.042371, 118.920804}, {2426.970302, 118.196401}, + {2432.862078, 117.474672}, {2438.717835, 116.755587}, + {2444.537702, 116.039115}, {2450.321811, 115.325227}, + {2456.070289, 114.613894}, {2461.783263, 113.905087}, + {2467.460860, 113.198779}, {2473.103203, 112.494941}, + {2478.710415, 111.793546}, {2484.282618, 111.094567}, + {2489.819932, 110.397978}, {2495.322475, 109.703752}, + {2500.790365, 109.011864}, {2506.223719, 108.322289}, + {2511.622651, 107.635001}, {2516.987276, 106.949975}, + {2522.317705, 106.267188}, {2527.614050, 105.586616}, + {2532.876421, 104.908234}, {2538.104928, 104.232020}, + {2543.299677, 103.557951}, {2548.460776, 102.886004}, + {2553.588330, 102.216156}, {2558.682443, 101.548385}, + {2563.743220, 100.882671}, {2568.770761, 100.218991}, + {2573.765169, 99.557324}, {2578.726543, 98.897649}, + {2583.654983, 98.239946}, {2588.550587, 97.584194}, + {2593.413451, 96.930373}, {2598.243672, 96.278464}, + {2603.041345, 95.628446}, {2607.806563, 94.980301}, + {2612.539421, 94.334009}, {2617.240010, 93.689552}, + {2621.908422, 93.046910}, {2626.544746, 92.406066}, + {2631.149073, 91.767001}, {2635.721490, 91.129698}, + {2640.262086, 90.494138}, {2644.770947, 89.860304}, + {2649.248159, 89.228179}, {2653.693807, 88.597746}, + {2658.107976, 87.968987}, {2662.490747, 87.341886}, + {2666.842205, 86.716427}, {2671.162431, 86.092594}, + {2675.451505, 85.470369}, {2679.709507, 84.849737}, + {2683.936518, 84.230683}, {2688.132615, 83.613191}, + {2692.297876, 82.997245}, {2696.432378, 82.382830}, + {2700.536197, 81.769931}, {2704.609408, 81.158534}, + {2708.652087, 80.548623}, {2712.664307, 79.940183}, + {2716.646142, 79.333202}, {2720.597664, 78.727663}, + {2724.518944, 78.123554}, {2728.410054, 77.520859}, + {2732.271065, 76.919566}, {2736.102046, 76.319661}, + {2739.903065, 75.721130}, {2743.674193, 75.123959}, + {2747.415495, 74.528137}, {2751.127040, 73.933648}, + {2754.808893, 73.340482}, {2758.461121, 72.748624}, + {2762.083788, 72.158062}, {2765.676959, 71.568783}, + {2769.240698, 70.980776}, {2772.775068, 70.394027}, + {2776.280132, 69.808525}, {2779.755951, 69.224258}, + {2783.202588, 68.641213}, {2786.620103, 68.059379}, + {2790.008556, 67.478744}, {2793.368007, 66.899297}, + {2796.698515, 66.321026}, {2800.000139, 65.743920}, + {2803.272936, 65.167967}, {2806.516964, 64.593158}, + {2809.732280, 64.019479}, {2812.918940, 63.446921}, + {2816.077000, 62.875473}, {2819.206515, 62.305125}, + {2822.307539, 61.735865}, {2825.380128, 61.167683}, + {2828.424334, 60.600569}, {2831.440212, 60.034513}, + {2834.427812, 59.469504}, {2837.387188, 58.905533}, + {2840.318391, 58.342589}, {2843.221472, 57.780663}, + {2846.096482, 57.219745}, {2848.943472, 56.659825}, + {2851.762490, 56.100893}, {2854.553585, 55.542941}, + {2857.316808, 54.985959}, {2860.052205, 54.429938}, + {2862.759826, 53.874868}, {2865.439716, 53.320741}, + {2868.091923, 52.767547}, {2870.716494, 52.215277}, + {2873.313474, 51.663923}, {2875.882908, 51.113476}, + {2878.424844, 50.563928}, {2880.939324, 50.015269}, + {2883.426393, 49.467492}, {2885.886094, 48.920587}, + {2888.318473, 48.374548}, {2890.723571, 47.829364}, + {2893.101430, 47.285030}, {2895.452095, 46.741535}, + {2897.775605, 46.198873}, {2900.072003, 45.657036}, + {2902.341329, 45.116016}, {2904.583624, 44.575804}, + {2906.798929, 44.036394}, {2908.987284, 43.497779}, + {2911.148727, 42.959949}, {2913.283298, 42.422899}, + {2915.391036, 41.886621}, {2917.471979, 41.351107}, + {2919.526166, 40.816351}, {2921.553633, 40.282346}, + {2923.554419, 39.749084}, {2925.528560, 39.216558}, + {2927.476093, 38.684763}, {2929.397054, 38.153691}, + {2931.291480, 37.623335}, {2933.159406, 37.093689}, + {2935.000866, 36.564747}, {2936.815898, 36.036501}, + {2938.604534, 35.508946}, {2940.366809, 34.982076}, + {2942.102758, 34.455883}, {2943.812415, 33.930363}, + {2945.495811, 33.405509}, {2947.152982, 32.881315}, + {2948.783959, 32.357775}, {2950.388776, 31.834884}, + {2951.967464, 31.312635}, {2953.520055, 30.791023}, + {2955.046582, 30.270043}, {2956.547075, 29.749689}, + {2958.021566, 29.229955}, {2959.470086, 28.710836}, + {2960.892665, 28.192327}, {2962.289334, 27.674423}, + {2963.660122, 27.157119}, {2965.005061, 26.640408}, + {2966.324178, 26.124288}, {2967.617504, 25.608751}, + {2968.885068, 25.093795}, {2970.126898, 24.579414}, + {2971.343023, 24.065603}, {2972.533472, 23.552357}, + {2973.698273, 23.039673}, {2974.837453, 22.527546}, + {2975.951041, 22.015970}, {2977.039064, 21.504943}, + {2978.101549, 20.994460}, {2979.138524, 20.484516}, + {2980.150014, 19.975108}, {2981.136048, 19.466231}, + {2982.096651, 18.957882}, {2983.031849, 18.450056}, + {2983.941669, 17.942750}, {2984.826137, 17.435961}, + {2985.685278, 16.929684}, {2986.519118, 16.423915}, + {2987.327682, 15.918653}, {2988.110996, 15.413892}, + {2988.869084, 14.909630}, {2989.601971, 14.405863}, + {2990.309682, 13.902589}, {2990.992242, 13.399803}, + {2991.649675, 12.897504}, {2992.282005, 12.395687}, + {2992.889256, 11.894351}, {2993.471452, 11.393492}, + {2994.028617, 10.893107}, {2994.560774, 10.393194}, + {2995.067948, 9.893750}, {2995.550161, 9.394773}, + {2996.007437, 8.896260}, {2996.439799, 8.398209}, + {2996.847269, 7.900617}, {2997.229872, 7.403482}, + {2997.587629, 6.906802}, {2997.920563, 6.410575}, + {2998.228697, 5.914799}, {2998.512054, 5.419471}, + {2998.770656, 4.924591}, {2999.004524, 4.430155}, + {2999.213682, 3.936163}, {2999.398152, 3.442613}, + {2999.557955, 2.949503}, {2999.693113, 2.456831}, + {2999.803649, 1.964596}, {2999.889583, 1.472797}, + {2999.950939, 0.981432}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, + {311, + { + {1630.901244, 220.000000}, {1630.901244, 220.000000}, + {1632.171627, 219.820698}, {1643.124367, 218.288876}, + {1654.001002, 216.776551}, {1664.802495, 215.283164}, + {1675.529779, 213.808175}, {1686.183760, 212.351069}, + {1696.765320, 210.911351}, {1707.275318, 209.488545}, + {1717.714586, 208.082196}, {1728.083938, 206.691863}, + {1738.384162, 205.317125}, {1748.616030, 203.957575}, + {1758.780290, 202.612822}, {1768.877672, 201.282489}, + {1778.908890, 199.966214}, {1788.874637, 198.663647}, + {1798.775589, 197.374450}, {1808.612408, 196.098298}, + {1818.385737, 194.834876}, {1828.096206, 193.583881}, + {1837.744429, 192.345021}, {1847.331004, 191.118012}, + {1856.856519, 189.902580}, {1866.321545, 188.698462}, + {1875.726642, 187.505401}, {1885.072355, 186.323150}, + {1894.359221, 185.151469}, {1903.587761, 183.990127}, + {1912.758487, 182.838899}, {1921.871898, 181.697567}, + {1930.928485, 180.565921}, {1939.928727, 179.443757}, + {1948.873093, 178.330876}, {1957.762042, 177.227087}, + {1966.596024, 176.132203}, {1975.375481, 175.046043}, + {1984.100843, 173.968432}, {1992.772533, 172.899201}, + {2001.390968, 171.838183}, {2009.956553, 170.785217}, + {2018.469687, 169.740149}, {2026.930761, 168.702826}, + {2035.340160, 167.673101}, {2043.698258, 166.650831}, + {2052.005426, 165.635876}, {2060.262025, 164.628101}, + {2068.468412, 163.627375}, {2076.624936, 162.633569}, + {2084.731939, 161.646558}, {2092.789758, 160.666220}, + {2100.798725, 159.692438}, {2108.759163, 158.725097}, + {2116.671392, 157.764083}, {2124.535727, 156.809288}, + {2132.352474, 155.860605}, {2140.121937, 154.917931}, + {2147.844415, 153.981163}, {2155.520199, 153.050204}, + {2163.149578, 152.124957}, {2170.732835, 151.205328}, + {2178.270249, 150.291226}, {2185.762094, 149.382561}, + {2193.208639, 148.479245}, {2200.610150, 147.581194}, + {2207.966888, 146.688325}, {2215.279110, 145.800556}, + {2222.547069, 144.917808}, {2229.771014, 144.040003}, + {2236.951191, 143.167066}, {2244.087841, 142.298922}, + {2251.181201, 141.435500}, {2258.231507, 140.576728}, + {2265.238989, 139.722537}, {2272.203873, 138.872860}, + {2279.126386, 138.027631}, {2286.006746, 137.186785}, + {2292.845172, 136.350258}, {2299.641878, 135.517988}, + {2306.397076, 134.689915}, {2313.110973, 133.865980}, + {2319.783776, 133.046124}, {2326.415686, 132.230291}, + {2333.006904, 131.418424}, {2339.557626, 130.610469}, + {2346.068047, 129.806372}, {2352.538359, 129.006081}, + {2358.968749, 128.209545}, {2365.359406, 127.416713}, + {2371.710512, 126.627535}, {2378.022250, 125.841964}, + {2384.294798, 125.059951}, {2390.528333, 124.281451}, + {2396.723029, 123.506416}, {2402.879060, 122.734804}, + {2408.996594, 121.966568}, {2415.075800, 121.201667}, + {2421.116843, 120.440058}, {2427.119887, 119.681699}, + {2433.085093, 118.926549}, {2439.012621, 118.174568}, + {2444.902628, 117.425717}, {2450.755270, 116.679957}, + {2456.570700, 115.937249}, {2462.349070, 115.197558}, + {2468.090530, 114.460845}, {2473.795228, 113.727074}, + {2479.463310, 112.996211}, {2485.094921, 112.268219}, + {2490.690203, 111.543066}, {2496.249298, 110.820717}, + {2501.772344, 110.101138}, {2507.259480, 109.384298}, + {2512.710842, 108.670164}, {2518.126563, 107.958704}, + {2523.506778, 107.249888}, {2528.851618, 106.543685}, + {2534.161211, 105.840064}, {2539.435688, 105.138996}, + {2544.675174, 104.440451}, {2549.879795, 103.744402}, + {2555.049676, 103.050820}, {2560.184938, 102.359676}, + {2565.285704, 101.670943}, {2570.352092, 100.984595}, + {2575.384222, 100.300604}, {2580.382211, 99.618945}, + {2585.346174, 98.939591}, {2590.276227, 98.262517}, + {2595.172482, 97.587697}, {2600.035052, 96.915108}, + {2604.864048, 96.244724}, {2609.659579, 95.576521}, + {2614.421754, 94.910476}, {2619.150680, 94.246565}, + {2623.846464, 93.584765}, {2628.509209, 92.925054}, + {2633.139021, 92.267408}, {2637.736001, 91.611806}, + {2642.300252, 90.958225}, {2646.831873, 90.306645}, + {2651.330966, 89.657043}, {2655.797627, 89.009399}, + {2660.231954, 88.363692}, {2664.634044, 87.719901}, + {2669.003991, 87.078007}, {2673.341891, 86.437988}, + {2677.647837, 85.799827}, {2681.921920, 85.163502}, + {2686.164232, 84.528995}, {2690.374864, 83.896287}, + {2694.553905, 83.265358}, {2698.701444, 82.636192}, + {2702.817568, 82.008768}, {2706.902364, 81.383070}, + {2710.955918, 80.759078}, {2714.978314, 80.136777}, + {2718.969637, 79.516147}, {2722.929970, 78.897172}, + {2726.859396, 78.279836}, {2730.757994, 77.664120}, + {2734.625848, 77.050008}, {2738.463035, 76.437485}, + {2742.269635, 75.826533}, {2746.045727, 75.217136}, + {2749.791387, 74.609280}, {2753.506693, 74.002947}, + {2757.191720, 73.398123}, {2760.846543, 72.794792}, + {2764.471236, 72.192939}, {2768.065873, 71.592549}, + {2771.630527, 70.993607}, {2775.165270, 70.396098}, + {2778.670172, 69.800009}, {2782.145306, 69.205324}, + {2785.590740, 68.612029}, {2789.006543, 68.020111}, + {2792.392785, 67.429555}, {2795.749532, 66.840348}, + {2799.076853, 66.252476}, {2802.374813, 65.665926}, + {2805.643478, 65.080684}, {2808.882914, 64.496737}, + {2812.093184, 63.914073}, {2815.274353, 63.332677}, + {2818.426483, 62.752539}, {2821.549638, 62.173645}, + {2824.643878, 61.595982}, {2827.709266, 61.019538}, + {2830.745862, 60.444302}, {2833.753726, 59.870261}, + {2836.732918, 59.297402}, {2839.683496, 58.725715}, + {2842.605518, 58.155187}, {2845.499043, 57.585808}, + {2848.364128, 57.017565}, {2851.200828, 56.450447}, + {2854.009200, 55.884443}, {2856.789300, 55.319543}, + {2859.541182, 54.755734}, {2862.264900, 54.193006}, + {2864.960509, 53.631349}, {2867.628062, 53.070752}, + {2870.267611, 52.511204}, {2872.879208, 51.952695}, + {2875.462906, 51.395214}, {2878.018755, 50.838752}, + {2880.546806, 50.283298}, {2883.047110, 49.728843}, + {2885.519715, 49.175377}, {2887.964672, 48.622889}, + {2890.382028, 48.071371}, {2892.771833, 47.520812}, + {2895.134133, 46.971204}, {2897.468977, 46.422536}, + {2899.776410, 45.874801}, {2902.056480, 45.327988}, + {2904.309232, 44.782089}, {2906.534711, 44.237094}, + {2908.732964, 43.692995}, {2910.904033, 43.149784}, + {2913.047964, 42.607451}, {2915.164800, 42.065988}, + {2917.254584, 41.525386}, {2919.317360, 40.985639}, + {2921.353169, 40.446736}, {2923.362054, 39.908670}, + {2925.344057, 39.371433}, {2927.299218, 38.835016}, + {2929.227579, 38.299413}, {2931.129180, 37.764616}, + {2933.004061, 37.230616}, {2934.852261, 36.697406}, + {2936.673821, 36.164979}, {2938.468778, 35.633327}, + {2940.237173, 35.102443}, {2941.979042, 34.572320}, + {2943.694423, 34.042950}, {2945.383355, 33.514328}, + {2947.045875, 32.986444}, {2948.682018, 32.459294}, + {2950.291822, 31.932870}, {2951.875323, 31.407165}, + {2953.432557, 30.882174}, {2954.963558, 30.357888}, + {2956.468363, 29.834303}, {2957.947006, 29.311411}, + {2959.399521, 28.789207}, {2960.825944, 28.267684}, + {2962.226307, 27.746837}, {2963.600644, 27.226659}, + {2964.948989, 26.707144}, {2966.271375, 26.188288}, + {2967.567834, 25.670083}, {2968.838399, 25.152525}, + {2970.083103, 24.635607}, {2971.301976, 24.119325}, + {2972.495051, 23.603674}, {2973.662359, 23.088647}, + {2974.803931, 22.574240}, {2975.919798, 22.060447}, + {2977.009991, 21.547264}, {2978.074540, 21.034686}, + {2979.113475, 20.522708}, {2980.126826, 20.011325}, + {2981.114622, 19.500533}, {2982.076893, 18.990326}, + {2983.013669, 18.480701}, {2983.924978, 17.971653}, + {2984.810849, 17.463177}, {2985.671310, 16.955270}, + {2986.506390, 16.447928}, {2987.316117, 15.941145}, + {2988.100518, 15.434919}, {2988.859622, 14.929246}, + {2989.593457, 14.424121}, {2990.302048, 13.919540}, + {2990.985424, 13.415501}, {2991.643612, 12.912000}, + {2992.276637, 12.409033}, {2992.884528, 11.906597}, + {2993.467310, 11.404688}, {2994.025010, 10.903303}, + {2994.557654, 10.402440}, {2995.065267, 9.902095}, + {2995.547876, 9.402264}, {2996.005506, 8.902946}, + {2996.438183, 8.404138}, {2996.845933, 7.905836}, + {2997.228779, 7.408038}, {2997.586749, 6.910741}, + {2997.919866, 6.413944}, {2998.228156, 5.917643}, + {2998.511643, 5.421836}, {2998.770352, 4.926521}, + {2999.004307, 4.431696}, {2999.213533, 3.937359}, + {2999.398055, 3.443508}, {2999.557896, 2.950141}, + {2999.693081, 2.457255}, {2999.803634, 1.964850}, + {2999.889578, 1.472924}, {2999.950938, 0.981474}, + {2999.987737, 0.490500}, {2999.998745, 0.156943}, + {2999.999950, 0.031348}, {2999.999998, 0.006229}, + {3000.000000, 0.001206}, {3000.000000, 0.000201}, + {3000.000000, 0.000000}, + }}, + {301, + { + {1695.390174, 220.000000}, {1695.390174, 220.000000}, + {1704.778388, 218.550122}, {1715.664202, 216.882448}, + {1726.467249, 215.239455}, {1737.188744, 213.620339}, + {1747.829861, 212.024332}, {1758.391737, 210.450702}, + {1768.875473, 208.898750}, {1779.282137, 207.367807}, + {1789.612763, 205.857231}, {1799.868354, 204.366411}, + {1810.049883, 202.894761}, {1820.158295, 201.441717}, + {1830.194507, 200.006741}, {1840.159408, 198.589317}, + {1850.053865, 197.188947}, {1859.878717, 195.805157}, + {1869.634784, 194.437489}, {1879.322858, 193.085503}, + {1888.943715, 191.748777}, {1898.498107, 190.426905}, + {1907.986767, 189.119495}, {1917.410409, 187.826172}, + {1926.769728, 186.546574}, {1936.065401, 185.280351}, + {1945.298089, 184.027168}, {1954.468436, 182.786700}, + {1963.577069, 181.558635}, {1972.624602, 180.342670}, + {1981.611631, 179.138517}, {1990.538742, 177.945892}, + {1999.406502, 176.764526}, {2008.215469, 175.594156}, + {2016.966186, 174.434530}, {2025.659185, 173.285402}, + {2034.294983, 172.146536}, {2042.874089, 171.017703}, + {2051.396999, 169.898683}, {2059.864197, 168.789259}, + {2068.276159, 167.689226}, {2076.633350, 166.598382}, + {2084.936222, 165.516534}, {2093.185223, 164.443491}, + {2101.380787, 163.379072}, {2109.523341, 162.323100}, + {2117.613304, 161.275403}, {2125.651084, 160.235814}, + {2133.637084, 159.204172}, {2141.571696, 158.180320}, + {2149.455307, 157.164105}, {2157.288294, 156.155380}, + {2165.071029, 155.154001}, {2172.803874, 154.159829}, + {2180.487188, 153.172729}, {2188.121321, 152.192567}, + {2195.706615, 151.219217}, {2203.243410, 150.252553}, + {2210.732035, 149.292456}, {2218.172816, 148.338805}, + {2225.566074, 147.391488}, {2232.912121, 146.450392}, + {2240.211266, 145.515409}, {2247.463812, 144.586433}, + {2254.670057, 143.663360}, {2261.830293, 142.746090}, + {2268.944808, 141.834525}, {2276.013886, 140.928571}, + {2283.037803, 140.028133}, {2290.016835, 139.133121}, + {2296.951249, 138.243446}, {2303.841311, 137.359023}, + {2310.687280, 136.479767}, {2317.489414, 135.605596}, + {2324.247965, 134.736430}, {2330.963181, 133.872189}, + {2337.635305, 133.012799}, {2344.264580, 132.158183}, + {2350.851241, 131.308270}, {2357.395523, 130.462987}, + {2363.897654, 129.622265}, {2370.357861, 128.786036}, + {2376.776368, 127.954232}, {2383.153394, 127.126790}, + {2389.489155, 126.303645}, {2395.783864, 125.484735}, + {2402.037732, 124.669999}, {2408.250967, 123.859376}, + {2414.423771, 123.052810}, {2420.556348, 122.250241}, + {2426.648894, 121.451615}, {2432.701606, 120.656876}, + {2438.714678, 119.865971}, {2444.688298, 119.078847}, + {2450.622656, 118.295453}, {2456.517935, 117.515737}, + {2462.374320, 116.739650}, {2468.191990, 115.967143}, + {2473.971123, 115.198169}, {2479.711894, 114.432682}, + {2485.414477, 113.670634}, {2491.079042, 112.911982}, + {2496.705759, 112.156680}, {2502.294793, 111.404687}, + {2507.846309, 110.655958}, {2513.360469, 109.910452}, + {2518.837434, 109.168129}, {2524.277361, 108.428947}, + {2529.680406, 107.692868}, {2535.046724, 106.959853}, + {2540.376467, 106.229862}, {2545.669785, 105.502860}, + {2550.926827, 104.778808}, {2556.147739, 104.057671}, + {2561.332666, 103.339413}, {2566.481751, 102.623999}, + {2571.595136, 101.911394}, {2576.672960, 101.201565}, + {2581.715361, 100.494479}, {2586.722475, 99.790101}, + {2591.694438, 99.088401}, {2596.631382, 98.389346}, + {2601.533438, 97.692906}, {2606.400737, 96.999048}, + {2611.233407, 96.307744}, {2616.031574, 95.618963}, + {2620.795365, 94.932676}, {2625.524904, 94.248855}, + {2630.220312, 93.567469}, {2634.881711, 92.888492}, + {2639.509220, 92.211896}, {2644.102959, 91.537654}, + {2648.663044, 90.865738}, {2653.189590, 90.196123}, + {2657.682713, 89.528781}, {2662.142525, 88.863689}, + {2666.569138, 88.200820}, {2670.962662, 87.540149}, + {2675.323207, 86.881652}, {2679.650881, 86.225304}, + {2683.945790, 85.571081}, {2688.208041, 84.918961}, + {2692.437738, 84.268919}, {2696.634985, 83.620933}, + {2700.799882, 82.974979}, {2704.932533, 82.331037}, + {2709.033036, 81.689082}, {2713.101490, 81.049095}, + {2717.137994, 80.411052}, {2721.142644, 79.774934}, + {2725.115535, 79.140719}, {2729.056762, 78.508386}, + {2732.966420, 77.877915}, {2736.844600, 77.249286}, + {2740.691394, 76.622478}, {2744.506893, 75.997474}, + {2748.291186, 75.374252}, {2752.044362, 74.752793}, + {2755.766509, 74.133080}, {2759.457713, 73.515093}, + {2763.118061, 72.898813}, {2766.747637, 72.284223}, + {2770.346525, 71.671304}, {2773.914809, 71.060039}, + {2777.452570, 70.450410}, {2780.959890, 69.842400}, + {2784.436850, 69.235991}, {2787.883529, 68.631167}, + {2791.300006, 68.027910}, {2794.686359, 67.426205}, + {2798.042665, 66.826034}, {2801.369000, 66.227382}, + {2804.665440, 65.630233}, {2807.932060, 65.034571}, + {2811.168934, 64.440379}, {2814.376135, 63.847644}, + {2817.553735, 63.256349}, {2820.701805, 62.666480}, + {2823.820418, 62.078020}, {2826.909642, 61.490957}, + {2829.969548, 60.905275}, {2833.000204, 60.320960}, + {2836.001678, 59.737997}, {2838.974037, 59.156372}, + {2841.917348, 58.576073}, {2844.831677, 57.997083}, + {2847.717089, 57.419391}, {2850.573648, 56.842982}, + {2853.401419, 56.267844}, {2856.200464, 55.693963}, + {2858.970846, 55.121326}, {2861.712628, 54.549920}, + {2864.425869, 53.979733}, {2867.110631, 53.410752}, + {2869.766974, 52.842964}, {2872.394957, 52.276358}, + {2874.994639, 51.710920}, {2877.566078, 51.146640}, + {2880.109332, 50.583505}, {2882.624457, 50.021503}, + {2885.111510, 49.460623}, {2887.570547, 48.900853}, + {2890.001623, 48.342183}, {2892.404792, 47.784600}, + {2894.780110, 47.228093}, {2897.127628, 46.672653}, + {2899.447401, 46.118267}, {2901.739481, 45.564925}, + {2904.003920, 45.012617}, {2906.240768, 44.461331}, + {2908.450078, 43.911059}, {2910.631899, 43.361789}, + {2912.786282, 42.813511}, {2914.913275, 42.266215}, + {2917.012928, 41.719893}, {2919.085288, 41.174532}, + {2921.130405, 40.630125}, {2923.148324, 40.086662}, + {2925.139094, 39.544133}, {2927.102761, 39.002528}, + {2929.039370, 38.461840}, {2930.948967, 37.922058}, + {2932.831598, 37.383173}, {2934.687307, 36.845178}, + {2936.516138, 36.308063}, {2938.318135, 35.771819}, + {2940.093341, 35.236438}, {2941.841800, 34.701912}, + {2943.563554, 34.168232}, {2945.258644, 33.635391}, + {2946.927114, 33.103379}, {2948.569003, 32.572190}, + {2950.184353, 32.041815}, {2951.773204, 31.512246}, + {2953.335597, 30.983476}, {2954.871572, 30.455498}, + {2956.381167, 29.928303}, {2957.864422, 29.401885}, + {2959.321375, 28.876236}, {2960.752064, 28.351350}, + {2962.156528, 27.827218}, {2963.534805, 27.303835}, + {2964.886930, 26.781193}, {2966.212942, 26.259287}, + {2967.512877, 25.738108}, {2968.786771, 25.217651}, + {2970.034660, 24.697909}, {2971.256580, 24.178875}, + {2972.452565, 23.660545}, {2973.622652, 23.142911}, + {2974.766874, 22.625968}, {2975.885266, 22.109709}, + {2976.977862, 21.594130}, {2978.044695, 21.079223}, + {2979.085801, 20.564984}, {2980.101210, 20.051408}, + {2981.090958, 19.538487}, {2982.055076, 19.026219}, + {2982.993596, 18.514596}, {2983.906551, 18.003614}, + {2984.793973, 17.493269}, {2985.655894, 16.983554}, + {2986.492344, 16.474466}, {2987.303356, 15.965999}, + {2988.088960, 15.458149}, {2988.849186, 14.950912}, + {2989.584066, 14.444283}, {2990.293630, 13.938257}, + {2990.977907, 13.432831}, {2991.636928, 12.928000}, + {2992.270721, 12.423760}, {2992.879318, 11.920107}, + {2993.462747, 11.417038}, {2994.021036, 10.914548}, + {2994.554216, 10.412635}, {2995.062314, 9.911294}, + {2995.545360, 9.410521}, {2996.003380, 8.910315}, + {2996.436405, 8.410670}, {2996.844461, 7.911585}, + {2997.227577, 7.413055}, {2997.585781, 6.915079}, + {2997.919099, 6.417652}, {2998.227560, 5.920773}, + {2998.511190, 5.424438}, {2998.770017, 4.928645}, + {2999.004068, 4.433392}, {2999.213370, 3.938675}, + {2999.397949, 3.444492}, {2999.557832, 2.950842}, + {2999.693046, 2.457722}, {2999.803618, 1.965129}, + {2999.889572, 1.473063}, {2999.950937, 0.981520}, + {2999.987737, 0.490500}, {2999.998745, 0.156943}, + {2999.999950, 0.031348}, {2999.999998, 0.006229}, + {3000.000000, 0.001206}, {3000.000000, 0.000201}, + {3000.000000, 0.000000}, + }}, + {292, + { + {1759.490932, 220.000000}, {1759.490932, 220.000000}, + {1765.012838, 219.052668}, {1775.919233, 217.203118}, + {1786.733957, 215.385836}, {1797.458593, 213.599624}, + {1808.094667, 211.843341}, {1818.643648, 210.115907}, + {1829.106953, 208.416293}, {1839.485949, 206.743523}, + {1849.781953, 205.096665}, {1859.996241, 203.474836}, + {1870.130042, 201.877191}, {1880.184545, 200.302927}, + {1890.160900, 198.751276}, {1900.060219, 197.221506}, + {1909.883580, 195.712920}, {1919.632024, 194.224849}, + {1929.306562, 192.756656}, {1938.908171, 191.307730}, + {1948.437802, 189.877488}, {1957.896373, 188.465370}, + {1967.284779, 187.070842}, {1976.603884, 185.693390}, + {1985.854532, 184.332523}, {1995.037540, 182.987769}, + {2004.153701, 181.658677}, {2013.203788, 180.344811}, + {2022.188552, 179.045756}, {2031.108724, 177.761109}, + {2039.965014, 176.490487}, {2048.758114, 175.233518}, + {2057.488698, 173.989848}, {2066.157422, 172.759132}, + {2074.764927, 171.541042}, {2083.311834, 170.335259}, + {2091.798753, 169.141478}, {2100.226275, 167.959404}, + {2108.594979, 166.788752}, {2116.905429, 165.629247}, + {2125.158176, 164.480627}, {2133.353757, 163.342634}, + {2141.492698, 162.215023}, {2149.575513, 161.097555}, + {2157.602702, 159.990001}, {2165.574755, 158.892138}, + {2173.492152, 157.803751}, {2181.355362, 156.724632}, + {2189.164842, 155.654580}, {2196.921042, 154.593402}, + {2204.624400, 153.540907}, {2212.275345, 152.496914}, + {2219.874299, 151.461247}, {2227.421674, 150.433734}, + {2234.917872, 149.414209}, {2242.363290, 148.402512}, + {2249.758315, 147.398486}, {2257.103327, 146.401980}, + {2264.398698, 145.412848}, {2271.644793, 144.430947}, + {2278.841970, 143.456139}, {2285.990580, 142.488288}, + {2293.090969, 141.527266}, {2300.143474, 140.572945}, + {2307.148428, 139.625202}, {2314.106156, 138.683917}, + {2321.016978, 137.748973}, {2327.881209, 136.820258}, + {2334.699157, 135.897661}, {2341.471125, 134.981074}, + {2348.197412, 134.070394}, {2354.878310, 133.165518}, + {2361.514107, 132.266348}, {2368.105085, 131.372787}, + {2374.651523, 130.484740}, {2381.153695, 129.602118}, + {2387.611868, 128.724829}, {2394.026309, 127.852787}, + {2400.397276, 126.985907}, {2406.725026, 126.124105}, + {2413.009811, 125.267302}, {2419.251879, 124.415418}, + {2425.451474, 123.568376}, {2431.608836, 122.726101}, + {2437.724202, 121.888519}, {2443.797804, 121.055558}, + {2449.829871, 120.227148}, {2455.820631, 119.403221}, + {2461.770304, 118.583710}, {2467.679110, 117.768548}, + {2473.547266, 116.957672}, {2479.374983, 116.151019}, + {2485.162472, 115.348528}, {2490.909938, 114.550138}, + {2496.617587, 113.755792}, {2502.285617, 112.965430}, + {2507.914228, 112.178997}, {2513.503614, 111.396438}, + {2519.053967, 110.617699}, {2524.565478, 109.842726}, + {2530.038333, 109.071468}, {2535.472716, 108.303874}, + {2540.868810, 107.539894}, {2546.226795, 106.779478}, + {2551.546846, 106.022580}, {2556.829140, 105.269152}, + {2562.073847, 104.519148}, {2567.281139, 103.772522}, + {2572.451183, 103.029230}, {2577.584144, 102.289229}, + {2582.680187, 101.552475}, {2587.739472, 100.818927}, + {2592.762158, 100.088544}, {2597.748404, 99.361284}, + {2602.698364, 98.637107}, {2607.612191, 97.915976}, + {2612.490037, 97.197851}, {2617.332050, 96.482694}, + {2622.138379, 95.770469}, {2626.909170, 95.061138}, + {2631.644565, 94.354667}, {2636.344707, 93.651018}, + {2641.009736, 92.950159}, {2645.639792, 92.252054}, + {2650.235010, 91.556670}, {2654.795526, 90.863973}, + {2659.321473, 90.173932}, {2663.812985, 89.486514}, + {2668.270190, 88.801688}, {2672.693217, 88.119423}, + {2677.082195, 87.439687}, {2681.437249, 86.762452}, + {2685.758502, 86.087686}, {2690.046078, 85.415362}, + {2694.300099, 84.745450}, {2698.520683, 84.077922}, + {2702.707950, 83.412750}, {2706.862016, 82.749906}, + {2710.982998, 82.089364}, {2715.071009, 81.431096}, + {2719.126164, 80.775077}, {2723.148573, 80.121280}, + {2727.138347, 79.469680}, {2731.095595, 78.820251}, + {2735.020425, 78.172969}, {2738.912945, 77.527809}, + {2742.773259, 76.884747}, {2746.601471, 76.243758}, + {2750.397686, 75.604820}, {2754.162004, 74.967909}, + {2757.894527, 74.333002}, {2761.595354, 73.700077}, + {2765.264583, 73.069111}, {2768.902313, 72.440083}, + {2772.508640, 71.812969}, {2776.083658, 71.187750}, + {2779.627461, 70.564404}, {2783.140144, 69.942909}, + {2786.621798, 69.323246}, {2790.072514, 68.705394}, + {2793.492382, 68.089332}, {2796.881492, 67.475041}, + {2800.239930, 66.862502}, {2803.567785, 66.251695}, + {2806.865142, 65.642601}, {2810.132087, 65.035200}, + {2813.368704, 64.429475}, {2816.575076, 63.825407}, + {2819.751286, 63.222977}, {2822.897415, 62.622169}, + {2826.013543, 62.022963}, {2829.099751, 61.425343}, + {2832.156116, 60.829290}, {2835.182718, 60.234789}, + {2838.179634, 59.641823}, {2841.146939, 59.050373}, + {2844.084709, 58.460425}, {2846.993018, 57.871961}, + {2849.871941, 57.284966}, {2852.721551, 56.699423}, + {2855.541920, 56.115318}, {2858.333118, 55.532634}, + {2861.095218, 54.951356}, {2863.828289, 54.371469}, + {2866.532399, 53.792957}, {2869.207618, 53.215808}, + {2871.854014, 52.640004}, {2874.471652, 52.065533}, + {2877.060600, 51.492380}, {2879.620923, 50.920530}, + {2882.152685, 50.349970}, {2884.655952, 49.780687}, + {2887.130786, 49.212665}, {2889.577249, 48.645893}, + {2891.995406, 48.080357}, {2894.385316, 47.516043}, + {2896.747040, 46.952939}, {2899.080640, 46.391031}, + {2901.386173, 45.830308}, {2903.663700, 45.270757}, + {2905.913278, 44.712364}, {2908.134965, 44.155119}, + {2910.328818, 43.599010}, {2912.494894, 43.044023}, + {2914.633248, 42.490148}, {2916.743936, 41.937373}, + {2918.827013, 41.385686}, {2920.882532, 40.835077}, + {2922.910547, 40.285533}, {2924.911111, 39.737044}, + {2926.884277, 39.189599}, {2928.830097, 38.643188}, + {2930.748622, 38.097799}, {2932.639902, 37.553422}, + {2934.503989, 37.010047}, {2936.340932, 36.467663}, + {2938.150780, 35.926261}, {2939.933582, 35.385830}, + {2941.689387, 34.846362}, {2943.418242, 34.307845}, + {2945.120195, 33.770270}, {2946.795292, 33.233629}, + {2948.443581, 32.697911}, {2950.065106, 32.163108}, + {2951.659914, 31.629211}, {2953.228050, 31.096210}, + {2954.769557, 30.564096}, {2956.284481, 30.032862}, + {2957.772865, 29.502499}, {2959.234753, 28.972997}, + {2960.670186, 28.444350}, {2962.079209, 27.916548}, + {2963.461862, 27.389583}, {2964.818188, 26.863449}, + {2966.148228, 26.338136}, {2967.452022, 25.813637}, + {2968.729612, 25.289945}, {2969.981036, 24.767052}, + {2971.206337, 24.244951}, {2972.405551, 23.723634}, + {2973.578719, 23.203095}, {2974.725880, 22.683327}, + {2975.847071, 22.164322}, {2976.942331, 21.646073}, + {2978.011697, 21.128575}, {2979.055207, 20.611821}, + {2980.072898, 20.095804}, {2981.064806, 19.580518}, + {2982.030968, 19.065957}, {2982.971420, 18.552115}, + {2983.886197, 18.038985}, {2984.775336, 17.526563}, + {2985.638871, 17.014842}, {2986.476837, 16.503816}, + {2987.289270, 15.993480}, {2988.076202, 15.483830}, + {2988.837670, 14.974859}, {2989.573705, 14.466562}, + {2990.284343, 13.958934}, {2990.969615, 13.451971}, + {2991.629556, 12.945668}, {2992.264198, 12.440019}, + {2992.873574, 11.935020}, {2993.457717, 11.430667}, + {2994.016657, 10.926955}, {2994.550428, 10.423881}, + {2995.059061, 9.921439}, {2995.542588, 9.419626}, + {2996.001039, 8.918438}, {2996.434447, 8.417870}, + {2996.842842, 7.917920}, {2997.226254, 7.418583}, + {2997.584715, 6.919857}, {2997.918255, 6.421737}, + {2998.226904, 5.924220}, {2998.510692, 5.427303}, + {2998.769649, 4.930983}, {2999.003805, 4.435257}, + {2999.213190, 3.940122}, {2999.397832, 3.445575}, + {2999.557762, 2.951613}, {2999.693008, 2.458235}, + {2999.803600, 1.965436}, {2999.889566, 1.473216}, + {2999.950936, 0.981571}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, + {284, + { + {1823.184013, 220.000000}, {1823.184013, 220.000000}, + {1823.184013, 220.000000}, {1823.760793, 219.889280}, + {1834.703590, 217.822632}, {1845.544118, 215.798484}, + {1856.284456, 213.815035}, {1866.926597, 211.870587}, + {1877.472450, 209.963538}, {1887.923848, 208.092373}, + {1898.282549, 206.255660}, {1908.550241, 204.452046}, + {1918.728549, 202.680249}, {1928.819031, 200.939055}, + {1938.823190, 199.227313}, {1948.742471, 197.543931}, + {1958.578267, 195.887875}, {1968.331917, 194.258160}, + {1978.004718, 192.653852}, {1987.597916, 191.074062}, + {1997.112716, 189.517945}, {2006.550282, 187.984697}, + {2015.911738, 186.473550}, {2025.198171, 184.983776}, + {2034.410632, 183.514676}, {2043.550139, 182.065588}, + {2052.617676, 180.635876}, {2061.614196, 179.224936}, + {2070.540624, 177.832189}, {2079.397856, 176.457081}, + {2088.186760, 175.099083}, {2096.908179, 173.757690}, + {2105.562932, 172.432416}, {2114.151812, 171.122796}, + {2122.675592, 169.828387}, {2131.135020, 168.548760}, + {2139.530827, 167.283507}, {2147.863721, 166.032236}, + {2156.134391, 164.794568}, {2164.343508, 163.570141}, + {2172.491727, 162.358608}, {2180.579683, 161.159634}, + {2188.607997, 159.972897}, {2196.577271, 158.798087}, + {2204.488096, 157.634906}, {2212.341045, 156.483068}, + {2220.136679, 155.342295}, {2227.875545, 154.212321}, + {2235.558175, 153.092889}, {2243.185091, 151.983752}, + {2250.756802, 150.884670}, {2258.273804, 149.795413}, + {2265.736583, 148.715758}, {2273.145614, 147.645490}, + {2280.501361, 146.584400}, {2287.804279, 145.532289}, + {2295.054810, 144.488961}, {2302.253390, 143.454228}, + {2309.400443, 142.427909}, {2316.496387, 141.409828}, + {2323.541628, 140.399814}, {2330.536565, 139.397702}, + {2337.481591, 138.403332}, {2344.377088, 137.416549}, + {2351.223432, 136.437204}, {2358.020991, 135.465149}, + {2364.770126, 134.500244}, {2371.471191, 133.542351}, + {2378.124533, 132.591337}, {2384.730493, 131.647074}, + {2391.289406, 130.709434}, {2397.801599, 129.778296}, + {2404.267395, 128.853542}, {2410.687110, 127.935056}, + {2417.061055, 127.022726}, {2423.389534, 126.116442}, + {2429.672847, 125.216100}, {2435.911290, 124.321594}, + {2442.105150, 123.432825}, {2448.254713, 122.549694}, + {2454.360258, 121.672107}, {2460.422060, 120.799970}, + {2466.440389, 119.933193}, {2472.415511, 119.071686}, + {2478.347687, 118.215365}, {2484.237175, 117.364145}, + {2490.084227, 116.517943}, {2495.889093, 115.676681}, + {2501.652017, 114.840279}, {2507.373241, 114.008662}, + {2513.053001, 113.181756}, {2518.691532, 112.359486}, + {2524.289064, 111.541783}, {2529.845823, 110.728576}, + {2535.362032, 109.919799}, {2540.837912, 109.115384}, + {2546.273678, 108.315266}, {2551.669544, 107.519383}, + {2557.025721, 106.727671}, {2562.342414, 105.940071}, + {2567.619829, 105.156522}, {2572.858166, 104.376966}, + {2578.057624, 103.601346}, {2583.218398, 102.829607}, + {2588.340680, 102.061693}, {2593.424661, 101.297551}, + {2598.470528, 100.537128}, {2603.478466, 99.780372}, + {2608.448656, 99.027234}, {2613.381278, 98.277663}, + {2618.276510, 97.531612}, {2623.134526, 96.789031}, + {2627.955499, 96.049875}, {2632.739598, 95.314097}, + {2637.486992, 94.581653}, {2642.197846, 93.852498}, + {2646.872323, 93.126589}, {2651.510585, 92.403883}, + {2656.112790, 91.684337}, {2660.679096, 90.967912}, + {2665.209658, 90.254567}, {2669.704629, 89.544261}, + {2674.164160, 88.836956}, {2678.588399, 88.132613}, + {2682.977494, 87.431195}, {2687.331591, 86.732665}, + {2691.650832, 86.036985}, {2695.935359, 85.344121}, + {2700.185313, 84.654036}, {2704.400832, 83.966697}, + {2708.582051, 83.282068}, {2712.729105, 82.600116}, + {2716.842129, 81.920809}, {2720.921252, 81.244113}, + {2724.966604, 80.569997}, {2728.978315, 79.898428}, + {2732.956510, 79.229377}, {2736.901315, 78.562811}, + {2740.812853, 77.898701}, {2744.691246, 77.237018}, + {2748.536614, 76.577732}, {2752.349078, 75.920814}, + {2756.128754, 75.266236}, {2759.875759, 74.613969}, + {2763.590208, 73.963987}, {2767.272214, 73.316262}, + {2770.921890, 72.670767}, {2774.539346, 72.027475}, + {2778.124692, 71.386362}, {2781.678036, 70.747400}, + {2785.199485, 70.110566}, {2788.689145, 69.475833}, + {2792.147121, 68.843177}, {2795.573514, 68.212574}, + {2798.968429, 67.584000}, {2802.331964, 66.957431}, + {2805.664221, 66.332844}, {2808.965298, 65.710217}, + {2812.235291, 65.089525}, {2815.474298, 64.470748}, + {2818.682413, 63.853862}, {2821.859731, 63.238846}, + {2825.006344, 62.625679}, {2828.122345, 62.014339}, + {2831.207823, 61.404806}, {2834.262870, 60.797058}, + {2837.287573, 60.191076}, {2840.282021, 59.586839}, + {2843.246300, 58.984327}, {2846.180497, 58.383521}, + {2849.084695, 57.784401}, {2851.958978, 57.186948}, + {2854.803431, 56.591144}, {2857.618133, 55.996969}, + {2860.403168, 55.404406}, {2863.158614, 54.813436}, + {2865.884551, 54.224041}, {2868.581057, 53.636204}, + {2871.248210, 53.049907}, {2873.886086, 52.465132}, + {2876.494760, 51.881863}, {2879.074309, 51.300084}, + {2881.624806, 50.719776}, {2884.146323, 50.140925}, + {2886.638934, 49.563513}, {2889.102710, 48.987526}, + {2891.537722, 48.412946}, {2893.944039, 47.839758}, + {2896.321732, 47.267948}, {2898.670868, 46.697499}, + {2900.991516, 46.128398}, {2903.283741, 45.560628}, + {2905.547611, 44.994176}, {2907.783192, 44.429026}, + {2909.990546, 43.865166}, {2912.169740, 43.302580}, + {2914.320836, 42.741255}, {2916.443897, 42.181177}, + {2918.538984, 41.622333}, {2920.606160, 41.064708}, + {2922.645485, 40.508291}, {2924.657019, 39.953068}, + {2926.640822, 39.399026}, {2928.596951, 38.846153}, + {2930.525466, 38.294436}, {2932.426423, 37.743862}, + {2934.299880, 37.194420}, {2936.145893, 36.646097}, + {2937.964518, 36.098882}, {2939.755809, 35.552763}, + {2941.519821, 35.007729}, {2943.256609, 34.463767}, + {2944.966225, 33.920868}, {2946.648722, 33.379019}, + {2948.304152, 32.838211}, {2949.932568, 32.298431}, + {2951.534021, 31.759670}, {2953.108561, 31.221917}, + {2954.656238, 30.685162}, {2956.177102, 30.149395}, + {2957.671202, 29.614606}, {2959.138586, 29.080784}, + {2960.579304, 28.547921}, {2961.993402, 28.016006}, + {2963.380928, 27.485030}, {2964.741928, 26.954984}, + {2966.076450, 26.425859}, {2967.384537, 25.897646}, + {2968.666237, 25.370336}, {2969.921593, 24.843920}, + {2971.150651, 24.318390}, {2972.353454, 23.793737}, + {2973.530046, 23.269953}, {2974.680471, 22.747030}, + {2975.804771, 22.224960}, {2976.902988, 21.703734}, + {2977.975165, 21.183346}, {2979.021343, 20.663788}, + {2980.041564, 20.145051}, {2981.035869, 19.627130}, + {2982.004297, 19.110016}, {2982.946890, 18.593702}, + {2983.863688, 18.078183}, {2984.754728, 17.563450}, + {2985.620052, 17.049497}, {2986.459697, 16.536318}, + {2987.273703, 16.023906}, {2988.062107, 15.512254}, + {2988.824947, 15.001358}, {2989.562261, 14.491210}, + {2990.274087, 13.981805}, {2990.960460, 13.473137}, + {2991.621419, 12.965200}, {2992.256999, 12.457990}, + {2992.867236, 11.951499}, {2993.452166, 11.445724}, + {2994.011826, 10.940659}, {2994.546250, 10.436299}, + {2995.055473, 9.932639}, {2995.539531, 9.429675}, + {2995.998458, 8.927401}, {2996.432289, 8.425813}, + {2996.841057, 7.924907}, {2997.224796, 7.424679}, + {2997.583541, 6.925124}, {2997.917325, 6.426238}, + {2998.226182, 5.928018}, {2998.510144, 5.430459}, + {2998.769244, 4.933558}, {2999.003516, 4.437311}, + {2999.212991, 3.941715}, {2999.397703, 3.446766}, + {2999.557684, 2.952462}, {2999.692966, 2.458799}, + {2999.803580, 1.965774}, {2999.889559, 1.473384}, + {2999.950934, 0.981627}, {2999.987737, 0.490500}, + {2999.998745, 0.156943}, {2999.999950, 0.031348}, + {2999.999998, 0.006229}, {3000.000000, 0.001206}, + {3000.000000, 0.000201}, {3000.000000, 0.000000}, + }}, +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h new file mode 100644 index 0000000000000000000000000000000000000000..519bf952a929e8d2e64544f09a2d25bc81321aec --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h @@ -0,0 +1,1462 @@ +/* + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace DeathStackBoard +{ + +static const unsigned int TOT_TRAJECTORIES = 9; +static const unsigned int TRAJECTORY_MAX_LENGTH = 320; + +struct point_t +{ + float z; + float vz; +}; + +struct trajectory_t +{ + unsigned int length; + point_t data[TRAJECTORY_MAX_LENGTH]; +}; + +const trajectory_t TRAJECTORIES_DATA[TOT_TRAJECTORIES] = { + {320, + { + {2.237702, 190.662198}, {11.748404, 189.765855}, + {21.214388, 188.873519}, {30.635855, 187.985148}, + {40.013001, 187.100700}, {49.346022, 186.220134}, + {58.635110, 185.343410}, {67.880458, 184.470488}, + {77.082253, 183.601329}, {86.240684, 182.735893}, + {95.355935, 181.874143}, {104.428189, 181.016040}, + {113.457629, 180.161549}, {122.444433, 179.310631}, + {131.388780, 178.463251}, {140.290846, 177.619372}, + {149.150804, 176.778961}, {157.968828, 175.941981}, + {166.745087, 175.108399}, {175.479752, 174.278181}, + {184.172989, 173.451293}, {192.824964, 172.627702}, + {201.435841, 171.807376}, {210.005782, 170.990282}, + {218.534949, 170.176389}, {227.023500, 169.365665}, + {235.471594, 168.558080}, {243.879386, 167.753602}, + {252.247031, 166.952202}, {260.574682, 166.153850}, + {268.862491, 165.358516}, {277.110608, 164.566171}, + {285.319182, 163.776786}, {293.488360, 162.990334}, + {301.618288, 162.206785}, {309.709111, 161.426113}, + {317.760971, 160.648290}, {325.774010, 159.873289}, + {333.748370, 159.101082}, {341.684188, 158.331645}, + {349.581603, 157.564950}, {357.440751, 156.800972}, + {365.261767, 156.039686}, {373.044786, 155.281065}, + {380.789940, 154.525086}, {388.497360, 153.771724}, + {396.167177, 153.020954}, {403.799520, 152.272752}, + {411.394516, 151.527095}, {418.952292, 150.783959}, + {426.472974, 150.043321}, {433.956686, 149.305158}, + {441.403551, 148.569447}, {448.813692, 147.836166}, + {456.187228, 147.105293}, {463.524280, 146.376805}, + {470.824968, 145.650681}, {478.089407, 144.926899}, + {485.317716, 144.205439}, {492.510009, 143.486279}, + {499.666401, 142.769399}, {506.787005, 142.054777}, + {513.871934, 141.342394}, {520.921300, 140.632230}, + {527.935212, 139.924264}, {534.913781, 139.218478}, + {541.857114, 138.514851}, {548.765319, 137.813364}, + {555.638503, 137.113998}, {562.476772, 136.416735}, + {569.280229, 135.721556}, {576.048979, 135.028442}, + {582.783124, 134.337375}, {589.482767, 133.648337}, + {596.148008, 132.961310}, {602.778948, 132.276277}, + {609.375685, 131.593219}, {615.938319, 130.912120}, + {622.466946, 130.232962}, {628.961663, 129.555729}, + {635.422566, 128.880403}, {641.849751, 128.206968}, + {648.243310, 127.535408}, {654.603338, 126.865706}, + {660.929927, 126.197846}, {667.223168, 125.531811}, + {673.483153, 124.867587}, {679.709972, 124.205157}, + {685.903713, 123.544506}, {692.064467, 122.885619}, + {698.192319, 122.228480}, {704.287358, 121.573074}, + {710.349669, 120.919387}, {716.379339, 120.267402}, + {722.376452, 119.617107}, {728.341092, 118.968486}, + {734.273342, 118.321525}, {740.173285, 117.676209}, + {746.041004, 117.032525}, {751.876578, 116.390458}, + {757.680090, 115.749995}, {763.451617, 115.111122}, + {769.191241, 114.473825}, {774.899039, 113.838091}, + {780.575089, 113.203907}, {786.219468, 112.571258}, + {791.832253, 111.940133}, {797.413519, 111.310518}, + {802.963342, 110.682400}, {808.481796, 110.055766}, + {813.968956, 109.430605}, {819.424893, 108.806902}, + {824.849682, 108.184646}, {830.243394, 107.563825}, + {835.606100, 106.944426}, {840.937872, 106.326437}, + {846.238779, 105.709846}, {851.508891, 105.094641}, + {856.748277, 104.480810}, {861.957006, 103.868343}, + {867.135145, 103.257226}, {872.282762, 102.647448}, + {877.399923, 102.038999}, {882.486695, 101.431867}, + {887.543143, 100.826040}, {892.569331, 100.221507}, + {897.565325, 99.618258}, {902.531189, 99.016281}, + {907.466985, 98.415566}, {912.372777, 97.816101}, + {917.248626, 97.217877}, {922.094595, 96.620882}, + {926.910745, 96.025106}, {931.697136, 95.430538}, + {936.453829, 94.837169}, {941.180883, 94.244988}, + {945.878357, 93.653984}, {950.546310, 93.064148}, + {955.184800, 92.475469}, {959.793886, 91.887938}, + {964.373623, 91.301545}, {968.924068, 90.716279}, + {973.445279, 90.132132}, {977.937309, 89.549093}, + {982.400215, 88.967154}, {986.834052, 88.386303}, + {991.238873, 87.806533}, {995.614732, 87.227834}, + {999.961683, 86.650196}, {1004.279778, 86.073610}, + {1008.569070, 85.498067}, {1012.829610, 84.923558}, + {1017.061451, 84.350074}, {1021.264643, 83.777606}, + {1025.439237, 83.206145}, {1029.585283, 82.635683}, + {1033.702830, 82.066210}, {1037.791928, 81.497717}, + {1041.852626, 80.930197}, {1045.884972, 80.363641}, + {1049.889014, 79.798040}, {1053.864800, 79.233385}, + {1057.812376, 78.669669}, {1061.731790, 78.106882}, + {1065.623087, 77.545018}, {1069.486314, 76.984067}, + {1073.321517, 76.424021}, {1077.128739, 75.864873}, + {1080.908026, 75.306613}, {1084.659422, 74.749235}, + {1088.382971, 74.192731}, {1092.078717, 73.637092}, + {1095.746702, 73.082310}, {1099.386969, 72.528379}, + {1102.999561, 71.975289}, {1106.584519, 71.423035}, + {1110.141885, 70.871607}, {1113.671700, 70.320998}, + {1117.174005, 69.771202}, {1120.648841, 69.222209}, + {1124.096246, 68.674014}, {1127.516262, 68.126609}, + {1130.908927, 67.579986}, {1134.274280, 67.034138}, + {1137.612360, 66.489058}, {1140.923204, 65.944739}, + {1144.206852, 65.401174}, {1147.463340, 64.858355}, + {1150.692706, 64.316276}, {1153.894986, 63.774929}, + {1157.070217, 63.234309}, {1160.218435, 62.694407}, + {1163.339676, 62.155217}, {1166.433975, 61.616733}, + {1169.501367, 61.078947}, {1172.541887, 60.541853}, + {1175.555569, 60.005444}, {1178.542448, 59.469714}, + {1181.502557, 58.934656}, {1184.435930, 58.400263}, + {1187.342600, 57.866529}, {1190.222599, 57.333448}, + {1193.075961, 56.801014}, {1195.902717, 56.269219}, + {1198.702899, 55.738058}, {1201.476538, 55.207524}, + {1204.223667, 54.677611}, {1206.944315, 54.148313}, + {1209.638513, 53.619624}, {1212.306292, 53.091537}, + {1214.947682, 52.564047}, {1217.562712, 52.037148}, + {1220.151411, 51.510833}, {1222.713809, 50.985097}, + {1225.249935, 50.459933}, {1227.759817, 49.935337}, + {1230.243483, 49.411301}, {1232.700961, 48.887821}, + {1235.132279, 48.364890}, {1237.537463, 47.842503}, + {1239.916542, 47.320654}, {1242.269542, 46.799338}, + {1244.596489, 46.278549}, {1246.897410, 45.758281}, + {1249.172330, 45.238528}, {1251.421276, 44.719286}, + {1253.644272, 44.200549}, {1255.841343, 43.682311}, + {1258.012515, 43.164568}, {1260.157812, 42.647313}, + {1262.277258, 42.130542}, {1264.370878, 41.614249}, + {1266.438695, 41.098429}, {1268.480733, 40.583076}, + {1270.497014, 40.068187}, {1272.487563, 39.553755}, + {1274.452401, 39.039775}, {1276.391552, 38.526243}, + {1278.305037, 38.013154}, {1280.192878, 37.500502}, + {1282.055097, 36.988282}, {1283.891717, 36.476490}, + {1285.702757, 35.965121}, {1287.488239, 35.454171}, + {1289.248184, 34.943633}, {1290.982613, 34.433505}, + {1292.691545, 33.923780}, {1294.375001, 33.414454}, + {1296.033000, 32.905523}, {1297.665563, 32.396983}, + {1299.272708, 31.888827}, {1300.854455, 31.381053}, + {1302.410823, 30.873656}, {1303.941830, 30.366631}, + {1305.447495, 29.859974}, {1306.927837, 29.353681}, + {1308.382872, 28.847747}, {1309.812620, 28.342168}, + {1311.217098, 27.836940}, {1312.596323, 27.332059}, + {1313.950312, 26.827521}, {1315.279083, 26.323321}, + {1316.582653, 25.819456}, {1317.861037, 25.315922}, + {1319.114253, 24.812715}, {1320.342317, 24.309831}, + {1321.545244, 23.807267}, {1322.723051, 23.305018}, + {1323.875754, 22.803080}, {1325.003367, 22.301451}, + {1326.105907, 21.800126}, {1327.183387, 21.299103}, + {1328.235824, 20.798377}, {1329.263232, 20.297945}, + {1330.265626, 19.797804}, {1331.243020, 19.297950}, + {1332.195428, 18.798381}, {1333.122865, 18.299093}, + {1334.025344, 17.800082}, {1334.902880, 17.301347}, + {1335.755486, 16.802883}, {1336.583175, 16.304688}, + {1337.385961, 15.806759}, {1338.163858, 15.309093}, + {1338.916877, 14.811688}, {1339.645033, 14.314540}, + {1340.348338, 13.817648}, {1341.026804, 13.321008}, + {1341.680445, 12.824618}, {1342.309272, 12.328476}, + {1342.913298, 11.832580}, {1343.492536, 11.336926}, + {1344.046997, 10.841514}, {1344.576693, 10.346340}, + {1345.081637, 9.851404}, {1345.561840, 9.356702}, + {1346.017313, 8.862233}, {1346.448069, 8.367995}, + {1346.854118, 7.873987}, {1347.235473, 7.380207}, + {1347.592145, 6.886653}, {1347.924144, 6.393324}, + {1348.231483, 5.900219}, {1348.514171, 5.407335}, + {1348.772222, 4.914673}, {1349.005644, 4.422231}, + {1349.214450, 3.930007}, {1349.398650, 3.438001}, + {1349.558256, 2.946213}, {1349.693277, 2.454640}, + {1349.803725, 1.963283}, {1349.889611, 1.472141}, + {1349.950945, 0.981214}, {1349.987737, 0.490500}, + {1349.998745, 0.156943}, {1349.999950, 0.031348}, + {1349.999998, 0.006229}, {1350.000000, 0.001206}, + {1350.000000, 0.000201}, {1350.000000, 0.000000}, + }}, + {318, + { + {3.091605, 194.582690}, {12.796400, 193.609132}, + {22.452651, 192.640879}, {32.060619, 191.677864}, + {41.620566, 190.720017}, {51.132749, 189.767273}, + {60.597419, 188.819566}, {70.014829, 187.876831}, + {79.385225, 186.939007}, {88.708851, 186.006030}, + {97.985948, 185.077841}, {107.216754, 184.154380}, + {116.401503, 183.235589}, {125.540428, 182.321409}, + {134.633758, 181.411784}, {143.681719, 180.506659}, + {152.684535, 179.605979}, {161.642426, 178.709691}, + {170.555612, 177.817742}, {179.424308, 176.930080}, + {188.248726, 176.046654}, {197.029078, 175.167414}, + {205.765571, 174.292312}, {214.458411, 173.421297}, + {223.107802, 172.554323}, {231.713943, 171.691343}, + {240.277035, 170.832311}, {248.797272, 169.977182}, + {257.274849, 169.125910}, {265.709958, 168.278451}, + {274.102789, 167.434764}, {282.453528, 166.594804}, + {290.762361, 165.758531}, {299.029472, 164.925902}, + {307.255042, 164.096878}, {315.439249, 163.271418}, + {323.582272, 162.449483}, {331.684284, 161.631034}, + {339.745461, 160.816032}, {347.765973, 160.004441}, + {355.745990, 159.196224}, {363.685679, 158.391343}, + {371.585206, 157.589762}, {379.444737, 156.791447}, + {387.264432, 155.996362}, {395.044453, 155.204473}, + {402.784958, 154.415746}, {410.486106, 153.630147}, + {418.148050, 152.847644}, {425.770946, 152.068204}, + {433.354946, 151.291795}, {440.900201, 150.518385}, + {448.406859, 149.747943}, {455.875069, 148.980439}, + {463.304976, 148.215841}, {470.696725, 147.454121}, + {478.050459, 146.695249}, {485.366320, 145.939195}, + {492.644448, 145.185931}, {499.884982, 144.435429}, + {507.088059, 143.687660}, {514.253816, 142.942598}, + {521.382386, 142.200214}, {528.473904, 141.460482}, + {535.528500, 140.723376}, {542.546306, 139.988869}, + {549.527451, 139.256936}, {556.472063, 138.527551}, + {563.380269, 137.800688}, {570.252195, 137.076324}, + {577.087964, 136.354433}, {583.887699, 135.634991}, + {590.651523, 134.917975}, {597.379557, 134.203360}, + {604.071919, 133.491124}, {610.728728, 132.781242}, + {617.350101, 132.073694}, {623.936155, 131.368455}, + {630.487004, 130.665504}, {637.002762, 129.964819}, + {643.483542, 129.266377}, {649.929455, 128.570159}, + {656.340613, 127.876141}, {662.717124, 127.184304}, + {669.059097, 126.494627}, {675.366640, 125.807090}, + {681.639859, 125.121671}, {687.878860, 124.438351}, + {694.083746, 123.757110}, {700.254622, 123.077929}, + {706.391590, 122.400789}, {712.494752, 121.725670}, + {718.564207, 121.052553}, {724.600057, 120.381419}, + {730.602398, 119.712251}, {736.571330, 119.045029}, + {742.506949, 118.379737}, {748.409352, 117.716355}, + {754.278632, 117.054866}, {760.114885, 116.395253}, + {765.918204, 115.737498}, {771.688681, 115.081585}, + {777.426408, 114.427495}, {783.131476, 113.775213}, + {788.803974, 113.124722}, {794.443992, 112.476004}, + {800.051619, 111.829045}, {805.626940, 111.183828}, + {811.170045, 110.540337}, {816.681017, 109.898556}, + {822.159943, 109.258470}, {827.606906, 108.620062}, + {833.021990, 107.983319}, {838.405279, 107.348224}, + {843.756854, 106.714762}, {849.076796, 106.082919}, + {854.365186, 105.452681}, {859.622103, 104.824031}, + {864.847628, 104.196957}, {870.041838, 103.571443}, + {875.204811, 102.947475}, {880.336624, 102.325040}, + {885.437353, 101.704124}, {890.507074, 101.084712}, + {895.545862, 100.466791}, {900.553790, 99.850348}, + {905.530933, 99.235369}, {910.477363, 98.621841}, + {915.393153, 98.009750}, {920.278374, 97.399085}, + {925.133097, 96.789831}, {929.957392, 96.181976}, + {934.751329, 95.575507}, {939.514977, 94.970412}, + {944.248404, 94.366678}, {948.951679, 93.764293}, + {953.624867, 93.163245}, {958.268036, 92.563521}, + {962.881252, 91.965110}, {967.464580, 91.367999}, + {972.018084, 90.772177}, {976.541829, 90.177632}, + {981.035879, 89.584352}, {985.500296, 88.992327}, + {989.935143, 88.401543}, {994.340481, 87.811991}, + {998.716372, 87.223658}, {1003.062877, 86.636534}, + {1007.380056, 86.050608}, {1011.667967, 85.465868}, + {1015.926672, 84.882304}, {1020.156227, 84.299905}, + {1024.356691, 83.718661}, {1028.528122, 83.138560}, + {1032.670575, 82.559592}, {1036.784109, 81.981747}, + {1040.868778, 81.405014}, {1044.924638, 80.829384}, + {1048.951744, 80.254845}, {1052.950149, 79.681388}, + {1056.919909, 79.109003}, {1060.861076, 78.537679}, + {1064.773703, 77.967408}, {1068.657843, 77.398178}, + {1072.513547, 76.829981}, {1076.340867, 76.262807}, + {1080.139853, 75.696646}, {1083.910556, 75.131488}, + {1087.653027, 74.567325}, {1091.367314, 74.004147}, + {1095.053466, 73.441944}, {1098.711532, 72.880708}, + {1102.341561, 72.320429}, {1105.943599, 71.761098}, + {1109.517694, 71.202706}, {1113.063893, 70.645244}, + {1116.582241, 70.088704}, {1120.072786, 69.533076}, + {1123.535571, 68.978352}, {1126.970643, 68.424523}, + {1130.378046, 67.871580}, {1133.757823, 67.319516}, + {1137.110019, 66.768320}, {1140.434677, 66.217986}, + {1143.731839, 65.668505}, {1147.001548, 65.119868}, + {1150.243847, 64.572066}, {1153.458776, 64.025093}, + {1156.646377, 63.478940}, {1159.806690, 62.933598}, + {1162.939756, 62.389060}, {1166.045616, 61.845318}, + {1169.124308, 61.302363}, {1172.175872, 60.760189}, + {1175.200346, 60.218787}, {1178.197770, 59.678149}, + {1181.168180, 59.138268}, {1184.111615, 58.599136}, + {1187.028112, 58.060746}, {1189.917708, 57.523090}, + {1192.780439, 56.986161}, {1195.616342, 56.449951}, + {1198.425452, 55.914453}, {1201.207805, 55.379659}, + {1203.963436, 54.845563}, {1206.692379, 54.312158}, + {1209.394668, 53.779435}, {1212.070339, 53.247388}, + {1214.719424, 52.716010}, {1217.341956, 52.185294}, + {1219.937970, 51.655233}, {1222.507496, 51.125820}, + {1225.050568, 50.597048}, {1227.567217, 50.068911}, + {1230.057474, 49.541401}, {1232.521372, 49.014512}, + {1234.958941, 48.488238}, {1237.370211, 47.962571}, + {1239.755213, 47.437506}, {1242.113977, 46.913035}, + {1244.446531, 46.389153}, {1246.752906, 45.865852}, + {1249.033131, 45.343127}, {1251.287233, 44.820971}, + {1253.515242, 44.299377}, {1255.717185, 43.778341}, + {1257.893090, 43.257855}, {1260.042984, 42.737913}, + {1262.166895, 42.218509}, {1264.264848, 41.699638}, + {1266.336872, 41.181292}, {1268.382991, 40.663467}, + {1270.403231, 40.146157}, {1272.397619, 39.629355}, + {1274.366179, 39.113055}, {1276.308937, 38.597253}, + {1278.225917, 38.081941}, {1280.117143, 37.567116}, + {1281.982640, 37.052770}, {1283.822432, 36.538898}, + {1285.636542, 36.025496}, {1287.424993, 35.512556}, + {1289.187809, 35.000075}, {1290.925012, 34.488046}, + {1292.636625, 33.976465}, {1294.322669, 33.465325}, + {1295.983168, 32.954622}, {1297.618142, 32.444351}, + {1299.227614, 31.934505}, {1300.811603, 31.425081}, + {1302.370132, 30.916074}, {1303.903221, 30.407477}, + {1305.410890, 29.899287}, {1306.893160, 29.391498}, + {1308.350050, 28.884106}, {1309.781580, 28.377105}, + {1311.187770, 27.870491}, {1312.568639, 27.364260}, + {1313.924206, 26.858407}, {1315.254489, 26.352926}, + {1316.559507, 25.847814}, {1317.839279, 25.343067}, + {1319.093823, 24.838679}, {1320.323156, 24.334646}, + {1321.527297, 23.830965}, {1322.706261, 23.327631}, + {1323.860068, 22.824639}, {1324.988734, 22.321986}, + {1326.092275, 21.819667}, {1327.170709, 21.317679}, + {1328.224051, 20.816017}, {1329.252319, 20.314679}, + {1330.255527, 19.813659}, {1331.233692, 19.312955}, + {1332.186830, 18.812562}, {1333.114956, 18.312477}, + {1334.018086, 17.812697}, {1334.896233, 17.313217}, + {1335.749415, 16.814035}, {1336.577644, 16.315148}, + {1337.380937, 15.816551}, {1338.159307, 15.318243}, + {1338.912768, 14.820219}, {1339.641336, 14.322477}, + {1340.345023, 13.825014}, {1341.023844, 13.327827}, + {1341.677812, 12.830914}, {1342.306942, 12.334270}, + {1342.911246, 11.837895}, {1343.490738, 11.341785}, + {1344.045431, 10.845938}, {1344.575339, 10.350352}, + {1345.080473, 9.855024}, {1345.560847, 9.359952}, + {1346.016474, 8.865134}, {1346.447367, 8.370568}, + {1346.853538, 7.876252}, {1347.234998, 7.382184}, + {1347.591762, 6.888363}, {1347.923841, 6.394787}, + {1348.231247, 5.901454}, {1348.513992, 5.408363}, + {1348.772089, 4.915512}, {1349.005550, 4.422901}, + {1349.214385, 3.930528}, {1349.398608, 3.438391}, + {1349.558230, 2.946491}, {1349.693263, 2.454825}, + {1349.803719, 1.963394}, {1349.889608, 1.472197}, + {1349.950944, 0.981232}, {1349.987737, 0.490500}, + {1349.998745, 0.156943}, {1349.999950, 0.031348}, + {1349.999998, 0.006229}, {1350.000000, 0.001206}, + {1350.000000, 0.000201}, {1350.000000, 0.000000}, + }}, + {316, + { + {2.097465, 199.279625}, {12.034724, 198.210703}, + {21.918713, 197.148886}, {31.749787, 196.094060}, + {41.528291, 195.046115}, {51.254568, 194.004945}, + {60.928953, 192.970443}, {70.551776, 191.942508}, + {80.123365, 190.921039}, {89.644039, 189.905938}, + {99.114116, 188.897109}, {108.533905, 187.894459}, + {117.903714, 186.897897}, {127.223844, 185.907331}, + {136.494595, 184.922675}, {145.716258, 183.943844}, + {154.889122, 182.970752}, {164.013474, 182.003318}, + {173.089594, 181.041462}, {182.117758, 180.085104}, + {191.098240, 179.134168}, {200.031308, 178.188578}, + {208.917229, 177.248259}, {217.756264, 176.313140}, + {226.548672, 175.383150}, {235.294706, 174.458217}, + {243.994618, 173.538276}, {252.648656, 172.623257}, + {261.257065, 171.713096}, {269.820086, 170.807729}, + {278.337956, 169.907092}, {286.810912, 169.011123}, + {295.239184, 168.119761}, {303.623001, 167.232948}, + {311.962591, 166.350624}, {320.258175, 165.472731}, + {328.509973, 164.599215}, {336.718204, 163.730019}, + {344.883082, 162.865088}, {353.004818, 162.004370}, + {361.083623, 161.147813}, {369.119702, 160.295364}, + {377.113261, 159.446973}, {385.064500, 158.602591}, + {392.973619, 157.762169}, {400.840814, 156.925658}, + {408.666281, 156.093012}, {416.450211, 155.264184}, + {424.192794, 154.439129}, {431.894217, 153.617801}, + {439.554666, 152.800158}, {447.174324, 151.986155}, + {454.753372, 151.175751}, {462.291988, 150.368902}, + {469.790350, 149.565569}, {477.248632, 148.765710}, + {484.667007, 147.969285}, {492.045645, 147.176256}, + {499.384716, 146.386583}, {506.684386, 145.600229}, + {513.944821, 144.817157}, {521.166183, 144.037328}, + {528.348634, 143.260708}, {535.492333, 142.487260}, + {542.597439, 141.716950}, {549.664106, 140.949741}, + {556.692489, 140.185602}, {563.682742, 139.424497}, + {570.635014, 138.666394}, {577.549455, 137.911260}, + {584.426214, 137.159063}, {591.265434, 136.409771}, + {598.067262, 135.663353}, {604.831841, 134.919778}, + {611.559311, 134.179016}, {618.249812, 133.441037}, + {624.903483, 132.705812}, {631.520461, 131.973311}, + {638.100882, 131.243506}, {644.644878, 130.516368}, + {651.152584, 129.791869}, {657.624131, 129.069983}, + {664.059647, 128.350681}, {670.459263, 127.633938}, + {676.823104, 126.919726}, {683.151298, 126.208020}, + {689.443968, 125.498793}, {695.701239, 124.792021}, + {701.923231, 124.087678}, {708.110067, 123.385740}, + {714.261865, 122.686182}, {720.378744, 121.988980}, + {726.460821, 121.294110}, {732.508212, 120.601549}, + {738.521033, 119.911273}, {744.499396, 119.223260}, + {750.443415, 118.537486}, {756.353200, 117.853930}, + {762.228863, 117.172569}, {768.070512, 116.493382}, + {773.878255, 115.816347}, {779.652200, 115.141442}, + {785.392452, 114.468647}, {791.099116, 113.797940}, + {796.772298, 113.129302}, {802.412098, 112.462711}, + {808.018619, 111.798148}, {813.591963, 111.135593}, + {819.132228, 110.475025}, {824.639515, 109.816427}, + {830.113920, 109.159777}, {835.555541, 108.505058}, + {840.964473, 107.852251}, {846.340813, 107.201337}, + {851.684654, 106.552297}, {856.996089, 105.905114}, + {862.275211, 105.259770}, {867.522112, 104.616246}, + {872.736881, 103.974525}, {877.919609, 103.334590}, + {883.070384, 102.696423}, {888.189295, 102.060007}, + {893.276428, 101.425326}, {898.331870, 100.792363}, + {903.355707, 100.161101}, {908.348023, 99.531524}, + {913.308901, 98.903616}, {918.238426, 98.277361}, + {923.136678, 97.652742}, {928.003740, 97.029744}, + {932.839693, 96.408352}, {937.644615, 95.788550}, + {942.418587, 95.170323}, {947.161687, 94.553656}, + {951.873991, 93.938534}, {956.555578, 93.324941}, + {961.206523, 92.712864}, {965.826902, 92.102288}, + {970.416789, 91.493199}, {974.976259, 90.885582}, + {979.505384, 90.279422}, {984.004237, 89.674707}, + {988.472890, 89.071422}, {992.911415, 88.469554}, + {997.319881, 87.869089}, {1001.698358, 87.270013}, + {1006.046917, 86.672313}, {1010.365624, 86.075976}, + {1014.654548, 85.480989}, {1018.913756, 84.887339}, + {1023.143315, 84.295012}, {1027.343290, 83.703997}, + {1031.513747, 83.114280}, {1035.654750, 82.525849}, + {1039.766364, 81.938692}, {1043.848651, 81.352796}, + {1047.901675, 80.768149}, {1051.925497, 80.184739}, + {1055.920179, 79.602553}, {1059.885783, 79.021581}, + {1063.822367, 78.441810}, {1067.729993, 77.863228}, + {1071.608720, 77.285825}, {1075.458605, 76.709588}, + {1079.279707, 76.134505}, {1083.072084, 75.560567}, + {1086.835792, 74.987761}, {1090.570888, 74.416076}, + {1094.277428, 73.845502}, {1097.955466, 73.276027}, + {1101.605057, 72.707641}, {1105.226257, 72.140332}, + {1108.819117, 71.574091}, {1112.383692, 71.008906}, + {1115.920034, 70.444767}, {1119.428195, 69.881664}, + {1122.908226, 69.319586}, {1126.360179, 68.758524}, + {1129.784104, 68.198466}, {1133.180050, 67.639403}, + {1136.548069, 67.081325}, {1139.888207, 66.524222}, + {1143.200515, 65.968084}, {1146.485040, 65.412901}, + {1149.741829, 64.858664}, {1152.970929, 64.305363}, + {1156.172388, 63.752988}, {1159.346251, 63.201531}, + {1162.492564, 62.650981}, {1165.611372, 62.101329}, + {1168.702719, 61.552566}, {1171.766650, 61.004684}, + {1174.803209, 60.457672}, {1177.812439, 59.911522}, + {1180.794383, 59.366224}, {1183.749083, 58.821771}, + {1186.676581, 58.278152}, {1189.576918, 57.735360}, + {1192.450137, 57.193386}, {1195.296277, 56.652221}, + {1198.115379, 56.111856}, {1200.907483, 55.572283}, + {1203.672627, 55.033494}, {1206.410851, 54.495479}, + {1209.122194, 53.958232}, {1211.806694, 53.421744}, + {1214.464387, 52.886006}, {1217.095313, 52.351010}, + {1219.699507, 51.816749}, {1222.277006, 51.283214}, + {1224.827846, 50.750397}, {1227.352063, 50.218291}, + {1229.849693, 49.686888}, {1232.320769, 49.156180}, + {1234.765328, 48.626159}, {1237.183402, 48.096818}, + {1239.575026, 47.568150}, {1241.940234, 47.040145}, + {1244.279057, 46.512799}, {1246.591530, 45.986101}, + {1248.877684, 45.460047}, {1251.137551, 44.934628}, + {1253.371162, 44.409837}, {1255.578550, 43.885666}, + {1257.759744, 43.362110}, {1259.914776, 42.839160}, + {1262.043675, 42.316810}, {1264.146472, 41.795053}, + {1266.223195, 41.273881}, {1268.273874, 40.753289}, + {1270.298538, 40.233269}, {1272.297215, 39.713815}, + {1274.269934, 39.194920}, {1276.216721, 38.676578}, + {1278.137605, 38.158781}, {1280.032613, 37.641524}, + {1281.901771, 37.124800}, {1283.745106, 36.608603}, + {1285.562644, 36.092926}, {1287.354411, 35.577763}, + {1289.120433, 35.063108}, {1290.860735, 34.548954}, + {1292.575341, 34.035297}, {1294.264277, 33.522129}, + {1295.927566, 33.009445}, {1297.565233, 32.497238}, + {1299.177302, 31.985504}, {1300.763795, 31.474236}, + {1302.324737, 30.963428}, {1303.860149, 30.453075}, + {1305.370055, 29.943171}, {1306.854477, 29.433711}, + {1308.313437, 28.924689}, {1309.746957, 28.416100}, + {1311.155058, 27.907938}, {1312.537762, 27.400198}, + {1313.895088, 26.892875}, {1315.227059, 26.385964}, + {1316.533695, 25.879459}, {1317.815015, 25.373356}, + {1319.071041, 24.867649}, {1320.301790, 24.362334}, + {1321.507284, 23.857405}, {1322.687540, 23.352859}, + {1323.842579, 22.848690}, {1324.972419, 22.344893}, + {1326.077077, 21.841465}, {1327.156574, 21.338400}, + {1328.210926, 20.835694}, {1329.240152, 20.333343}, + {1330.244269, 19.831342}, {1331.223295, 19.329688}, + {1332.177247, 18.828376}, {1333.106141, 18.327402}, + {1334.009995, 17.826762}, {1334.888826, 17.326453}, + {1335.742649, 16.826470}, {1336.571481, 16.326810}, + {1337.375338, 15.827468}, {1338.154236, 15.328443}, + {1338.908190, 14.829729}, {1339.637216, 14.331324}, + {1340.341330, 13.833225}, {1341.020546, 13.335428}, + {1341.674880, 12.837929}, {1342.304347, 12.340727}, + {1342.908960, 11.843818}, {1343.488736, 11.347199}, + {1344.043687, 10.850868}, {1344.573830, 10.354821}, + {1345.079176, 9.859057}, {1345.559742, 9.363572}, + {1346.015541, 8.868365}, {1346.446586, 8.373434}, + {1346.852891, 7.878775}, {1347.234470, 7.384387}, + {1347.591336, 6.890268}, {1347.923503, 6.396416}, + {1348.230985, 5.902830}, {1348.513793, 5.409507}, + {1348.771942, 4.916447}, {1349.005444, 4.423647}, + {1349.214313, 3.931107}, {1349.398561, 3.438825}, + {1349.558202, 2.946800}, {1349.693248, 2.455031}, + {1349.803712, 1.963518}, {1349.889606, 1.472258}, + {1349.950944, 0.981253}, {1349.987737, 0.490500}, + {1349.998745, 0.156943}, {1349.999950, 0.031348}, + {1349.999998, 0.006229}, {1350.000000, 0.001206}, + {1350.000000, 0.000201}, {1350.000000, 0.000000}, + }}, + {313, + { + {9.166003, 203.762698}, {19.324644, 202.582946}, + {29.424535, 201.412695}, {39.466146, 200.251763}, + {49.449940, 199.099976}, {59.376368, 197.957162}, + {69.245876, 196.823156}, {79.058900, 195.697795}, + {88.815868, 194.580923}, {98.517201, 193.472388}, + {108.163311, 192.372040}, {117.754606, 191.279736}, + {127.291482, 190.195333}, {136.774333, 189.118695}, + {146.203543, 188.049688}, {155.579489, 186.988182}, + {164.902545, 185.934050}, {174.173076, 184.887168}, + {183.391440, 183.847416}, {192.557993, 182.814676}, + {201.673080, 181.788833}, {210.737045, 180.769775}, + {219.750225, 179.757393}, {228.712949, 178.751580}, + {237.625544, 177.752233}, {246.488331, 176.759249}, + {255.301626, 175.772529}, {264.065738, 174.791976}, + {272.780975, 173.817495}, {281.447637, 172.848994}, + {290.066022, 171.886383}, {298.636421, 170.929572}, + {307.159122, 169.978476}, {315.634409, 169.033009}, + {324.062562, 168.093089}, {332.443855, 167.158635}, + {340.778560, 166.229568}, {349.066944, 165.305810}, + {357.309272, 164.387286}, {365.505802, 163.473921}, + {373.656791, 162.565642}, {381.762491, 161.662378}, + {389.823152, 160.764059}, {397.839019, 159.870618}, + {405.810334, 158.981987}, {413.737337, 158.098101}, + {421.620261, 157.218895}, {429.459341, 156.344306}, + {437.254806, 155.474273}, {445.006881, 154.608735}, + {452.715790, 153.747633}, {460.381754, 152.890908}, + {468.004989, 152.038503}, {475.585711, 151.190362}, + {483.124131, 150.346430}, {490.620458, 149.506654}, + {498.074898, 148.670980}, {505.487657, 147.839356}, + {512.858934, 147.011731}, {520.188929, 146.188055}, + {527.477837, 145.368279}, {534.725853, 144.552354}, + {541.933167, 143.740232}, {549.099970, 142.931868}, + {556.226447, 142.127216}, {563.312783, 141.326229}, + {570.359161, 140.528864}, {577.365759, 139.735077}, + {584.332757, 138.944826}, {591.260329, 138.158069}, + {598.148650, 137.374763}, {604.997891, 136.594868}, + {611.808221, 135.818344}, {618.579808, 135.045152}, + {625.312818, 134.275253}, {632.007415, 133.508609}, + {638.663760, 132.745182}, {645.282013, 131.984935}, + {651.862332, 131.227832}, {658.404874, 130.473838}, + {664.909793, 129.722915}, {671.377241, 128.975031}, + {677.807371, 128.230151}, {684.200330, 127.488240}, + {690.556268, 126.749267}, {696.875330, 126.013198}, + {703.157660, 125.280000}, {709.403401, 124.549643}, + {715.612694, 123.822095}, {721.785680, 123.097325}, + {727.922496, 122.375303}, {734.023278, 121.655999}, + {740.088163, 120.939382}, {746.117283, 120.225425}, + {752.110771, 119.514098}, {758.068758, 118.805372}, + {763.991372, 118.099221}, {769.878743, 117.395617}, + {775.730997, 116.694531}, {781.548259, 115.995938}, + {787.330652, 115.299811}, {793.078301, 114.606123}, + {798.791325, 113.914849}, {804.469845, 113.225964}, + {810.113981, 112.539443}, {815.723848, 111.855259}, + {821.299564, 111.173390}, {826.841244, 110.493811}, + {832.349002, 109.816498}, {837.822950, 109.141427}, + {843.263200, 108.468575}, {848.669863, 107.797919}, + {854.043047, 107.129437}, {859.382860, 106.463105}, + {864.689410, 105.798902}, {869.962803, 105.136806}, + {875.203143, 104.476795}, {880.410534, 103.818847}, + {885.585079, 103.162942}, {890.726879, 102.509059}, + {895.836035, 101.857177}, {900.912646, 101.207275}, + {905.956811, 100.559333}, {910.968628, 99.913332}, + {915.948192, 99.269251}, {920.895600, 98.627071}, + {925.810947, 97.986773}, {930.694324, 97.348338}, + {935.545826, 96.711746}, {940.365545, 96.076979}, + {945.153570, 95.444019}, {949.909991, 94.812847}, + {954.634899, 94.183446}, {959.328380, 93.555796}, + {963.990522, 92.929881}, {968.621411, 92.305683}, + {973.221132, 91.683185}, {977.789771, 91.062369}, + {982.327411, 90.443219}, {986.834134, 89.825717}, + {991.310023, 89.209848}, {995.755160, 88.595594}, + {1000.169623, 87.982940}, {1004.553493, 87.371868}, + {1008.906849, 86.762364}, {1013.229768, 86.154411}, + {1017.522328, 85.547994}, {1021.784606, 84.943097}, + {1026.016676, 84.339705}, {1030.218613, 83.737802}, + {1034.390493, 83.137374}, {1038.532387, 82.538406}, + {1042.644369, 81.940882}, {1046.726511, 81.344788}, + {1050.778884, 80.750111}, {1054.801557, 80.156834}, + {1058.794602, 79.564945}, {1062.758086, 78.974429}, + {1066.692079, 78.385271}, {1070.596647, 77.797459}, + {1074.471858, 77.210979}, {1078.317778, 76.625816}, + {1082.134472, 76.041958}, {1085.922006, 75.459391}, + {1089.680443, 74.878102}, {1093.409848, 74.298077}, + {1097.110282, 73.719305}, {1100.781809, 73.141771}, + {1104.424490, 72.565464}, {1108.038386, 71.990370}, + {1111.623557, 71.416477}, {1115.180063, 70.843773}, + {1118.707964, 70.272246}, {1122.207317, 69.701882}, + {1125.678181, 69.132671}, {1129.120612, 68.564599}, + {1132.534669, 67.997656}, {1135.920406, 67.431829}, + {1139.277879, 66.867107}, {1142.607144, 66.303479}, + {1145.908254, 65.740931}, {1149.181264, 65.179455}, + {1152.426226, 64.619037}, {1155.643194, 64.059667}, + {1158.832219, 63.501334}, {1161.993353, 62.944026}, + {1165.126647, 62.387733}, {1168.232151, 61.832445}, + {1171.309916, 61.278149}, {1174.359991, 60.724837}, + {1177.382424, 60.172496}, {1180.377264, 59.621117}, + {1183.344559, 59.070689}, {1186.284357, 58.521202}, + {1189.196703, 57.972646}, {1192.081644, 57.425011}, + {1194.939227, 56.878287}, {1197.769496, 56.332463}, + {1200.572495, 55.787529}, {1203.348271, 55.243477}, + {1206.096865, 54.700296}, {1208.818322, 54.157977}, + {1211.512684, 53.616510}, {1214.179994, 53.075886}, + {1216.820293, 52.536095}, {1219.433624, 51.997128}, + {1222.020026, 51.458975}, {1224.579542, 50.921628}, + {1227.112209, 50.385078}, {1229.618069, 49.849315}, + {1232.097160, 49.314331}, {1234.549521, 48.780116}, + {1236.975191, 48.246662}, {1239.374206, 47.713961}, + {1241.746606, 47.182003}, {1244.092425, 46.650779}, + {1246.411702, 46.120283}, {1248.704471, 45.590504}, + {1250.970770, 45.061435}, {1253.210632, 44.533067}, + {1255.424094, 44.005392}, {1257.611189, 43.478403}, + {1259.771951, 42.952090}, {1261.906414, 42.426446}, + {1264.014612, 41.901464}, {1266.096577, 41.377134}, + {1268.152342, 40.853449}, {1270.181938, 40.330402}, + {1272.185398, 39.807985}, {1274.162752, 39.286191}, + {1276.114032, 38.765011}, {1278.039268, 38.244438}, + {1279.938491, 37.724465}, {1281.811730, 37.205085}, + {1283.659014, 36.686290}, {1285.480373, 36.168074}, + {1287.275836, 35.650428}, {1289.045430, 35.133346}, + {1290.789184, 34.616822}, {1292.507126, 34.100847}, + {1294.199283, 33.585415}, {1295.865681, 33.070520}, + {1297.506348, 32.556155}, {1299.121310, 32.042313}, + {1300.710592, 31.528987}, {1302.274221, 31.016171}, + {1303.812222, 30.503859}, {1305.324619, 29.992044}, + {1306.811438, 29.480720}, {1308.272703, 28.969881}, + {1309.708438, 28.459520}, {1311.118667, 27.949631}, + {1312.503413, 27.440210}, {1313.862700, 26.931248}, + {1315.196549, 26.422742}, {1316.504985, 25.914684}, + {1317.788029, 25.407070}, {1319.045703, 24.899893}, + {1320.278029, 24.393148}, {1321.485028, 23.886830}, + {1322.666723, 23.380933}, {1323.823132, 22.875452}, + {1324.954278, 22.370382}, {1326.060180, 21.865717}, + {1327.140860, 21.361452}, {1328.196336, 20.857583}, + {1329.226628, 20.354104}, {1330.231756, 19.851011}, + {1331.211738, 19.348300}, {1332.166595, 18.845964}, + {1333.096344, 18.344000}, {1334.001004, 17.842404}, + {1334.880594, 17.341170}, {1335.735130, 16.840296}, + {1336.564632, 16.339775}, {1337.369117, 15.839605}, + {1338.148601, 15.339782}, {1338.903103, 14.840301}, + {1339.632640, 14.341158}, {1340.337227, 13.842351}, + {1341.016883, 13.343874}, {1341.671623, 12.845726}, + {1342.301464, 12.347902}, {1342.906421, 11.850399}, + {1343.486512, 11.353214}, {1344.041751, 10.856344}, + {1344.572154, 10.359786}, {1345.077737, 9.863537}, + {1345.558515, 9.367594}, {1346.014504, 8.871954}, + {1346.445718, 8.376616}, {1346.852173, 7.881576}, + {1347.233883, 7.386832}, {1347.590864, 6.892383}, + {1347.923129, 6.398225}, {1348.230693, 5.904357}, + {1348.513572, 5.410778}, {1348.771778, 4.917484}, + {1349.005327, 4.424476}, {1349.214233, 3.931750}, + {1349.398509, 3.439307}, {1349.558171, 2.947144}, + {1349.693231, 2.455260}, {1349.803704, 1.963655}, + {1349.889603, 1.472327}, {1349.950943, 0.981275}, + {1349.987737, 0.490500}, {1349.998745, 0.156943}, + {1349.999950, 0.031348}, {1349.999998, 0.006229}, + {1350.000000, 0.001206}, {1350.000000, 0.000201}, + {1350.000000, 0.000000}, + }}, + {311, + { + {3.792133, 210.541580}, {14.285893, 209.208804}, + {24.713345, 207.889278}, {35.075144, 206.582698}, + {45.371931, 205.288769}, {55.604330, 204.007205}, + {65.772953, 202.737732}, {75.878399, 201.480082}, + {85.921251, 200.233998}, {95.902082, 198.999231}, + {105.821451, 197.775540}, {115.679907, 196.562689}, + {125.477985, 195.360454}, {135.216212, 194.168615}, + {144.895101, 192.986959}, {154.515157, 191.815281}, + {164.076874, 190.653379}, {173.580735, 189.501061}, + {183.027215, 188.358139}, {192.416779, 187.224429}, + {201.749883, 186.099754}, {211.026976, 184.983943}, + {220.248495, 183.876827}, {229.414872, 182.778244}, + {238.526529, 181.688037}, {247.583881, 180.606051}, + {256.587336, 179.532137}, {265.537293, 178.466150}, + {274.434146, 177.407949}, {283.278279, 176.357395}, + {292.070073, 175.314355}, {300.809899, 174.278698}, + {309.498124, 173.250297}, {318.135107, 172.229029}, + {326.721202, 171.214773}, {335.256757, 170.207411}, + {343.742113, 169.206829}, {352.177606, 168.212915}, + {360.563568, 167.225560}, {368.900324, 166.244657}, + {377.188193, 165.270104}, {385.427490, 164.301799}, + {393.618526, 163.339642}, {401.761606, 162.383538}, + {409.857029, 161.433393}, {417.905092, 160.489114}, + {425.906085, 159.550612}, {433.860295, 158.617798}, + {441.768005, 157.690587}, {449.629492, 156.768895}, + {457.445030, 155.852641}, {465.214890, 154.941743}, + {472.939337, 154.036123}, {480.618632, 153.135705}, + {488.253035, 152.240413}, {495.842800, 151.350174}, + {503.388177, 150.464917}, {510.889414, 149.584570}, + {518.346755, 148.709064}, {525.760440, 147.838333}, + {533.130706, 146.972310}, {540.457787, 146.110930}, + {547.741914, 145.254129}, {554.983313, 144.401846}, + {562.182210, 143.554020}, {569.338825, 142.710590}, + {576.453377, 141.871498}, {583.526082, 141.036686}, + {590.557151, 140.206099}, {597.546796, 139.379680}, + {604.495222, 138.557376}, {611.402635, 137.739132}, + {618.269236, 136.924898}, {625.095224, 136.114621}, + {631.880795, 135.308251}, {638.626145, 134.505739}, + {645.331464, 133.707035}, {651.996943, 132.912093}, + {658.622767, 132.120865}, {665.209121, 131.333305}, + {671.756188, 130.549368}, {678.264147, 129.769009}, + {684.733177, 128.992184}, {691.163453, 128.218851}, + {697.555148, 127.448968}, {703.908435, 126.682492}, + {710.223482, 125.919382}, {716.500456, 125.159599}, + {722.739524, 124.403102}, {728.940847, 123.649854}, + {735.104589, 122.899815}, {741.230908, 122.152947}, + {747.319962, 121.409214}, {753.371907, 120.668579}, + {759.386897, 119.931005}, {765.365083, 119.196458}, + {771.306617, 118.464903}, {777.211647, 117.736305}, + {783.080321, 117.010629}, {788.912783, 116.287844}, + {794.709177, 115.567915}, {800.469645, 114.850811}, + {806.194328, 114.136499}, {811.883364, 113.424949}, + {817.536891, 112.716128}, {823.155044, 112.010006}, + {828.737958, 111.306554}, {834.285765, 110.605741}, + {839.798597, 109.907538}, {845.276584, 109.211915}, + {850.719853, 108.518846}, {856.128531, 107.828300}, + {861.502745, 107.140251}, {866.842618, 106.454671}, + {872.148273, 105.771533}, {877.419832, 105.090811}, + {882.657414, 104.412477}, {887.861139, 103.736506}, + {893.031123, 103.062872}, {898.167484, 102.391549}, + {903.270335, 101.722514}, {908.339791, 101.055740}, + {913.375965, 100.391204}, {918.378967, 99.728881}, + {923.348908, 99.068748}, {928.285896, 98.410781}, + {933.190040, 97.754956}, {938.061445, 97.101252}, + {942.900217, 96.449644}, {947.706461, 95.800111}, + {952.480280, 95.152630}, {957.221775, 94.507180}, + {961.931048, 93.863738}, {966.608198, 93.222284}, + {971.253325, 92.582797}, {975.866527, 91.945254}, + {980.447899, 91.309637}, {984.997538, 90.675923}, + {989.515538, 90.044093}, {994.001994, 89.414128}, + {998.456997, 88.786006}, {1002.880640, 88.159709}, + {1007.273013, 87.535218}, {1011.634206, 86.912512}, + {1015.964309, 86.291574}, {1020.263408, 85.672384}, + {1024.531590, 85.054924}, {1028.768943, 84.439175}, + {1032.975550, 83.825120}, {1037.151497, 83.212740}, + {1041.296866, 82.602018}, {1045.411739, 81.992937}, + {1049.496200, 81.385478}, {1053.550327, 80.779624}, + {1057.574202, 80.175359}, {1061.567902, 79.572665}, + {1065.531507, 78.971527}, {1069.465094, 78.371927}, + {1073.368738, 77.773848}, {1077.242516, 77.177276}, + {1081.086503, 76.582193}, {1084.900772, 75.988584}, + {1088.685398, 75.396433}, {1092.440452, 74.805725}, + {1096.166006, 74.216444}, {1099.862131, 73.628575}, + {1103.528898, 73.042102}, {1107.166376, 72.457012}, + {1110.774634, 71.873288}, {1114.353739, 71.290916}, + {1117.903759, 70.709882}, {1121.424760, 70.130171}, + {1124.916809, 69.551770}, {1128.379969, 68.974663}, + {1131.814307, 68.398837}, {1135.219885, 67.824277}, + {1138.596766, 67.250971}, {1141.945013, 66.678905}, + {1145.264687, 66.108064}, {1148.555850, 65.538436}, + {1151.818561, 64.970008}, {1155.052880, 64.402765}, + {1158.258866, 63.836696}, {1161.436579, 63.271787}, + {1164.586074, 62.708026}, {1167.707410, 62.145400}, + {1170.800642, 61.583896}, {1173.865827, 61.023502}, + {1176.903020, 60.464206}, {1179.912275, 59.905994}, + {1182.893646, 59.348857}, {1185.847187, 58.792780}, + {1188.772950, 58.237753}, {1191.670988, 57.683763}, + {1194.541352, 57.130799}, {1197.384093, 56.578850}, + {1200.199262, 56.027903}, {1202.986908, 55.477948}, + {1205.747081, 54.928973}, {1208.479830, 54.380968}, + {1211.185202, 53.833920}, {1213.863246, 53.287819}, + {1216.514007, 52.742654}, {1219.137534, 52.198414}, + {1221.733872, 51.655089}, {1224.303066, 51.112668}, + {1226.845161, 50.571141}, {1229.360202, 50.030497}, + {1231.848232, 49.490725}, {1234.309296, 48.951816}, + {1236.743435, 48.413759}, {1239.150693, 47.876545}, + {1241.531111, 47.340163}, {1243.884730, 46.804604}, + {1246.211591, 46.269857}, {1248.511736, 45.735914}, + {1250.785203, 45.202764}, {1253.032032, 44.670398}, + {1255.252262, 44.138806}, {1257.445931, 43.607980}, + {1259.613079, 43.077909}, {1261.753741, 42.548585}, + {1263.867956, 42.019998}, {1265.955759, 41.492141}, + {1268.017188, 40.965002}, {1270.052277, 40.438575}, + {1272.061063, 39.912849}, {1274.043579, 39.387817}, + {1275.999861, 38.863470}, {1277.929943, 38.339799}, + {1279.833858, 37.816795}, {1281.711639, 37.294451}, + {1283.563319, 36.772758}, {1285.388931, 36.251708}, + {1287.188506, 35.731293}, {1288.962076, 35.211505}, + {1290.709672, 34.692335}, {1292.431325, 34.173777}, + {1294.127065, 33.655821}, {1295.796922, 33.138461}, + {1297.440926, 32.621689}, {1299.059105, 32.105498}, + {1300.651490, 31.589879}, {1302.218107, 31.074825}, + {1303.758986, 30.560330}, {1305.274154, 30.046385}, + {1306.763638, 29.532985}, {1308.227466, 29.020121}, + {1309.665664, 28.507787}, {1311.078258, 27.995976}, + {1312.465274, 27.484681}, {1313.826739, 26.973896}, + {1315.162676, 26.463614}, {1316.473112, 25.953827}, + {1317.758071, 25.444531}, {1319.017578, 24.935718}, + {1320.251655, 24.427382}, {1321.460328, 23.919518}, + {1322.643619, 23.412118}, {1323.801551, 22.905178}, + {1324.934148, 22.398690}, {1326.041431, 21.892650}, + {1327.123424, 21.387051}, {1328.180147, 20.881888}, + {1329.211623, 20.377156}, {1330.217873, 19.872849}, + {1331.198919, 19.368961}, {1332.154780, 18.865488}, + {1333.085478, 18.362424}, {1333.991032, 17.859764}, + {1334.871464, 17.357504}, {1335.726793, 16.855638}, + {1336.557038, 16.354162}, {1337.362218, 15.853071}, + {1338.142354, 15.352361}, {1338.897464, 14.852028}, + {1339.627566, 14.352066}, {1340.332680, 13.852472}, + {1341.012823, 13.353242}, {1341.668013, 12.854372}, + {1342.298269, 12.355858}, {1342.903608, 11.857696}, + {1343.484047, 11.359883}, {1344.039605, 10.862416}, + {1344.570297, 10.365290}, {1345.076142, 9.868503}, + {1345.557156, 9.372051}, {1346.013355, 8.875932}, + {1346.444757, 8.380143}, {1346.851378, 7.884680}, + {1347.233233, 7.389542}, {1347.590340, 6.894726}, + {1347.922714, 6.400229}, {1348.230371, 5.906049}, + {1348.513327, 5.412184}, {1348.771597, 4.918633}, + {1349.005198, 4.425393}, {1349.214144, 3.932462}, + {1349.398452, 3.439840}, {1349.558136, 2.947524}, + {1349.693212, 2.455513}, {1349.803695, 1.963806}, + {1349.889600, 1.472402}, {1349.950942, 0.981301}, + {1349.987737, 0.490500}, {1349.998745, 0.156943}, + {1349.999950, 0.031348}, {1349.999998, 0.006229}, + {1350.000000, 0.001206}, {1350.000000, 0.000201}, + {1350.000000, 0.000000}, + }}, + {308, + { + {6.273664, 217.425154}, {17.106924, 215.905259}, + {27.864656, 214.404008}, {38.547778, 212.920883}, + {49.157185, 211.455386}, {59.693745, 210.007041}, + {70.158306, 208.575391}, {80.551691, 207.159996}, + {90.874702, 205.760432}, {101.128120, 204.376293}, + {111.312707, 203.007187}, {121.429205, 201.652739}, + {131.478338, 200.312585}, {141.460812, 198.986376}, + {151.377316, 197.673775}, {161.228522, 196.374457}, + {171.015086, 195.088109}, {180.737649, 193.814429}, + {190.396838, 192.553125}, {199.993264, 191.303916}, + {209.527525, 190.066528}, {219.000206, 188.840700}, + {228.411878, 187.626176}, {237.763100, 186.422711}, + {247.054419, 185.230067}, {256.286371, 184.048014}, + {265.459480, 182.876329}, {274.574258, 181.714795}, + {283.631208, 180.563204}, {292.630822, 179.421354}, + {301.573582, 178.289048}, {310.459961, 177.166095}, + {319.290421, 176.052311}, {328.065417, 174.947517}, + {336.785393, 173.851538}, {345.450787, 172.764207}, + {354.062026, 171.685359}, {362.619531, 170.614834}, + {371.123713, 169.552479}, {379.574979, 168.498143}, + {387.973725, 167.451680}, {396.320340, 166.412948}, + {404.615209, 165.381808}, {412.858707, 164.358126}, + {421.051205, 163.341771}, {429.193065, 162.332616}, + {437.284643, 161.330537}, {445.326292, 160.335413}, + {453.318356, 159.347125}, {461.261173, 158.365561}, + {469.155077, 157.390607}, {477.000396, 156.422156}, + {484.797452, 155.460099}, {492.546563, 154.504335}, + {500.248041, 153.554762}, {507.902192, 152.611281}, + {515.509319, 151.673796}, {523.069719, 150.742212}, + {530.583685, 149.816439}, {538.051506, 148.896387}, + {545.473465, 147.981967}, {552.849841, 147.073095}, + {560.180911, 146.169686}, {567.466944, 145.271659}, + {574.708209, 144.378934}, {581.904968, 143.491432}, + {589.057481, 142.609078}, {596.166003, 141.731796}, + {603.230786, 140.859513}, {610.252077, 139.992158}, + {617.230123, 139.129660}, {624.165163, 138.271951}, + {631.057436, 137.418962}, {637.907176, 136.570629}, + {644.714614, 135.726887}, {651.479978, 134.887672}, + {658.203492, 134.052923}, {664.885380, 133.222578}, + {671.525859, 132.396578}, {678.125145, 131.574865}, + {684.683451, 130.757381}, {691.200987, 129.944069}, + {697.677961, 129.134876}, {704.114577, 128.329745}, + {710.511036, 127.528625}, {716.867538, 126.731463}, + {723.184280, 125.938208}, {729.461455, 125.148810}, + {735.699256, 124.363218}, {741.897871, 123.581385}, + {748.057487, 122.803263}, {754.178289, 122.028805}, + {760.260458, 121.257964}, {766.304175, 120.490696}, + {772.309616, 119.726957}, {778.276958, 118.966701}, + {784.206372, 118.209886}, {790.098031, 117.456470}, + {795.952103, 116.706411}, {801.768755, 115.959669}, + {807.548152, 115.216202}, {813.290456, 114.475971}, + {818.995829, 113.738938}, {824.664429, 113.005063}, + {830.296413, 112.274309}, {835.891937, 111.546638}, + {841.451153, 110.822014}, {846.974214, 110.100400}, + {852.461268, 109.381762}, {857.912463, 108.666063}, + {863.327947, 107.953270}, {868.707862, 107.243348}, + {874.052352, 106.536264}, {879.361559, 105.831985}, + {884.635620, 105.130477}, {889.874675, 104.431710}, + {895.078859, 103.735651}, {900.248307, 103.042268}, + {905.383152, 102.351532}, {910.483525, 101.663411}, + {915.549558, 100.977876}, {920.581377, 100.294898}, + {925.579111, 99.614446}, {930.542884, 98.936493}, + {935.472822, 98.261009}, {940.369046, 97.587967}, + {945.231679, 96.917339}, {950.060839, 96.249098}, + {954.856647, 95.583216}, {959.619219, 94.919668}, + {964.348672, 94.258427}, {969.045119, 93.599466}, + {973.708675, 92.942761}, {978.339451, 92.288286}, + {982.937559, 91.636016}, {987.503107, 90.985926}, + {992.036205, 90.337992}, {996.536960, 89.692190}, + {1001.005477, 89.048495}, {1005.441861, 88.406886}, + {1009.846217, 87.767337}, {1014.218646, 87.129827}, + {1018.559250, 86.494333}, {1022.868129, 85.860831}, + {1027.145382, 85.229301}, {1031.391108, 84.599719}, + {1035.605402, 83.972065}, {1039.788362, 83.346317}, + {1043.940081, 82.722454}, {1048.060654, 82.100454}, + {1052.150173, 81.480298}, {1056.208729, 80.861964}, + {1060.236414, 80.245432}, {1064.233317, 79.630683}, + {1068.199526, 79.017697}, {1072.135130, 78.406453}, + {1076.040215, 77.796933}, {1079.914866, 77.189117}, + {1083.759169, 76.582987}, {1087.573206, 75.978523}, + {1091.357062, 75.375707}, {1095.110818, 74.774521}, + {1098.834555, 74.174947}, {1102.528352, 73.576966}, + {1106.192291, 72.980561}, {1109.826447, 72.385714}, + {1113.430900, 71.792407}, {1117.005726, 71.200624}, + {1120.551000, 70.610347}, {1124.066798, 70.021560}, + {1127.553193, 69.434245}, {1131.010259, 68.848387}, + {1134.438068, 68.263968}, {1137.836692, 67.680973}, + {1141.206200, 67.099385}, {1144.546665, 66.519189}, + {1147.858154, 65.940368}, {1151.140736, 65.362908}, + {1154.394478, 64.786792}, {1157.619448, 64.212006}, + {1160.815712, 63.638534}, {1163.983334, 63.066361}, + {1167.122380, 62.495473}, {1170.232913, 61.925854}, + {1173.314997, 61.357491}, {1176.368693, 60.790368}, + {1179.394064, 60.224472}, {1182.391170, 59.659788}, + {1185.360073, 59.096302}, {1188.300830, 58.534001}, + {1191.213502, 57.972870}, {1194.098146, 57.412897}, + {1196.954820, 56.854067}, {1199.783581, 56.296367}, + {1202.584485, 55.739785}, {1205.357587, 55.184306}, + {1208.102943, 54.629918}, {1210.820606, 54.076608}, + {1213.510630, 53.524363}, {1216.173069, 52.973172}, + {1218.807973, 52.423020}, {1221.415396, 51.873896}, + {1223.995388, 51.325788}, {1226.548000, 50.778683}, + {1229.073281, 50.232569}, {1231.571282, 49.687435}, + {1234.042049, 49.143268}, {1236.485632, 48.600057}, + {1238.902078, 48.057791}, {1241.291435, 47.516457}, + {1243.653747, 46.976045}, {1245.989062, 46.436544}, + {1248.297424, 45.897941}, {1250.578878, 45.360226}, + {1252.833469, 44.823388}, {1255.061239, 44.287417}, + {1257.262232, 43.752301}, {1259.436490, 43.218029}, + {1261.584055, 42.684592}, {1263.704970, 42.151979}, + {1265.799274, 41.620180}, {1267.867008, 41.089184}, + {1269.908212, 40.558980}, {1271.922925, 40.029560}, + {1273.911187, 39.500914}, {1275.873036, 38.973030}, + {1277.808509, 38.445901}, {1279.717645, 37.919515}, + {1281.600479, 37.393864}, {1283.457049, 36.868937}, + {1285.287391, 36.344727}, {1287.091539, 35.821223}, + {1288.869530, 35.298416}, {1290.621398, 34.776298}, + {1292.347177, 34.254858}, {1294.046901, 33.734089}, + {1295.720603, 33.213982}, {1297.368315, 32.694528}, + {1298.990072, 32.175718}, {1300.585903, 31.657544}, + {1302.155842, 31.139998}, {1303.699918, 30.623071}, + {1305.218164, 30.106756}, {1306.710609, 29.591043}, + {1308.177283, 29.075925}, {1309.618216, 28.561395}, + {1311.033437, 28.047444}, {1312.422975, 27.534064}, + {1313.786858, 27.021249}, {1315.125114, 26.508990}, + {1316.437770, 25.997281}, {1317.724855, 25.486114}, + {1318.986395, 24.975481}, {1320.222417, 24.465376}, + {1321.432946, 23.955792}, {1322.618009, 23.446722}, + {1323.777631, 22.938159}, {1324.911837, 22.430096}, + {1326.020653, 21.922527}, {1327.104102, 21.415446}, + {1328.162209, 20.908846}, {1329.194998, 20.402721}, + {1330.202493, 19.897064}, {1331.184716, 19.391871}, + {1332.141692, 18.887134}, {1333.073441, 18.382848}, + {1333.979988, 17.879008}, {1334.861353, 17.375608}, + {1335.717559, 16.872642}, {1336.548628, 16.370105}, + {1337.354580, 15.867993}, {1338.135438, 15.366299}, + {1338.891221, 14.865020}, {1339.621950, 14.364150}, + {1340.327646, 13.863684}, {1341.008328, 13.363618}, + {1341.664017, 12.863947}, {1342.294733, 12.364668}, + {1342.900494, 11.865776}, {1343.481320, 11.367267}, + {1344.037230, 10.869137}, {1344.568243, 10.371382}, + {1345.074377, 9.873999}, {1345.555652, 9.376984}, + {1346.012085, 8.880333}, {1346.443694, 8.384045}, + {1346.850498, 7.888114}, {1347.232515, 7.392539}, + {1347.589761, 6.897317}, {1347.922255, 6.402445}, + {1348.230014, 5.907920}, {1348.513056, 5.413740}, + {1348.771397, 4.919903}, {1349.005055, 4.426407}, + {1349.214046, 3.933250}, {1349.398388, 3.440429}, + {1349.558097, 2.947944}, {1349.693191, 2.455793}, + {1349.803685, 1.963974}, {1349.889596, 1.472486}, + {1349.950942, 0.981328}, {1349.987737, 0.490500}, + {1349.998745, 0.156943}, {1349.999950, 0.031348}, + {1349.999998, 0.006229}, {1350.000000, 0.001206}, + {1350.000000, 0.000201}, {1350.000000, 0.000000}, + }}, + {304, + { + {44.734552, 220.000000}, {44.734552, 220.000000}, + {44.734552, 220.000000}, {50.392600, 219.126265}, + {61.307148, 217.455638}, {72.138760, 215.808872}, + {82.888612, 214.185209}, {93.557841, 212.583928}, + {104.147547, 211.004336}, {114.658800, 209.445775}, + {125.092635, 207.907614}, {135.450056, 206.389251}, + {145.732040, 204.890110}, {155.939534, 203.409637}, + {166.073458, 201.947304}, {176.134705, 200.502604}, + {186.124147, 199.075052}, {196.042628, 197.664180}, + {205.890971, 196.269541}, {215.669977, 194.890706}, + {225.380426, 193.527261}, {235.023078, 192.178809}, + {244.598672, 190.844969}, {254.107931, 189.525374}, + {263.551557, 188.219669}, {272.930236, 186.927516}, + {282.244639, 185.648585}, {291.495418, 184.382560}, + {300.683210, 183.129138}, {309.808639, 181.888025}, + {318.872313, 180.658936}, {327.874826, 179.441598}, + {336.816760, 178.235747}, {345.698682, 177.041128}, + {354.521148, 175.857494}, {363.284700, 174.684607}, + {371.989871, 173.522235}, {380.637181, 172.370156}, + {389.227139, 171.228154}, {397.760243, 170.096021}, + {406.236982, 168.973552}, {414.657835, 167.860553}, + {423.023270, 166.756833}, {431.333746, 165.662208}, + {439.589713, 164.576500}, {447.791614, 163.499535}, + {455.939881, 162.431145}, {464.034939, 161.371166}, + {472.077204, 160.319441}, {480.067086, 159.275816}, + {488.004984, 158.240140}, {495.891295, 157.212269}, + {503.726403, 156.192061}, {511.510689, 155.179378}, + {519.244526, 154.174088}, {526.928279, 153.176061}, + {534.562310, 152.185168}, {542.146971, 151.201289}, + {549.682611, 150.224301}, {557.169571, 149.254089}, + {564.608187, 148.290539}, {571.998789, 147.333540}, + {579.341702, 146.382983}, {586.637245, 145.438764}, + {593.885734, 144.500779}, {601.087477, 143.568927}, + {608.242778, 142.643111}, {615.351936, 141.723236}, + {622.415247, 140.809208}, {629.433001, 139.900935}, + {636.405483, 138.998330}, {643.332973, 138.101303}, + {650.215750, 137.209772}, {657.054086, 136.323652}, + {663.848249, 135.442863}, {670.598503, 134.567324}, + {677.305111, 133.696959}, {683.968327, 132.831690}, + {690.588405, 131.971445}, {697.165595, 131.116149}, + {703.700142, 130.265731}, {710.192288, 129.420122}, + {716.642273, 128.579253}, {723.050330, 127.743058}, + {729.416694, 126.911470}, {735.741591, 126.084425}, + {742.025248, 125.261860}, {748.267887, 124.443713}, + {754.469728, 123.629924}, {760.630987, 122.820433}, + {766.751878, 122.015182}, {772.832610, 121.214113}, + {778.873392, 120.417171}, {784.874429, 119.624300}, + {790.835923, 118.835445}, {796.758073, 118.050555}, + {802.641076, 117.269576}, {808.485127, 116.492458}, + {814.290417, 115.719149}, {820.057136, 114.949602}, + {825.785470, 114.183766}, {831.475604, 113.421594}, + {837.127720, 112.663039}, {842.741997, 111.908055}, + {848.318613, 111.156597}, {853.857744, 110.408619}, + {859.359561, 109.664078}, {864.824236, 108.922931}, + {870.251938, 108.185136}, {875.642833, 107.450649}, + {880.997085, 106.719430}, {886.314856, 105.991439}, + {891.596308, 105.266636}, {896.841599, 104.544981}, + {902.050884, 103.826436}, {907.224319, 103.110962}, + {912.362056, 102.398522}, {917.464246, 101.689079}, + {922.531038, 100.982597}, {927.562579, 100.279040}, + {932.559014, 99.578372}, {937.520488, 98.880558}, + {942.447141, 98.185565}, {947.339114, 97.493358}, + {952.196545, 96.803904}, {957.019572, 96.117170}, + {961.808329, 95.433123}, {966.562951, 94.751732}, + {971.283568, 94.072965}, {975.970312, 93.396791}, + {980.623311, 92.723179}, {985.242693, 92.052099}, + {989.828584, 91.383520}, {994.381107, 90.717414}, + {998.900386, 90.053752}, {1003.386543, 89.392503}, + {1007.839696, 88.733641}, {1012.259966, 88.077137}, + {1016.647468, 87.422964}, {1021.002320, 86.771094}, + {1025.324635, 86.121500}, {1029.614526, 85.474156}, + {1033.872106, 84.829035}, {1038.097484, 84.186111}, + {1042.290771, 83.545359}, {1046.452074, 82.906754}, + {1050.581500, 82.270270}, {1054.679153, 81.635883}, + {1058.745140, 81.003568}, {1062.779562, 80.373302}, + {1066.782521, 79.745059}, {1070.754117, 79.118818}, + {1074.694452, 78.494554}, {1078.603622, 77.872245}, + {1082.481725, 77.251867}, {1086.328856, 76.633399}, + {1090.145112, 76.016817}, {1093.930585, 75.402101}, + {1097.685368, 74.789229}, {1101.409553, 74.178178}, + {1105.103231, 73.568928}, {1108.766490, 72.961458}, + {1112.399420, 72.355746}, {1116.002108, 71.751773}, + {1119.574641, 71.149519}, {1123.117103, 70.548962}, + {1126.629579, 69.950084}, {1130.112152, 69.352864}, + {1133.564906, 68.757283}, {1136.987921, 68.163322}, + {1140.381278, 67.570963}, {1143.745057, 66.980185}, + {1147.079336, 66.390971}, {1150.384193, 65.803302}, + {1153.659704, 65.217161}, {1156.905947, 64.632528}, + {1160.122995, 64.049386}, {1163.310922, 63.467718}, + {1166.469803, 62.887505}, {1169.599709, 62.308732}, + {1172.700711, 61.731380}, {1175.772882, 61.155433}, + {1178.816289, 60.580874}, {1181.831003, 60.007686}, + {1184.817092, 59.435854}, {1187.774622, 58.865360}, + {1190.703661, 58.296189}, {1193.604274, 57.728325}, + {1196.476526, 57.161752}, {1199.320481, 56.596455}, + {1202.136203, 56.032418}, {1204.923754, 55.469625}, + {1207.683196, 54.908063}, {1210.414590, 54.347715}, + {1213.117998, 53.788567}, {1215.793477, 53.230605}, + {1218.441087, 52.673813}, {1221.060887, 52.118178}, + {1223.652934, 51.563686}, {1226.217284, 51.010321}, + {1228.753994, 50.458071}, {1231.263118, 49.906921}, + {1233.744713, 49.356858}, {1236.198831, 48.807868}, + {1238.625526, 48.259939}, {1241.024851, 47.713055}, + {1243.396858, 47.167206}, {1245.741597, 46.622377}, + {1248.059120, 46.078555}, {1250.349478, 45.535729}, + {1252.612718, 44.993885}, {1254.848890, 44.453011}, + {1257.058043, 43.913094}, {1259.240223, 43.374123}, + {1261.395479, 42.836085}, {1263.523855, 42.298968}, + {1265.625398, 41.762761}, {1267.700153, 41.227452}, + {1269.748165, 40.693028}, {1271.769478, 40.159479}, + {1273.764135, 39.626793}, {1275.732179, 39.094960}, + {1277.673652, 38.563967}, {1279.588596, 38.033804}, + {1281.477053, 37.504460}, {1283.339062, 36.975924}, + {1285.174665, 36.448186}, {1286.983901, 35.921235}, + {1288.766808, 35.395060}, {1290.523426, 34.869651}, + {1292.253792, 34.344999}, {1293.957944, 33.821092}, + {1295.635920, 33.297922}, {1297.287755, 32.775478}, + {1298.913485, 32.253750}, {1300.513147, 31.732728}, + {1302.086776, 31.212405}, {1303.634405, 30.692768}, + {1305.156070, 30.173811}, {1306.651803, 29.655523}, + {1308.121638, 29.137895}, {1309.565609, 28.620919}, + {1310.983746, 28.104586}, {1312.376083, 27.588886}, + {1313.742651, 27.073812}, {1315.083480, 26.559354}, + {1316.398601, 26.045505}, {1317.688045, 25.532256}, + {1318.951842, 25.019600}, {1320.190020, 24.507527}, + {1321.402609, 23.996031}, {1322.589637, 23.485104}, + {1323.751133, 22.974737}, {1324.887125, 22.464924}, + {1325.997639, 21.955657}, {1327.082704, 21.446928}, + {1328.142345, 20.938731}, {1329.176590, 20.431058}, + {1330.185464, 19.923904}, {1331.168993, 19.417260}, + {1332.127203, 18.911120}, {1333.060118, 18.405478}, + {1333.967763, 17.900328}, {1334.850163, 17.395663}, + {1335.707341, 16.891476}, {1336.539322, 16.387763}, + {1337.346129, 15.884517}, {1338.127785, 15.381733}, + {1338.884314, 14.879404}, {1339.615737, 14.377527}, + {1340.322077, 13.876094}, {1341.003357, 13.375102}, + {1341.659599, 12.874544}, {1342.290823, 12.374417}, + {1342.897051, 11.874716}, {1343.478305, 11.375436}, + {1344.034605, 10.876572}, {1344.565972, 10.378120}, + {1345.072427, 9.880077}, {1345.553990, 9.382438}, + {1346.010681, 8.885200}, {1346.442520, 8.388359}, + {1346.849527, 7.891910}, {1347.231721, 7.395852}, + {1347.589122, 6.900181}, {1347.921748, 6.404894}, + {1348.229620, 5.909987}, {1348.512757, 5.415459}, + {1348.771176, 4.921306}, {1349.004897, 4.427527}, + {1349.213938, 3.934119}, {1349.398318, 3.441080}, + {1349.558055, 2.948408}, {1349.693168, 2.456101}, + {1349.803674, 1.964159}, {1349.889593, 1.472578}, + {1349.950941, 0.981359}, {1349.987737, 0.490500}, + {1349.998745, 0.156943}, {1349.999950, 0.031348}, + {1349.999998, 0.006229}, {1350.000000, 0.001206}, + {1350.000000, 0.000201}, {1350.000000, 0.000000}, + }}, + {295, + { + {100.494899, 220.000000}, {100.494899, 220.000000}, + {104.472997, 219.331216}, {115.394163, 217.515425}, + {126.225279, 215.729201}, {136.967796, 213.971491}, + {147.623116, 212.241292}, {158.192590, 210.537656}, + {168.677523, 208.859680}, {179.079178, 207.206504}, + {189.398773, 205.577313}, {199.637489, 203.971327}, + {209.796468, 202.387807}, {219.876814, 200.826045}, + {229.879599, 199.285367}, {239.805862, 197.765130}, + {249.656608, 196.264720}, {259.432815, 194.783548}, + {269.135430, 193.321054}, {278.765373, 191.876699}, + {288.323540, 190.449970}, {297.810799, 189.040373}, + {307.227994, 187.647437}, {316.575948, 186.270708}, + {325.855459, 184.909753}, {335.067307, 183.564154}, + {344.212248, 182.233510}, {353.291022, 180.917438}, + {362.304347, 179.615567}, {371.252925, 178.327543}, + {380.137439, 177.053022}, {388.958557, 175.791675}, + {397.716928, 174.543187}, {406.413189, 173.307249}, + {415.047959, 172.083570}, {423.621845, 170.871864}, + {432.135438, 169.671857}, {440.589317, 168.483286}, + {448.984046, 167.305895}, {457.320180, 166.139438}, + {465.598258, 164.983676}, {473.818809, 163.838379}, + {481.982352, 162.703324}, {490.089392, 161.578297}, + {498.140427, 160.463089}, {506.135941, 159.357498}, + {514.076412, 158.261329}, {521.962305, 157.174391}, + {529.794077, 156.096503}, {537.572177, 155.027486}, + {545.297044, 153.967167}, {552.969107, 152.915379}, + {560.588791, 151.871960}, {568.156509, 150.836753}, + {575.672667, 149.809602}, {583.137666, 148.790361}, + {590.551898, 147.778885}, {597.915746, 146.775032}, + {605.229588, 145.778667}, {612.493796, 144.789656}, + {619.708734, 143.807870}, {626.874761, 142.833183}, + {633.992227, 141.865472}, {641.061479, 140.904619}, + {648.082857, 139.950506}, {655.056696, 139.003021}, + {661.983322, 138.062052}, {668.863061, 137.127493}, + {675.696229, 136.199238}, {682.483140, 135.277185}, + {689.224100, 134.361233}, {695.919413, 133.451285}, + {702.569377, 132.547246}, {709.174283, 131.649022}, + {715.734422, 130.756524}, {722.250077, 129.869661}, + {728.721527, 128.988347}, {735.149048, 128.112497}, + {741.532911, 127.242028}, {747.873383, 126.376859}, + {754.170727, 125.516911}, {760.425203, 124.662105}, + {766.637065, 123.812366}, {772.806564, 122.967620}, + {778.933950, 122.127793}, {785.019465, 121.292813}, + {791.063350, 120.462612}, {797.065844, 119.637120}, + {803.027178, 118.816270}, {808.947585, 117.999997}, + {814.827291, 117.188236}, {820.666520, 116.380923}, + {826.465493, 115.577997}, {832.224428, 114.779396}, + {837.943539, 113.985062}, {843.623039, 113.194934}, + {849.263136, 112.408956}, {854.864037, 111.627071}, + {860.425944, 110.849224}, {865.949059, 110.075360}, + {871.433579, 109.305425}, {876.879698, 108.539368}, + {882.287611, 107.777136}, {887.657506, 107.018679}, + {892.989572, 106.263946}, {898.283993, 105.512889}, + {903.540952, 104.765459}, {908.760628, 104.021610}, + {913.943201, 103.281293}, {919.088845, 102.544465}, + {924.197733, 101.811078}, {929.270038, 101.081090}, + {934.305926, 100.354455}, {939.305566, 99.631132}, + {944.269121, 98.911078}, {949.196754, 98.194251}, + {954.088626, 97.480611}, {958.944894, 96.770116}, + {963.765715, 96.062727}, {968.551243, 95.358404}, + {973.301631, 94.657110}, {978.017029, 93.958807}, + {982.697586, 93.263455}, {987.343448, 92.571020}, + {991.954760, 91.881464}, {996.531665, 91.194751}, + {1001.074305, 90.510847}, {1005.582819, 89.829716}, + {1010.057345, 89.151324}, {1014.498019, 88.475636}, + {1018.904976, 87.802621}, {1023.278347, 87.132244}, + {1027.618265, 86.464473}, {1031.924859, 85.799276}, + {1036.198256, 85.136622}, {1040.438584, 84.476479}, + {1044.645966, 83.818816}, {1048.820527, 83.163604}, + {1052.962387, 82.510812}, {1057.071668, 81.860410}, + {1061.148487, 81.212369}, {1065.192963, 80.566661}, + {1069.205211, 79.923257}, {1073.185346, 79.282129}, + {1077.133480, 78.643249}, {1081.049726, 78.006590}, + {1084.934194, 77.372125}, {1088.786993, 76.739826}, + {1092.608230, 76.109668}, {1096.398012, 75.481625}, + {1100.156445, 74.855671}, {1103.883631, 74.231780}, + {1107.579674, 73.609928}, {1111.244674, 72.990089}, + {1114.878732, 72.372239}, {1118.481947, 71.756354}, + {1122.054416, 71.142410}, {1125.596236, 70.530383}, + {1129.107502, 69.920249}, {1132.588308, 69.311987}, + {1136.038747, 68.705573}, {1139.458911, 68.100983}, + {1142.848890, 67.498198}, {1146.208775, 66.897193}, + {1149.538653, 66.297947}, {1152.838613, 65.700439}, + {1156.108740, 65.104648}, {1159.349120, 64.510552}, + {1162.559837, 63.918130}, {1165.740975, 63.327363}, + {1168.892614, 62.738229}, {1172.014838, 62.150709}, + {1175.107725, 61.564782}, {1178.171355, 60.980429}, + {1181.205807, 60.397631}, {1184.211157, 59.816369}, + {1187.187482, 59.236622}, {1190.134857, 58.658373}, + {1193.053356, 58.081602}, {1195.943053, 57.506292}, + {1198.804021, 56.932425}, {1201.636331, 56.359981}, + {1204.440054, 55.788944}, {1207.215260, 55.219295}, + {1209.962018, 54.651018}, {1212.680396, 54.084095}, + {1215.370461, 53.518509}, {1218.032280, 52.954243}, + {1220.665918, 52.391280}, {1223.271440, 51.829604}, + {1225.848910, 51.269199}, {1228.398391, 50.710048}, + {1230.919946, 50.152136}, {1233.413636, 49.595446}, + {1235.879521, 49.039963}, {1238.317662, 48.485671}, + {1240.728117, 47.932556}, {1243.110946, 47.380601}, + {1245.466206, 46.829793}, {1247.793954, 46.280115}, + {1250.094245, 45.731554}, {1252.367137, 45.184095}, + {1254.612682, 44.637723}, {1256.830936, 44.092425}, + {1259.021951, 43.548186}, {1261.185781, 43.004992}, + {1263.322476, 42.462830}, {1265.432089, 41.921685}, + {1267.514670, 41.381546}, {1269.570268, 40.842397}, + {1271.598934, 40.304227}, {1273.600715, 39.767021}, + {1275.575660, 39.230768}, {1277.523815, 38.695454}, + {1279.445228, 38.161066}, {1281.339945, 37.627594}, + {1283.208010, 37.095023}, {1285.049469, 36.563341}, + {1286.864366, 36.032538}, {1288.652745, 35.502601}, + {1290.414648, 34.973517}, {1292.150118, 34.445277}, + {1293.859196, 33.917867}, {1295.541925, 33.391277}, + {1297.198344, 32.865495}, {1298.828494, 32.340510}, + {1300.432415, 31.816313}, {1302.010145, 31.292890}, + {1303.561723, 30.770233}, {1305.087187, 30.248330}, + {1306.586574, 29.727171}, {1308.059922, 29.206745}, + {1309.507267, 28.687043}, {1310.928645, 28.168055}, + {1312.324090, 27.649770}, {1313.693639, 27.132180}, + {1315.037325, 26.615274}, {1316.355183, 26.099042}, + {1317.647246, 25.583477}, {1318.913547, 25.068567}, + {1320.154119, 24.554305}, {1321.368994, 24.040682}, + {1322.558203, 23.527688}, {1323.721778, 23.015315}, + {1324.859750, 22.503555}, {1325.972149, 21.992399}, + {1327.059005, 21.481840}, {1328.120347, 20.971868}, + {1329.156206, 20.462476}, {1330.166609, 19.953656}, + {1331.151586, 19.445401}, {1332.111163, 18.937704}, + {1333.045370, 18.430556}, {1333.954232, 17.923950}, + {1334.837778, 17.417881}, {1335.696034, 16.912340}, + {1336.529025, 16.407321}, {1337.336779, 15.902817}, + {1338.119320, 15.398823}, {1338.876673, 14.895331}, + {1339.608865, 14.392336}, {1340.315919, 13.889831}, + {1340.997860, 13.387811}, {1341.654712, 12.886271}, + {1342.286499, 12.385205}, {1342.893245, 11.884606}, + {1343.474972, 11.384472}, {1344.031703, 10.884795}, + {1344.563462, 10.385573}, {1345.070272, 9.886799}, + {1345.552153, 9.388469}, {1346.009130, 8.890580}, + {1346.441222, 8.393127}, {1346.848453, 7.896106}, + {1347.230844, 7.399514}, {1347.588415, 6.903346}, + {1347.921189, 6.407599}, {1348.229186, 5.912271}, + {1348.512426, 5.417357}, {1348.770932, 4.922856}, + {1349.004722, 4.428764}, {1349.213818, 3.935079}, + {1349.398240, 3.441798}, {1349.558008, 2.948920}, + {1349.693142, 2.456442}, {1349.803662, 1.964363}, + {1349.889588, 1.472680}, {1349.950940, 0.981393}, + {1349.987737, 0.490500}, {1349.998745, 0.156943}, + {1349.999950, 0.031348}, {1349.999998, 0.006229}, + {1350.000000, 0.001206}, {1350.000000, 0.000201}, + {1350.000000, 0.000000}, + }}, + {288, + { + {155.385104, 220.000000}, {155.385104, 220.000000}, + {155.385104, 220.000000}, {157.579183, 219.597702}, + {168.509644, 217.620747}, {179.342172, 215.680361}, + {190.078558, 213.775084}, {200.720524, 211.903539}, + {211.269723, 210.064429}, {221.727747, 208.256524}, + {232.096127, 206.478664}, {242.376337, 204.729749}, + {252.569799, 203.008739}, {262.677884, 201.314647}, + {272.701913, 199.646537}, {282.643165, 198.003522}, + {292.502872, 196.384756}, {302.282226, 194.789437}, + {311.982383, 193.216803}, {321.604456, 191.666126}, + {331.149527, 190.136714}, {340.618642, 188.627908}, + {350.012817, 187.139077}, {359.333034, 185.669622}, + {368.580249, 184.218967}, {377.755387, 182.786566}, + {386.859349, 181.371893}, {395.893007, 179.974448}, + {404.857212, 178.593749}, {413.752790, 177.229337}, + {422.580542, 175.880771}, {431.341252, 174.547628}, + {440.035680, 173.229502}, {448.664568, 171.926005}, + {457.228637, 170.636762}, {465.728592, 169.361414}, + {474.165117, 168.099616}, {482.538884, 166.851034}, + {490.850543, 165.615350}, {499.100733, 164.392255}, + {507.290076, 163.181453}, {515.419179, 161.982657}, + {523.488635, 160.795593}, {531.499025, 159.619994}, + {539.450915, 158.455603}, {547.344859, 157.302174}, + {555.181400, 156.159466}, {562.961068, 155.027248}, + {570.684382, 153.905297}, {578.351849, 152.793396}, + {585.963967, 151.691337}, {593.521224, 150.598917}, + {601.024095, 149.515939}, {608.473049, 148.442214}, + {615.868543, 147.377558}, {623.211027, 146.321793}, + {630.500940, 145.274744}, {637.738715, 144.236245}, + {644.924775, 143.206133}, {652.059534, 142.184248}, + {659.143401, 141.170439}, {666.176776, 140.164554}, + {673.160051, 139.166450}, {680.093612, 138.175986}, + {686.977837, 137.193023}, {693.813099, 136.217429}, + {700.599761, 135.249073}, {707.338184, 134.287830}, + {714.028719, 133.333576}, {720.671713, 132.386192}, + {727.267507, 131.445559}, {733.816435, 130.511566}, + {740.318827, 129.584099}, {746.775005, 128.663051}, + {753.185290, 127.748317}, {759.549992, 126.839793}, + {765.869422, 125.937378}, {772.143880, 125.040975}, + {778.373667, 124.150487}, {784.559075, 123.265820}, + {790.700392, 122.386883}, {796.797904, 121.513586}, + {802.851890, 120.645842}, {808.862625, 119.783565}, + {814.830381, 118.926671}, {820.755424, 118.075078}, + {826.638019, 117.228706}, {832.478424, 116.387476}, + {838.276893, 115.551312}, {844.033680, 114.720138}, + {849.749030, 113.893881}, {855.423189, 113.072468}, + {861.056396, 112.255828}, {866.648889, 111.443893}, + {872.200901, 110.636593}, {877.712663, 109.833863}, + {883.184400, 109.035636}, {888.616337, 108.241850}, + {894.008695, 107.452439}, {899.361689, 106.667344}, + {904.675535, 105.886503}, {909.950444, 105.109856}, + {915.186624, 104.337346}, {920.384281, 103.568914}, + {925.543616, 102.804505}, {930.664831, 102.044063}, + {935.748120, 101.287533}, {940.793680, 100.534862}, + {945.801702, 99.785998}, {950.772374, 99.040888}, + {955.705883, 98.299483}, {960.602414, 97.561732}, + {965.462147, 96.827585}, {970.285261, 96.096995}, + {975.071934, 95.369915}, {979.822339, 94.646296}, + {984.536649, 93.926094}, {989.215033, 93.209263}, + {993.857658, 92.495759}, {998.464691, 91.785536}, + {1003.036293, 91.078554}, {1007.572626, 90.374768}, + {1012.073849, 89.674137}, {1016.540118, 88.976620}, + {1020.971588, 88.282177}, {1025.368411, 87.590766}, + {1029.730739, 86.902349}, {1034.058720, 86.216887}, + {1038.352501, 85.534342}, {1042.612226, 84.854675}, + {1046.838039, 84.177851}, {1051.030081, 83.503831}, + {1055.188492, 82.832581}, {1059.313408, 82.164063}, + {1063.404965, 81.498245}, {1067.463299, 80.835089}, + {1071.488540, 80.174564}, {1075.480820, 79.516634}, + {1079.440268, 78.861267}, {1083.367010, 78.208430}, + {1087.261173, 77.558091}, {1091.122881, 76.910217}, + {1094.952256, 76.264778}, {1098.749419, 75.621742}, + {1102.514489, 74.981078}, {1106.247585, 74.342757}, + {1109.948823, 73.706749}, {1113.618317, 73.073024}, + {1117.256181, 72.441553}, {1120.862528, 71.812308}, + {1124.437467, 71.185260}, {1127.981108, 70.560381}, + {1131.493559, 69.937644}, {1134.974925, 69.317021}, + {1138.425313, 68.698485}, {1141.844825, 68.082011}, + {1145.233565, 67.467571}, {1148.591633, 66.855140}, + {1151.919128, 66.244692}, {1155.216151, 65.636202}, + {1158.482797, 65.029644}, {1161.719163, 64.424996}, + {1164.925344, 63.822231}, {1168.101432, 63.221325}, + {1171.247522, 62.622256}, {1174.363703, 62.024999}, + {1177.450067, 61.429532}, {1180.506701, 60.835831}, + {1183.533693, 60.243874}, {1186.531131, 59.653638}, + {1189.499100, 59.065101}, {1192.437683, 58.478241}, + {1195.346965, 57.893038}, {1198.227028, 57.309468}, + {1201.077952, 56.727511}, {1203.899819, 56.147147}, + {1206.692706, 55.568355}, {1209.456693, 54.991113}, + {1212.191856, 54.415403}, {1214.898271, 53.841204}, + {1217.576014, 53.268497}, {1220.225158, 52.697261}, + {1222.845776, 52.127479}, {1225.437941, 51.559130}, + {1228.001724, 50.992195}, {1230.537196, 50.426658}, + {1233.044425, 49.862498}, {1235.523480, 49.299697}, + {1237.974428, 48.738239}, {1240.397337, 48.178104}, + {1242.792271, 47.619276}, {1245.159296, 47.061737}, + {1247.498477, 46.505470}, {1249.809875, 45.950458}, + {1252.093553, 45.396684}, {1254.349574, 44.844131}, + {1256.577997, 44.292784}, {1258.778882, 43.742625}, + {1260.952288, 43.193639}, {1263.098275, 42.645811}, + {1265.216898, 42.099123}, {1267.308215, 41.553562}, + {1269.372282, 41.009111}, {1271.409154, 40.465755}, + {1273.418884, 39.923480}, {1275.401528, 39.382270}, + {1277.357138, 38.842112}, {1279.285765, 38.302990}, + {1281.187462, 37.764891}, {1283.062280, 37.227799}, + {1284.910267, 36.691702}, {1286.731474, 36.156586}, + {1288.525950, 35.622436}, {1290.293742, 35.089240}, + {1292.034897, 34.556984}, {1293.749463, 34.025655}, + {1295.437486, 33.495240}, {1297.099010, 32.965726}, + {1298.734081, 32.437101}, {1300.342742, 31.909353}, + {1301.925037, 31.382468}, {1303.481010, 30.856435}, + {1305.010702, 30.331242}, {1306.514155, 29.806876}, + {1307.991410, 29.283327}, {1309.442508, 28.760583}, + {1310.867488, 28.238632}, {1312.266390, 27.717463}, + {1313.639254, 27.197066}, {1314.986116, 26.677428}, + {1316.307015, 26.158541}, {1317.601989, 25.640392}, + {1318.871073, 25.122972}, {1320.114304, 24.606270}, + {1321.331717, 24.090276}, {1322.523349, 23.574980}, + {1323.689233, 23.060374}, {1324.829403, 22.546446}, + {1325.943894, 22.033187}, {1327.032738, 21.520589}, + {1328.095969, 21.008642}, {1329.133619, 20.497337}, + {1330.145719, 19.986666}, {1331.132301, 19.476619}, + {1332.093396, 18.967189}, {1333.029035, 18.458366}, + {1333.939248, 17.950144}, {1334.824064, 17.442513}, + {1335.683514, 16.935467}, {1336.517625, 16.428998}, + {1337.326428, 15.923098}, {1338.109949, 15.417759}, + {1338.868217, 14.912976}, {1339.601260, 14.408740}, + {1340.309105, 13.905046}, {1340.991778, 13.401887}, + {1341.649307, 12.899256}, {1342.281717, 12.397148}, + {1342.889034, 11.895555}, {1343.471285, 11.394473}, + {1344.028494, 10.893896}, {1344.560687, 10.393819}, + {1345.067889, 9.894235}, {1345.550123, 9.395141}, + {1346.007415, 8.896531}, {1346.439788, 8.398401}, + {1346.847267, 7.900746}, {1347.229875, 7.403562}, + {1347.587635, 6.906844}, {1347.920571, 6.410590}, + {1348.228705, 5.914795}, {1348.512061, 5.419455}, + {1348.770662, 4.924568}, {1349.004529, 4.430130}, + {1349.213686, 3.936139}, {1349.398154, 3.442592}, + {1349.557956, 2.949486}, {1349.693114, 2.456818}, + {1349.803649, 1.964588}, {1349.889584, 1.472793}, + {1349.950939, 0.981430}, {1349.987737, 0.490500}, + {1349.998745, 0.156943}, {1349.999950, 0.031348}, + {1349.999998, 0.006229}, {1350.000000, 0.001206}, + {1350.000000, 0.000201}, {1350.000000, 0.000000}, + }}, +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/trajectories/Trajectory.h b/src/boards/DeathStack/AirBrakes/trajectories/Trajectory.h new file mode 100644 index 0000000000000000000000000000000000000000..b617f37986482411d16524e08c557890b8b88991 --- /dev/null +++ b/src/boards/DeathStack/AirBrakes/trajectories/Trajectory.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#ifdef EUROC +#include "TrajectoriesEuroc.h" +#else +#include "TrajectoriesRoccaraso.h" +#endif + +#include "TrajectoryPoint.h" + +namespace DeathStackBoard +{ + +class Trajectory +{ +private: + uint8_t index; // trajectory id, from 0 to 9, generated with openings from + // 10% to 90% + float s_bar; // fixed area for trajectory generation + // if error = 0, this is the needed airbrakes area + +public: + Trajectory(uint8_t index, float s_max) + : index(index), + // (index + 1) / 10 : airbrakes opening percentage, from 0.1 to 0.9 + s_bar(((index + 1) / 10.0f) * s_max) + { + } + + Trajectory() : index(0), s_bar(0.0f) {} + + Trajectory& operator=(const Trajectory& other) + { + this->index = other.index; + this->s_bar = other.s_bar; + return *this; + } + + uint32_t length() { return TRAJECTORIES_DATA[index].length; } + + TrajectoryPoint get(uint32_t idx) + { + point_t point = TRAJECTORIES_DATA[index].data[idx]; + return TrajectoryPoint(point.z, point.vz); + } + + uint8_t getTrajectoryIndex() { return index; } + + float getRefSurface() { return s_bar; } +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.h b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoryPoint.h similarity index 55% rename from src/boards/DeathStack/DeploymentController/Motor/MotorDriver.h rename to src/boards/DeathStack/AirBrakes/trajectories/TrajectoryPoint.h index 686595819863e19e5b850c1665bddc17f862b53c..92281da44d6b96bc373a93b0e96a8a54ff4ea028 100644 --- a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.h +++ b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoryPoint.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise de'Faveri Tron +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,57 +13,47 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#pragma once -#include <Common.h> +#pragma once -#include "DeathStack/configs/MotorConfig.h" -#include "MotorData.h" +#include <math.h> namespace DeathStackBoard { -/** - * This class gives access to the H-Bridge that controls the DC motor of the nosecone. - */ -class MotorDriver +class TrajectoryPoint { public: - /** - * @brief Class constructor. - */ - MotorDriver(); + TrajectoryPoint() : TrajectoryPoint(0, 0) {} + TrajectoryPoint(float z, float vz) : z(z), vz(vz) {} - /** - * @brief Class destructor. - */ - ~MotorDriver(); + float getZ() { return z; } + float getVz() { return vz; } - /** - * @brief Activates the H-Bridge to start the motor. - * - * @param direction direction of activation (normal or reverse) - */ - void start(MotorDirection direction); + static float distance(TrajectoryPoint a, TrajectoryPoint b) + { + return powf(a.getZ() - b.getZ(), 2) + powf(a.getVz() - b.getVz(), 2); + } - /** - * @brief Stop the motor - */ - void stop(); + static float zDistance(TrajectoryPoint a, TrajectoryPoint b) + { + return abs(a.getZ() - b.getZ()); + } - MotorStatus getStatus() + static float vzDistance(TrajectoryPoint a, TrajectoryPoint b) { - return status; + return abs(a.getVz() - b.getVz()); } private: - MotorStatus status; + float z; + float vz; }; -} /* namespace */ \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Algorithm.h b/src/boards/DeathStack/Algorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..b8347916fc47cd70c9d68ca8ed504ccdaafe1af7 --- /dev/null +++ b/src/boards/DeathStack/Algorithm.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace DeathStackBoard +{ + +class Algorithm +{ +public: + /** + * @brief Initializes the Algorithm object, must be called as soon as the + * object is created. + * */ + virtual bool init() = 0; + + /** + * @brief Starts the execution of the algorithm and set the running flag to + * true. + * */ + void begin() { running = true; } + + /** + * @brief Terminates the algorithm's execution and sets the running flag to + * false. + * */ + void end() { running = false; } + + /** + * @brief Checks wether the algorithm is in a running state or not, and + * eventually calls the @see{step} routine. + * */ + void update() + { + if (running) + { + step(); + } + } + +protected: + /** + * @brief The actual algorithm step. + */ + virtual void step() = 0; + + bool running = false; +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADA.scxml b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADA.scxml new file mode 100644 index 0000000000000000000000000000000000000000..640dcf44e3d968a666600888f87a3db7c2f837b1 --- /dev/null +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADA.scxml @@ -0,0 +1,31 @@ +<?xml version="1.0" encoding="UTF-8"?> +<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="idle" version="1.0"> + <state id="idle"> + <transition event="ADA.EV_CALIBRATE_ADA" target="calibrating"/> + </state> + <state id="calibrating"> + <onentry>resetCalibration()</onentry> + <transition event="ADA.EV_CALIBRATE_ADA" target="calibrating"/> + <transition event="ADA.EV_ADA_READY" target="ready"/> + </state> + <state id="ready"> + <transition event="ADA.EV_LIFTOFF" target="shadowMode"/> + </state> + <state id="shadowMode"> + <onentry>postD(ADA.EV_SHADOW_MODE_TIMEOUT)</onentry> + <onexit>removeD(ADA.EV_SHADOW_MODE_TIMEOUT)</onexit> + <transition event="ADA.EV_SHADOW_MODE_TIMEOUT" target="activeMode"/> + </state> + <state id="activeMode"> + <transition event="ADA.EV_ADA_APOGEE_DETECTED" target="pressureStabilization"/> + </state> + <state id="pressureStabilization"> + <onentry>postD(ADA.EV_TIMEOUT_PRESS_STABILIZATION)</onentry> + <transition event="ADA.EV_TIMEOUT_PRESS_STABILIZATION" target="drogueDescent"/> + </state> + <state id="drogueDescent"> + <transition event="ADA.EV_ADA_DPL_ALT_DETECTED" target=""/> + </state> + <state id="end"> + </state> +</scxml> diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0edebf458453a23051ef704e9079b147798eb82e --- /dev/null +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp @@ -0,0 +1,140 @@ +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> +#include <Debug.h> +#include <TimestampTimer.h> +#include <configs/ADAConfig.h> +#include <diagnostic/CpuMeter.h> +#include <events/EventBroker.h> +#include <events/Events.h> +#include <utils/aero/AeroUtils.h> + +namespace DeathStackBoard +{ + +using namespace ADAConfigs; + +ADAAlgorithm::ADAAlgorithm(ADAReferenceValues ref_values) + : ref_values(ref_values), filter(getKalmanConfig(ref_values.ref_pressure)) +{ + TRACE( + "[ADA] Initial reference values : alt_ref: %.3f, p_ref: %.3f, t_ref: " + "%.3f, p0: %.3f, t0: %.3f\n", + ref_values.ref_altitude, ref_values.ref_pressure, + ref_values.ref_temperature, ref_values.msl_pressure, + ref_values.msl_temperature); +} + +ADAAlgorithm::~ADAAlgorithm() {} + +void ADAAlgorithm::updateBaro(float pressure) +{ + updatePressureKalman(pressure); + + // Convert filter data to altitudes & speeds + ada_data.timestamp = TimestampTimer::getTimestamp(); + ada_data.msl_altitude = pressureToAltitude(filter.getState()(0, 0)); + ada_data.agl_altitude = altitudeMSLtoAGL(ada_data.msl_altitude); + ada_data.vert_speed = aeroutils::verticalSpeed( + filter.getState()(0, 0), filter.getState()(1, 0), + ref_values.msl_pressure, ref_values.msl_temperature); + +#ifdef DEBUG + if (counter == 50) + { + TRACE("[ADA] z : %.2f - vz : %.2f \n", ada_data.msl_altitude, + ada_data.vert_speed); + counter = 0; + } + else + { + counter++; + } +#endif +} + +void ADAAlgorithm::updateGPS(float lat, float lon, bool fix) +{ + last_lat = lat; + last_lon = lon; + last_fix = fix; +} + +float ADAAlgorithm::getAltitudeMsl() const { return ada_data.msl_altitude; } + +float ADAAlgorithm::getAltitudeForDeployment() const +{ + return ada_data.agl_altitude; +} + +float ADAAlgorithm::getVerticalSpeed() const { return ada_data.vert_speed; } + +float ADAAlgorithm::pressureToAltitude(float pressure) +{ + return aeroutils::relAltitude(pressure, ref_values.msl_pressure, + ref_values.msl_temperature); +} + +float ADAAlgorithm::altitudeMSLtoAGL(float altitude_msl) const +{ + return altitude_msl - ref_values.ref_altitude; +} + +ADAKalmanState ADAAlgorithm::getKalmanState() +{ + ADAKalmanState state; + state.timestamp = TimestampTimer::getTimestamp(); + + state.x0 = filter.getState()(0, 0); + state.x1 = filter.getState()(1, 0); + state.x2 = filter.getState()(2, 0); + + return state; +} + +void ADAAlgorithm::updatePressureKalman(float pressure) +{ + filter.predict(); + CVectorP y(pressure); // column vector + if (!filter.correct(y)) + { + TRACE("[ADA] Kalman correction step failed \n"); + } +} + +const KalmanEigen<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>::KalmanConfig +ADAAlgorithm::getKalmanConfig(const float init_pressure) +{ + KalmanEigen<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>::KalmanConfig + config; + config.F = F_INIT; + config.H = H_INIT; + config.Q = Q_INIT; + config.R = R_INIT; + config.P = P_INIT; + config.x = CVectorN(init_pressure, 0, KALMAN_INITIAL_ACCELERATION); + + return config; +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..329129db0c2e166937febb9910978de9fadec03d --- /dev/null +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h @@ -0,0 +1,118 @@ +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <ApogeeDetectionAlgorithm/ADAData.h> +#include <kalman/KalmanEigen.h> +#include <math/Stats.h> +#include <sensors/Sensor.h> + +namespace DeathStackBoard +{ + +class ADAAlgorithm +{ +public: + struct AltitudeAGL + { + float altitude; + }; + + ADAAlgorithm(ADAReferenceValues ref_values); + + ~ADAAlgorithm(); + + void updateBaro(float pressure); + + void updateGPS(float lat, float lon, bool fix); + + ADAKalmanState getKalmanState(); + + ADAData getADAData() const { return ada_data; } + + /** + * @brief Current altitude above mean sea level. + * + * @return Altitude in meters (MSL). + */ + float getAltitudeMsl() const; + + /** + * @brief Returns altitude for main chute deployment. + * + * Checks if last gps sample has fix, returns altitude above ground level, + * if not returns QFE altitude relative to the elevation of the launch site. + * + * @return Altitude in meters. + */ + float getAltitudeForDeployment() const; + + /** + * @brief Current vertical speed in m/s, positive upwards. + */ + float getVerticalSpeed() const; + + /** + * @brief Converts an atmospheric pressure to altitude based on the provided + * reference values. + * + * @param pressure Atmospheric pressure in Pa. + * @return Corresponding altitude above mean sea level (m). + */ + float pressureToAltitude(float pressure); + + /** + * @brief Converts an altitude above mean sea level to altitude for chute + * deployment (see getAltitudeForDeployment()). + */ + float altitudeMSLtoAGL(float altitude_msl) const; + + ADAReferenceValues getReferenceValues() const { return ref_values; } + +private: + void updatePressureKalman(float pressure); + + /** + * @brief Method to initialize the kalman configuration structure + */ + const KalmanEigen<float, KALMAN_STATES_NUM, + KALMAN_OUTPUTS_NUM>::KalmanConfig + getKalmanConfig(const float ref_pressure); + + // References for pressure to altitude conversion + ADAReferenceValues ref_values; + + KalmanEigen<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM> filter; + + ADAData ada_data; + + float last_lat = 0; + float last_lon = 0; + bool last_fix = false; + +#ifdef DEBUG + unsigned int counter = 0; +#endif +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/ADACalibrator.cpp b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp similarity index 84% rename from src/boards/DeathStack/ADA/ADACalibrator.cpp rename to src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp index 41e0d460bca3942529df38d2e214e59abd883429..4ea7e103136a0b5f4fcf71317d31af146f9cff29 100644 --- a/src/boards/DeathStack/ADA/ADACalibrator.cpp +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp @@ -1,4 +1,4 @@ -/* Copyright (c) 2018,2019 Skyward Experimental Rocketry +/* Copyright (c) 2018-2019 Skyward Experimental Rocketry * Authors: Luca Mozzarelli, Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy @@ -13,18 +13,21 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include "ADACalibrator.h" +#include <ApogeeDetectionAlgorithm/ADACalibrator.h> #include <utils/aero/AeroUtils.h> + namespace DeathStackBoard { +using namespace ADAConfigs; + void ADACalibrator::addBaroSample(float p) { pressure_stats.add(p); } bool ADACalibrator::calibIsComplete() @@ -35,7 +38,7 @@ bool ADACalibrator::calibIsComplete() ref_alt_set && ref_temp_set; } -void ADACalibrator::resetBaro() { pressure_stats.reset(); } +void ADACalibrator::reset() { pressure_stats.reset(); } void ADACalibrator::setReferenceTemperature(float ref_temp) { @@ -46,7 +49,7 @@ void ADACalibrator::setReferenceTemperature(float ref_temp) { ref_values.ref_temperature = temperature_ref; ref_temp_set = true; - TRACE("[ADA] Reference temperature set to %.3f K\n", temperature_ref); + LOG_INFO(log, "Reference temperature set to {:.3f} K", temperature_ref); } } @@ -54,10 +57,10 @@ void ADACalibrator::setReferenceAltitude(float ref_alt) { ref_values.ref_altitude = ref_alt; ref_alt_set = true; - TRACE("[ADA] Reference altitude set to %.3f m\n", ref_alt); + LOG_INFO(log, "Reference altitude set to {:.3f} m", ref_alt); } -ReferenceValues ADACalibrator::getReferenceValues() +ADAReferenceValues ADACalibrator::getReferenceValues() { // If calibration is not compete use default values if (calibIsComplete()) @@ -79,4 +82,5 @@ ReferenceValues ADACalibrator::getReferenceValues() return ref_values; } + } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/ADACalibrator.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h similarity index 64% rename from src/boards/DeathStack/ADA/ADACalibrator.h rename to src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h index e6b04a2ca0a3c0c7617e1aceebc03cef57fb55c1..2f03b0a4cda07b47c91ef61d2b7047eadbcb4e16 100644 --- a/src/boards/DeathStack/ADA/ADACalibrator.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2018,2019 Skyward Experimental Rocketry +/* Copyright (c) 2018-2019 Skyward Experimental Rocketry * Authors: Luca Mozzarelli, Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,44 +22,62 @@ #pragma once +#include <ApogeeDetectionAlgorithm/ADAData.h> #include <Common.h> #include <Debug.h> +#include <diagnostic/PrintLogger.h> #include <math/Stats.h> #include <miosix.h> -#include "ADAStatus.h" namespace DeathStackBoard { + class ADACalibrator { -private: - /* --- CALIBRATION --- */ - ReferenceValues ref_values{}; - - Stats pressure_stats; // Computes mean std dev etc for calibration of - // pressure conversion - // Refernece flags - bool ref_alt_set = false; - bool ref_temp_set = false; public: - /* --- CALIBRATION --- */ - ReferenceValues getReferenceValues(); + ADAReferenceValues getReferenceValues(); bool calibIsComplete(); - void addBaroSample(float p); // Adds a pressure sample to the stats - void resetBaro(); // Resets only pressure stats - /* --- TC ---*/ /** - * Sets the reference temperature to be used to calibrate the altimeter - * @param ref_temp Reference temperature in degrees Celsisus + * @brief Adds a pressure sample to the stats. + */ + void addBaroSample(float p); + + /** + * @brief Resets pressure stats. + */ + void reset(); + + /** + * @brief Sets the reference temperature to be used to calibrate the + * altimeter. + * + * @param ref_temp Reference temperature in degrees Celsisus. */ void setReferenceTemperature(float ref_temp); /** - * Sets the reference altitude to be used to calibrate the altimeter - * @param ref_alt Reference altitude in meters above mean sea level + * @brief Sets the reference altitude (msl) to be used to calibrate the + * altimeter. + * + * @param ref_alt Reference altitude in meters above mean sea level. */ void setReferenceAltitude(float ref_alt); + +private: + ADAReferenceValues ref_values{}; + + /** + * @brief Computes mean std dev etc for calibration of pressure conversion. + */ + Stats pressure_stats; + + // Refernece flags + bool ref_alt_set = false; + bool ref_temp_set = false; + + PrintLogger log = Logging::getLogger("deathstack.fsm.ada"); }; + } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h new file mode 100644 index 0000000000000000000000000000000000000000..7df4eee9174f8acc2585ae4fa84bfd85adfd7eec --- /dev/null +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h @@ -0,0 +1,772 @@ +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> +#include <ApogeeDetectionAlgorithm/ADACalibrator.h> +#include <ApogeeDetectionAlgorithm/ADAData.h> +#include <LoggerService/LoggerService.h> +#include <System/StackLogger.h> +#include <configs/ADAConfig.h> +#include <diagnostic/PrintLogger.h> +#include <events/EventBroker.h> +#include <events/Events.h> +#include <events/FSM.h> +#include <miosix.h> +#include <sensors/SensorData.h> + +using miosix::FastMutex; +using miosix::Lock; + +namespace DeathStackBoard +{ + +using namespace ADAConfigs; + +template <typename Press, typename GPS> +class ADAController : public FSM<ADAController<Press, GPS>> +{ + using ADACtrl = ADAController<Press, GPS>; + using ADAFsm = FSM<ADAController<Press, GPS>>; + + static_assert( + checkIfProduces<Sensor<Press>, PressureData>::value, + "Template argument must be a sensor that produces pressure data."); + static_assert(checkIfProduces<Sensor<GPS>, GPSData>::value, + "Template argument must be a sensor that produces GPS data."); + +public: + ADAController(Sensor<Press>& barometer, Sensor<GPS>& gps); + + ~ADAController(); + + /* + * It's critical that this methods are called at regualar intervals during + * the flight. Call frequency is defined in configs/ADAConfig.h The behavior + * of this functions changes depending on the ADA state + */ + void update(); + + /** + * Sets the reference temperature to be used to calibrate the altimeter + * @param ref_temp Reference temperature in degrees Celsisus + */ + void setReferenceTemperature(float ref_temp); + + /** + * Sets the reference altitude to be used to calibrate the altimeter + * @param ref_alt Reference altitude in meters above mean sea level + */ + void setReferenceAltitude(float ref_alt); + + /** + * Sets the deployment altitude + * @param dpl_alt Deployment altitude in meters above GROUND level + */ + void setDeploymentAltitude(float dpl_alt); + + /** + * ADAController status + * @returns A struct containing the timestamp, the ADA FSM state and + * several flags + */ + ADAControllerStatus getStatus() { return status; } + + /** + * @returns The current ADAData structure + */ + ADAData getADAData() { return ada.getADAData(); } + + /** + * @returns The current ADAReferenceValues structure + */ + ADAReferenceValues getReferenceValues() { return ada.getReferenceValues(); } + +private: + void state_init(const Event& ev); + + /** + * @brief Idle state: the ADA waits for a command to start calibration. This + * is the initial state. + */ + void state_idle(const Event& ev); + + /** + * @brief Calibrating state: the ADA calibrates the initial Kalman state. + * + * In this state a call to update() will result in a altitude sample being + * added to the average. The exiting transition to the idle state is + * triggered at the first sample update after having set the deployment + * altitude and having reached the minimum number of calibration samples. + */ + void state_calibrating(const Event& ev); + + /** + * @brief Ready state: ADA is ready and waiting for liftoff + * + * In this state a call to update() will have no effect. + * The exiting transition to the shadow mode state is triggered by the + * liftoff event. + */ + void state_ready(const Event& ev); + + /** + * @brief Shadow mode state: ADA is running and logging apogees detected, + * but is not generating events + * + * In this state a call to update() will trigger a one step update of the + * kalman filter followed by a check of vertical speed sign. The exiting + * transition to the active state is triggered by a timeout event. + */ + void state_shadowMode(const Event& ev); + + /** + * @brief Active state: ADA is running and it generates an event whe apogee + * is detected + * + * In this state a call to update() will trigger a one step update of the + * kalman filter followed by a check of vertical speed sign. The exiting + * transition to the descent state is triggered by the apogee reached event + * (NOT self generated!) + */ + void state_active(const Event& ev); + void state_pressureStabilization(const Event& ev); + + /** + * @brief First descent phase state: ADA is running and it generates an + * event when a set altitude is reached + * + * In this state a call to update() will trigger a one step update of the + * kalman filter followed by a check of the altitude. The exiting transition + * to the stop state is triggered by the parachute deployment altitude + * reached event (NOT self generated!) + */ + void state_drogueDescent(const Event& ev); + + /** + * @brief End state: ADA is stopped + * + * In this state a call to update() will have no effect. + * This is the final state + */ + void state_end(const Event& ev); + + void updateBaroAccordingToState(float pressure); + + void finalizeCalibration(); + + void logStatus(ADAState state); // Update and log ADA FSM state + void logStatus(); // Log the ADA FSM state without updating it + + void logData(const ADAKalmanState& s, const ADAData& d); + + uint16_t shadow_delayed_event_id = + 0; // Event id to store shadow mode timeout + + uint16_t pressure_delayed_event_id = + 0; // Event id to store pressure stabilization timeout + + ADAControllerStatus status; // ADA status: timestamp + state + + FastMutex calibrator_mutex; + ADACalibrator calibrator; + + ADAAlgorithm ada; + + Sensor<Press>& barometer; + Sensor<GPS>& gps; + + uint64_t last_press_timestamp = 0; + uint64_t last_gps_timestamp = 0; + + float deployment_altitude = DEFAULT_DEPLOYMENT_ALTITUDE; + bool deployment_altitude_set = false; + + unsigned int n_samples_apogee_detected = + 0; // Number of consecutive samples in which apogee is detected + unsigned int n_samples_deployment_detected = + 0; // Number of consecutive samples in which dpl altitude is + // detected + unsigned int n_samples_abk_disable_detected = + 0; // Number of consecutive samples for abk disable + + LoggerService& logger = *(LoggerService::getInstance()); // Logger + PrintLogger log = Logging::getLogger("deathstack.fms.ada"); +}; + +template <typename Press, typename GPS> +ADAController<Press, GPS>::ADAController(Sensor<Press>& barometer, + Sensor<GPS>& gps) + : ADAFsm(&ADACtrl::state_idle, STACK_MIN_FOR_SKYWARD, ADA_PRIORITY), + ada(ADAReferenceValues{}), barometer(barometer), gps(gps) +{ + // Subscribe to topics + sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker->subscribe(this, TOPIC_ADA); + + status.state = ADAState::IDLE; +} + +template <typename Press, typename GPS> +ADAController<Press, GPS>::~ADAController() +{ +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::update() +{ + // if new gps data available, update GPS, regardless of the current state + GPS gps_data = gps.getLastSample(); + if (gps_data.gps_timestamp != last_gps_timestamp) + { + last_gps_timestamp = gps_data.gps_timestamp; + + ada.updateGPS(gps_data.latitude, gps_data.longitude, gps_data.fix); + } + + // if new pressure data available, update baro, according to current state + Press press_data = barometer.getLastSample(); + + if (press_data.press_timestamp != last_press_timestamp) + { + last_press_timestamp = press_data.press_timestamp; + + updateBaroAccordingToState(press_data.press); + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) +{ + ADAState state = status.state; + + switch (state) + { + case ADAState::IDLE: + { + break; + } + case ADAState::CALIBRATING: + { + bool end_calib = false; + { + Lock<FastMutex> l(calibrator_mutex); + + // Add samples to the calibration + calibrator.addBaroSample(pressure); + + // Save the state of calibration to release mutex + end_calib = calibrator.calibIsComplete(); + } + + if (end_calib) + { + // If samples are enough and dpl altitude has been set init ada + finalizeCalibration(); + } + break; + } + + case ADAState::READY: + { + // Log the altitude & vertical speed but don't use kalman pressure + // while we are on the ramp + ADAData d; + d.timestamp = TimestampTimer::getTimestamp(); + d.msl_altitude = ada.pressureToAltitude(pressure); + d.agl_altitude = ada.altitudeMSLtoAGL(d.msl_altitude); + d.vert_speed = 0; + + logger.log(d); + break; + } + + case ADAState::SHADOW_MODE: + { + // Shadow mode state: update kalman, DO NOT send events + ada.updateBaro(pressure); + + // Check if the vertical speed smaller than the target apogee speed + if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) + { + // Log + ApogeeDetected apogee{TimestampTimer::getTimestamp(), + status.state}; + logger.log(apogee); + } + + logData(ada.getKalmanState(), ada.getADAData()); + + break; + } + + case ADAState::ACTIVE: + { + ada.updateBaro(pressure); + + // Check if we reached apogee + if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) + { + if (++n_samples_apogee_detected >= APOGEE_N_SAMPLES) + { + // Active state send notifications for apogee + sEventBroker->post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA); + status.apogee_reached = true; + } + + // Log + ApogeeDetected apogee{TimestampTimer::getTimestamp(), + status.state}; + logger.log(apogee); + } + else if (n_samples_apogee_detected != 0) + { + n_samples_apogee_detected = 0; + } + + // Check if we have to disable airbrakes + if (ada.getVerticalSpeed() < ABK_DISABLE_VERTICAL_SPEED_TARGET) + { + if (++n_samples_abk_disable_detected >= ABK_DISABLE_N_SAMPLES) + { + // Active state send notifications for disabling airbrakes + sEventBroker->post({EV_ADA_DISABLE_ABK}, + TOPIC_FLIGHT_EVENTS); + status.disable_airbrakes = true; + } + } + else if (n_samples_abk_disable_detected != 0) + { + n_samples_abk_disable_detected = 0; + } + + logData(ada.getKalmanState(), ada.getADAData()); + break; + } + + case ADAState::PRESSURE_STABILIZATION: + { + // Stabilization state: do not send notifications for target + // altitude reached, log it + ada.updateBaro(pressure); + + if (ada.getAltitudeForDeployment() <= deployment_altitude) + { + if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) + { + logger.log( + DplAltitudeReached{TimestampTimer::getTimestamp()}); + } + } + else if (n_samples_deployment_detected != 0) + { + n_samples_deployment_detected = 0; + } + + logData(ada.getKalmanState(), ada.getADAData()); + break; + } + + case ADAState::DROGUE_DESCENT: + { + // Descent state: send notifications for target altitude reached + ada.updateBaro(pressure); + + if (ada.getAltitudeForDeployment() <= deployment_altitude) + { + if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) + { + logger.log( + DplAltitudeReached{TimestampTimer::getTimestamp()}); + + sEventBroker->post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA); + } + } + else if (n_samples_deployment_detected != 0) + { + n_samples_deployment_detected = 0; + } + + logData(ada.getKalmanState(), ada.getADAData()); + break; + } + case ADAState::END: + { + // Continue updating the filter for logging & telemetry purposes + ada.updateBaro(pressure); + + logData(ada.getKalmanState(), ada.getADAData()); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::setReferenceTemperature(float ref_temp) +{ + if (status.state == ADAState::CALIBRATING || + status.state == ADAState::READY) + { + { + Lock<FastMutex> l(calibrator_mutex); + calibrator.setReferenceTemperature(ref_temp); + logger.log(calibrator.getReferenceValues()); + } + + finalizeCalibration(); + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::setReferenceAltitude(float ref_alt) +{ + if (status.state == ADAState::CALIBRATING || + status.state == ADAState::READY) + { + { + Lock<FastMutex> l(calibrator_mutex); + calibrator.setReferenceAltitude(ref_alt); + logger.log(calibrator.getReferenceValues()); + } + + finalizeCalibration(); + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::setDeploymentAltitude(float dpl_alt) +{ + if (status.state == ADAState::CALIBRATING || + status.state == ADAState::READY) + { + { + Lock<FastMutex> l(calibrator_mutex); + + deployment_altitude = dpl_alt; + deployment_altitude_set = true; + } + logger.log(TargetDeploymentAltitude{deployment_altitude}); + + LOG_INFO(log, "Deployment altitude set to {:.3f} m", + deployment_altitude); + + finalizeCalibration(); + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::finalizeCalibration() +{ + Lock<FastMutex> l(calibrator_mutex); + + if (calibrator.calibIsComplete() && deployment_altitude_set && + ada.getReferenceValues() != calibrator.getReferenceValues()) + { + // If samples are enough and dpl altitude has been set init ada + ada = ADAAlgorithm{calibrator.getReferenceValues()}; + + LOG_INFO(log, "Finalized calibration"); + + // ADA READY! + sEventBroker->post({EV_ADA_READY}, TOPIC_ADA); + + logger.log(calibrator.getReferenceValues()); + logger.log(ada.getKalmanState()); + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_idle(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state idle"); + logStatus(ADAState::IDLE); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state idle"); + break; + } + case EV_CALIBRATE_ADA: + { + this->transition(&ADACtrl::state_calibrating); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_calibrating(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + { + Lock<FastMutex> l(calibrator_mutex); + calibrator.reset(); + } + + logStatus(ADAState::CALIBRATING); + LOG_DEBUG(log, "Entering state calibrating"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state calibrating"); + break; + } + case EV_ADA_READY: + { + this->transition(&ADACtrl::state_ready); + break; + } + case EV_CALIBRATE_ADA: + { + this->transition(&ADACtrl::state_calibrating); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_ready(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + logStatus(ADAState::READY); + LOG_DEBUG(log, "Entering state ready"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state ready"); + break; + } + case EV_LIFTOFF: + { + this->transition(&ADACtrl::state_shadowMode); + break; + } + case EV_CALIBRATE_ADA: + { + this->transition(&ADACtrl::state_calibrating); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_shadowMode(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + shadow_delayed_event_id = + sEventBroker->postDelayed<TIMEOUT_ADA_SHADOW_MODE>( + {EV_SHADOW_MODE_TIMEOUT}, TOPIC_ADA); + logStatus(ADAState::SHADOW_MODE); + LOG_DEBUG(log, "Entering state shadowMode"); + break; + } + case EV_EXIT: + { + sEventBroker->removeDelayed(shadow_delayed_event_id); + LOG_DEBUG(log, "Exiting state shadowMode"); + break; + } + case EV_SHADOW_MODE_TIMEOUT: + { + this->transition(&ADACtrl::state_active); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_active(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + logStatus(ADAState::ACTIVE); + LOG_DEBUG(log, "Entering state active"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state active"); + break; + } + case EV_ADA_APOGEE_DETECTED: + { + this->transition(&ADACtrl::state_pressureStabilization); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + pressure_delayed_event_id = + sEventBroker->postDelayed<TIMEOUT_ADA_P_STABILIZATION>( + {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA); + logStatus(ADAState::PRESSURE_STABILIZATION); + LOG_DEBUG(log, "Entering state pressureStabilization"); + break; + } + case EV_EXIT: + { + sEventBroker->removeDelayed(pressure_delayed_event_id); + LOG_DEBUG(log, "Exiting state pressureStabilization"); + break; + } + case EV_TIMEOUT_PRESS_STABILIZATION: + { + this->transition(&ADACtrl::state_drogueDescent); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_drogueDescent(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + logStatus(ADAState::DROGUE_DESCENT); + LOG_DEBUG(log, "Entering state drogueDescent"); + n_samples_deployment_detected = 0; + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state drogueDescent"); + break; + } + case EV_ADA_DPL_ALT_DETECTED: + { + status.dpl_altitude_reached = true; + logStatus(); + + this->transition(&ADACtrl::state_end); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::state_end(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state end"); + logStatus(ADAState::END); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state end"); + break; + } + default: + { + break; + } + } +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::logStatus(ADAState state) +{ + status.state = state; + logStatus(); +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::logStatus() +{ + status.timestamp = TimestampTimer::getTimestamp(); + logger.log(status); + + StackLogger::getInstance()->updateStack(THID_ADA_FSM); +} + +template <typename Press, typename GPS> +void ADAController<Press, GPS>::logData(const ADAKalmanState& s, + const ADAData& d) +{ + logger.log(s); + logger.log(d); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/ADAStatus.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h similarity index 62% rename from src/boards/DeathStack/ADA/ADAStatus.h rename to src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h index fd483bfa56e6422dac4b796a4ffeb97a25c93fb5..dd1dc693708191b3dee252d6a6ffc23ceed2661a 100644 --- a/src/boards/DeathStack/ADA/ADAStatus.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h @@ -1,5 +1,5 @@ /* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli + * Author: Luca Mozzarelli * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,125 +22,129 @@ #pragma once -#include <boards/DeathStack/configs/ADA_config.h> +#include <configs/ADAConfig.h> #include <math/Stats.h> +#include <Constants.h> #include <ostream> namespace DeathStackBoard { -// All possible states of the ADA FMM -enum class ADAState +using namespace ADAConfigs; + +/** + * @brief All possible states of the ADA. + */ +enum class ADAState : uint8_t { - UNDEFINED, - IDLE, + IDLE = 0, CALIBRATING, READY, SHADOW_MODE, ACTIVE, - FIRST_DESCENT_PHASE, - END, - PRESSURE_STABILIZATION + PRESSURE_STABILIZATION, + DROGUE_DESCENT, + END }; -// Struct to log apogee detection -struct ApogeeDetected +/** + * @brief Struct to log altitude and vertical speed of first Kalman (state after + * conversion). + */ +struct ADAData { - ADAState state; - long long tick; + uint64_t timestamp; + float msl_altitude; + float agl_altitude; + float vert_speed; - static std::string header() { return "timestamp,state\n"; } + static std::string header() + { + return "timestamp,msl_altitude,agl_altitude,vert_speed\n"; + } void print(std::ostream& os) const { - os << tick << "," << (int)state << "\n"; + os << timestamp << "," << msl_altitude << "," << agl_altitude << "," + << vert_speed << "\n"; } }; -// Struct to log deployment pressure detection -struct DplAltitudeReached -{ - long long tick; - static std::string header() { return "tick\n"; } - - void print(std::ostream& os) const { os << tick << "\n"; } -}; - -// Struct to log current state -// Also used to keep track of current state +/** + * @brief Struct to log current state, also used to keep track of current state. + */ struct ADAControllerStatus { - long long timestamp; - ADAState state = ADAState::UNDEFINED; + uint64_t timestamp; + ADAState state = ADAState::IDLE; bool apogee_reached = false; + bool disable_airbrakes = false; bool dpl_altitude_reached = false; static std::string header() { - return "timestamp,state,apogee_reached,dpl_altitude_" + return "timestamp,state,apogee_reached,disable_airbrakes,dpl_altitude_" "reached\n"; } void print(std::ostream& os) const { os << timestamp << "," << (int)state << "," << apogee_reached << "," - << dpl_altitude_reached << "\n"; + << disable_airbrakes << "," << dpl_altitude_reached << "\n"; } }; -// Struct to log the two Kalman states -struct KalmanState +/** + * @brief Struct to log apogee detection. + */ +struct ApogeeDetected { - long long timestamp; - float x0; - float x1; - float x2; - float x0_acc; - float x1_acc; - float x2_acc; + uint64_t timestamp; + ADAState state; - static std::string header() - { - return "timestamp,x0,x1,x2,x0_acc,x1_acc,x2_acc\n"; - } + static std::string header() { return "timestamp,state\n"; } void print(std::ostream& os) const { - os << timestamp << "," << x0 << "," << x1 << "," << x2 << "," << x0_acc - << "," << x1_acc << "," << x2_acc << "\n"; + os << timestamp << "," << (int)state << "\n"; } }; -// Struct to log altitude and vertical speed of first Kalman -// (state after conversion) -struct ADAData +/** + * @brief Struct to log deployment pressure detection. + */ +struct DplAltitudeReached { - long long timestamp; - float msl_altitude; - float dpl_altitude; - bool is_dpl_altitude_agl; - float vert_speed; + uint64_t timestamp; - float acc_msl_altitude; - float acc_vert_speed; + static std::string header() { return "timestamp\n"; } - static std::string header() - { - return "timestamp,msl_altitude,dpl_altitude,is_agl,vert_speed,acc_msl_" - "altitude,acc_vert_speed\n"; - } + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +/** + * @brief Struct to log the two Kalman states. + */ +struct ADAKalmanState +{ + uint64_t timestamp; + float x0; + float x1; + float x2; + + static std::string header() { return "timestamp,x0,x1,x2\n"; } void print(std::ostream& os) const { - os << timestamp << "," << msl_altitude << "," << dpl_altitude << "," - << (int)is_dpl_altitude_agl << "," << vert_speed << "," - << acc_msl_altitude << "," << acc_vert_speed << "\n"; + os << timestamp << "," << x0 << "," << x1 << "," << x2 << "\n"; } }; -// Struct to log altimeter reference values -// Also used in ADA to store the values -struct ReferenceValues +/** + * @brief Struct to log altimeter reference values, also used in ADA to store + * the values. + */ +struct ADAReferenceValues { // Launch site altitude float ref_altitude = DEFAULT_REFERENCE_ALTITUDE; @@ -151,8 +155,8 @@ struct ReferenceValues // Pressure at mean sea level for altitude calculation, to be updated with // launch-day calibration - float msl_pressure = DEFAULT_MSL_PRESSURE; - float msl_temperature = DEFAULT_MSL_TEMPERATURE; + float msl_pressure = MSL_PRESSURE; + float msl_temperature = MSL_TEMPERATURE; static std::string header() { @@ -166,7 +170,7 @@ struct ReferenceValues << "," << msl_pressure << "," << msl_temperature << "\n"; } - bool operator==(const ReferenceValues& other) const + bool operator==(const ADAReferenceValues& other) const { return ref_altitude == other.ref_altitude && ref_pressure == other.ref_pressure && @@ -175,13 +179,15 @@ struct ReferenceValues msl_temperature == other.msl_temperature; } - bool operator!=(const ReferenceValues& other) const + bool operator!=(const ADAReferenceValues& other) const { return !(*this == other); } }; -// Struct to log dpl altitude +/** + * @brief Struct to log dpl altitude. + */ struct TargetDeploymentAltitude { float deployment_altitude; diff --git a/src/boards/DeathStack/Canbus/CanProxy.cpp b/src/boards/DeathStack/Canbus/CanProxy.cpp deleted file mode 100644 index cbe921e10cc502da63665a2d755a1206705e8543..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/Canbus/CanProxy.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise De Faveri - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <Common.h> -#include "CanProxy.h" - -#include "CanInterfaces.h" -#include "DeathStack/events/Events.h" -#include "DeathStack/events/Topics.h" - -#include <events/EventBroker.h> - -namespace DeathStackBoard -{ - -using namespace std::placeholders; - -/** - * Canbus receiving function. - * @param message the received message to be handled - * @param proxy the object that has the reference to the bus - */ -static void canRcv(const CanMsg& message, const CanStatus& status) -{ - TRACE("[CAN] Received message with id %lu\n", message.StdId); - - /* Create event */ - CanbusEvent ev; - ev.sig = EV_NEW_CAN_MSG; - ev.canTopic = message.StdId; - ev.len = message.DLC; - memcpy(ev.payload, message.Data, CAN_MAX_PAYLOAD); - - /* Log stats */ - LoggerService::getInstance()->log(status); - - /* Post event */ - sEventBroker->post(ev, TOPIC_CAN); -} - -/** - * Initialise CAN1 on PA11, PA12, set filters and set receiver function. - */ -CanProxy::CanProxy(CanManager* c) -{ - // Init structure (pins) - canbus_init_t st = { - CAN1, miosix::Mode::ALTERNATE, 9, {CAN1_RX0_IRQn, CAN1_RX1_IRQn}}; - - // Bus - c->addBus<GPIOA_BASE, 11, 12>(st, &canRcv); - bus = c->getBus(0); - - // Filters - c->addHWFilter(CanInterfaces::CAN_TOPIC_IGNITION, 0); - c->addHWFilter(CanInterfaces::CAN_TOPIC_NOSECONE, 0); - - TRACE("[CAN] Initialised CAN1 on PA11-12 \n"); -} - -/** - * Canbus receiving function. - */ -bool CanProxy::send(uint16_t id, const uint8_t* message, uint8_t len) -{ - // Send - bool ok = bus->send(id, message, len); - - // Log stats - CanStatus status = bus->getStatus(); - LoggerService::getInstance()->log(status); - - return ok; -} - -} /* namespace DeathStackBoard */ diff --git a/src/boards/DeathStack/Canbus/CanProxy.h b/src/boards/DeathStack/Canbus/CanProxy.h deleted file mode 100644 index 2762fe5e865d3996cc901b26fe46de2807e3028c..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/Canbus/CanProxy.h +++ /dev/null @@ -1,64 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise De Faveri - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <Common.h> -#include <drivers/canbus/CanManager.h> -#include <drivers/canbus/CanUtils.h> -#include <DeathStack/LoggerService/LoggerService.h> - -namespace DeathStackBoard -{ - -/** - * This class is interposed between the OBSW and the Canbus driver. - * Canbus initialization and status logging is done here. - */ -class CanProxy -{ -public: - CanProxy(CanManager* c); - ~CanProxy() {}; - - /* - * Sending proxy function: sends and logs the status. - * - * @param id Id of the message (aka topic) - * @para message message as byte array - * @param len length of the message (max 8 bytes, truncated if grater) - * @return true if the message was sent correctly - */ - bool send(uint16_t id, const uint8_t* message, uint8_t len); - - - /* - * Getters - */ - CanBus* getBus() { return bus; } - CanStatus getStatus() { return bus->getStatus(); } - -private: - CanBus* bus; -}; - -} /* namespace DeathStackBoard */ diff --git a/src/boards/DeathStack/DeathStack.h b/src/boards/DeathStack/DeathStack.h index d7c0dae025ecc3244cb40b2f2d62af0347283fab..540951a363cf1070d47db72a0afa84129de3dff9 100644 --- a/src/boards/DeathStack/DeathStack.h +++ b/src/boards/DeathStack/DeathStack.h @@ -1,5 +1,5 @@ /* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Alvise de'Faveri Tron + * Author: Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,38 +22,42 @@ #pragma once -#include <functional> -#include <stdexcept> -#include <vector> - #include <Common.h> - +#include <DeathStackStatus.h> +#include <Debug.h> +#include <LoggerService/LoggerService.h> +#include <Main/Actuators.h> +#include <Main/Bus.h> +#include <Main/Radio.h> +#include <Main/Sensors.h> +#include <Main/StateMachines.h> +#include <PinHandler/PinHandler.h> +#include <System/StackLogger.h> +#include <System/TaskID.h> #include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/EventInjector.h> +#include <events/Events.h> +#include <events/Topics.h> #include <events/utils/EventSniffer.h> -#include "DeathStack/LoggerService/TmRepository.h" -#include "DeathStack/events/Events.h" -#include "DeathStack/events/Topics.h" -#include "DeathStackStatus.h" - -#include "DeathStack/LoggerService/LoggerService.h" +#include <functional> +#include <stdexcept> +#include <vector> -#include "DeathStack/ADA/ADAController.h" -#include "DeathStack/DeploymentController/DeploymentController.h" -#include "DeathStack/FlightModeManager/FlightModeManager.h" -#include "DeathStack/PinHandler/PinHandler.h" -#include "DeathStack/SensorManager/SensorManager.h" -#include "DeathStack/TMTCManager/TMTCManager.h" -#include "DeathStack/events/EventData.h" +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL.h> +#endif using std::bind; namespace DeathStackBoard { -// Add heres the event that you don't want to be TRACEd in DeathStack.logEvent() +// Add heres the events that you don't want to be TRACEd in +// DeathStack.logEvent() static const std::vector<uint8_t> TRACE_EVENT_BLACKLIST{ - EV_SEND_HR_TM, EV_SEND_LR_TM, EV_SEND_TEST_TM}; + EV_SEND_HR_TM, EV_SEND_LR_TM, EV_SEND_TEST_TM, EV_SEND_SENS_TM}; /** * This file provides a simplified way to initialize and monitor all * the components of the DeathStack. @@ -66,122 +70,153 @@ public: // Shared Components EventBroker* broker; LoggerService* logger; - PinHandler* pin_observer; + EventSniffer* sniffer; + StateMachines* state_machines; - // FSMs - ADAController* ada; - SensorManager* sensor_manager; - TMTCManager* tmtc; - FlightModeManager* fmm; + Bus* bus; + Sensors* sensors; + Radio* radio; + Actuators* actuators; - Cutter* cutter; - Servo* servo; - DeploymentController* dpl; + PinHandler* pin_handler; -private: - /** - * Initialize Everything - */ - DeathStack() - { - memset(&status, 0, sizeof(status)); + TaskScheduler* scheduler; - /* Shared components */ - logger = Singleton<LoggerService>::getInstance(); - - broker = sEventBroker; - pin_observer = new PinHandler(); +#ifdef HARDWARE_IN_THE_LOOP + HIL* hil; +#endif - // Bind the logEvent function to the event sniffer in order to log every - // event + void start() + { + if (!broker->start()) { - using namespace std::placeholders; - sniffer = new EventSniffer( - *broker, TOPIC_LIST, bind(&DeathStack::logEvent, this, _1, _2)); + LOG_ERR(log, "Error starting EventBroker"); + status.setError(&DeathStackStatus::ev_broker); } - ada = new ADAController(); - sensor_manager = new SensorManager(ada); - tmtc = new TMTCManager(); - fmm = new FlightModeManager(); - cutter = new Cutter(CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE, - CUTTER_TEST_PWM_DUTY_CYCLE); - servo = new Servo(DeploymentConfigs::SERVO_TIMER); - dpl = new DeploymentController(*cutter, *servo); - - // Start threads - try + if (!radio->start()) { - logger->start(); + LOG_ERR(log, "Error starting radio module"); + status.setError(&DeathStackStatus::radio); } - catch (const std::runtime_error& re) + + if (!sensors->start()) { - TRACE("Logger init error\n"); - status.setError(&DeathStackStatus::logger); + LOG_ERR(log, "Error starting sensors"); + status.setError(&DeathStackStatus::sensors); } - if (!broker->start()) + if (!state_machines->start()) { - status.setError(&DeathStackStatus::ev_broker); + LOG_ERR(log, "Error starting state machines"); + status.setError(&DeathStackStatus::state_machines); } - if (!pin_observer->start()) + if (!pin_handler->start()) { + LOG_ERR(log, "Error starting PinObserver"); status.setError(&DeathStackStatus::pin_obs); } - /* State Machines */ - if (!fmm->start()) - { - status.setError(&DeathStackStatus::fmm); - } - if (!tmtc->start()) - { - status.setError(&DeathStackStatus::tmtc); - } - if (!dpl->start()) - { - status.setError(&DeathStackStatus::dpl); - } - if (!ada->start()) - { - status.setError(&DeathStackStatus::ada); - } - if (!sensor_manager->start()) - { - status.setError(&DeathStackStatus::sensor_manager); - } - TRACE("[DS] Sensor init status: %02X %d\n", - sensor_manager->getStatus().sensor_status, - sensor_manager->getStatus().sensor_status); - if (sensor_manager->getStatus().sensor_status != - NOMINAL_SENSOR_INIT_VALUE) - { - status.setError(&DeathStackStatus::sensor_manager); - } +#ifdef DEBUG + injector->start(); +#endif logger->log(status); +#ifdef HARDWARE_IN_THE_LOOP + hil->start(); +#endif + // If there was an error, signal it to the FMM and light a LED. if (status.death_stack != COMP_OK) { + LOG_ERR(log, "Initalization failed\n"); sEventBroker->post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); } else { + LOG_INFO(log, "Initalization ok"); sEventBroker->post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); } + } + + void startLogger() + { + try + { + logger->start(); + LOG_INFO(log, "Logger started"); + } + catch (const std::runtime_error& re) + { + LOG_ERR(log, "SD Logger init error"); + status.setError(&DeathStackStatus::logger); + } - TRACE("[DS] Init finished\n"); + logger->log(logger->getLogger().getLogStats()); } + +private: /** - * Helpers for debugging purposes + * @brief Initialize Everything. */ + DeathStack() + { + /* Shared components */ + logger = Singleton<LoggerService>::getInstance(); + startLogger(); + + TimestampTimer::enableTimestampTimer(); + + broker = sEventBroker; + + // Bind the logEvent function to the event sniffer in order to log every + // event + { + using namespace std::placeholders; + sniffer = new EventSniffer( + *broker, TOPIC_LIST, bind(&DeathStack::logEvent, this, _1, _2)); + } + +#ifdef HARDWARE_IN_THE_LOOP + hil = HIL::getInstance(); +#endif + + scheduler = new TaskScheduler(); + addSchedulerStatsTask(); + + bus = new Bus(); + radio = new Radio(*bus->spi2); + sensors = new Sensors(*bus->spi1, scheduler); + actuators = new Actuators(); + +#ifdef HARDWARE_IN_THE_LOOP + state_machines = + new StateMachines(*sensors->hil_imu, *sensors->hil_baro, + *sensors->hil_gps, scheduler); +#else + state_machines = new StateMachines(*sensors->imu_bmx160_with_correction, + *sensors->press_digital, + *sensors->gps_ublox, scheduler); +#endif + + pin_handler = new PinHandler(); + +#ifdef DEBUG + injector = new EventInjector(); +#endif + + LOG_INFO(log, "Init finished"); + } + /** + * @brief Helpers for debugging purposes. + */ void logEvent(uint8_t event, uint8_t topic) { - EventData ev{miosix::getTick(), event, topic}; + EventData ev{(long long)TimestampTimer::getTimestamp(), event, topic}; logger->log(ev); #ifdef DEBUG @@ -194,15 +229,36 @@ private: return; } } - TRACE("[%.3f] %s on %s\n", (miosix::getTick() / 1000.0f), - getEventString(event).c_str(), getTopicString(topic).c_str()); + LOG_DEBUG(log, "{:s} on {:s}", getEventString(event), + getTopicString(topic)); #endif } inline void postEvent(Event ev, uint8_t topic) { broker->post(ev, topic); } -private: - DeathStackStatus status; + void addSchedulerStatsTask() + { + // add lambda to log scheduler tasks statistics + scheduler->add( + [&]() { + std::vector<TaskStatResult> scheduler_stats = + scheduler->getTaskStats(); + + for (TaskStatResult stat : scheduler_stats) + { + logger->log(stat); + } + + StackLogger::getInstance()->updateStack(THID_TASK_SCHEDULER); + }, + 1000, // 1 hz + TASK_SCHEDULER_STATS_ID, miosix::getTick()); + } + + EventInjector* injector; + DeathStackStatus status{}; + + PrintLogger log = Logging::getLogger("deathstack"); }; -} /* namespace DeathStackBoard */ \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/DeathStackStatus.h b/src/boards/DeathStack/DeathStackStatus.h index 8ae0dcea69adac4c1d492e946df8ad0dacf7b00c..29e80aeaf3fa0068e35c4dd2576bfa5f7d78944b 100644 --- a/src/boards/DeathStack/DeathStackStatus.h +++ b/src/boards/DeathStack/DeathStackStatus.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -32,8 +31,8 @@ namespace DeathStackBoard enum DeathStackComponentStatus { - COMP_OK = 0, - COMP_ERROR = 1 + COMP_ERROR = 0, + COMP_OK = 1 }; struct DeathStackStatus @@ -44,12 +43,9 @@ struct DeathStackStatus uint8_t logger = COMP_OK; uint8_t ev_broker = COMP_OK; uint8_t pin_obs = COMP_OK; - uint8_t fmm = COMP_OK; - uint8_t sensor_manager = COMP_OK; - uint8_t ada = COMP_OK; - uint8_t tmtc = COMP_OK; - uint8_t ign = COMP_OK; - uint8_t dpl = COMP_OK; + uint8_t sensors = COMP_OK; + uint8_t radio = COMP_OK; + uint8_t state_machines = COMP_OK; /** * @brief Helper method to signal an error in the DeathStackStatus struct. @@ -65,14 +61,15 @@ struct DeathStackStatus static std::string header() { - return "logger,ev_broker,pin_obs,fmm,sensor_manager,ada,tmtc,ign,dpl\n"; + return "logger,ev_broker,pin_obs,sensors,radio,state_machines\n"; } void print(std::ostream& os) { - os << (int)logger << "," << (int)ev_broker << "," << (int)pin_obs << "," << (int)fmm << "," - << (int)sensor_manager << "," << (int)ada << "," << (int)tmtc << "," << (int)ign << "," - << (int)dpl << "\n"; + os << (int)logger << "," << (int)ev_broker << "," << (int)pin_obs << "," + << (int)sensors << "," << (int)radio << "," << (int)state_machines + << "\n"; } }; + } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Deployment/Deployment.scxml b/src/boards/DeathStack/Deployment/Deployment.scxml new file mode 100644 index 0000000000000000000000000000000000000000..8211ca5dde0d9ae81c57e47c1e138277dc1773c8 --- /dev/null +++ b/src/boards/DeathStack/Deployment/Deployment.scxml @@ -0,0 +1,31 @@ +<?xml version="1.0" encoding="UTF-8"?> +<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="idle" version="1.0"> + <state id="idle"> + <onentry>initServo()</onentry> + <onentry>initCutters()</onentry> + <transition event="DPL.EV_RESET_SERVO" target="idle"> + resetServo() + </transition> + <transition event="DPL.EV_WIGGLE_SERVO" target="idle"> + wiggleServo() + </transition> + <transition event="DPL.EV_NC_OPEN" target="noseconeEjection"/> + <transition event="DPL.EV_CUT_DROGUE" target="cuttingPrimary"/> + </state> + <state id="noseconeEjection"> + <onentry>ejectNosecone()</onentry> + <onentry>postD(DPL.EV_NC_OPEN_TIMEOUT)</onentry> + <onexit>disableServo()</onexit> + <transition event="DPL.EV_NC_DETACHED" target="idle"> + removeD(DPL.EV_NC_OPEN_TIMEOUT) + </transition> + <transition event="DPL.EV_NC_OPEN_TIMEOUT" target="idle"/> + </state> + <state id="cutting"> + <onentry>startCutting()</onentry> + <onentry>postD(DPL.EV_CUTTING_TIMEOUT)</onentry> + <onexit>stopCutting()</onexit> + <transition event="DPL.EV_CUTTING_TIMEOUT" target="idle"/> + </state> +</scxml> + diff --git a/src/boards/DeathStack/Deployment/DeploymentController.cpp b/src/boards/DeathStack/Deployment/DeploymentController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7ef47769535f6ae11b9874e6cc651b43dd1d7c07 --- /dev/null +++ b/src/boards/DeathStack/Deployment/DeploymentController.cpp @@ -0,0 +1,245 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Deployment/DeploymentController.h> +#include <LoggerService/LoggerService.h> +#include <System/StackLogger.h> +#include <TimestampTimer.h> +#include <configs/DeploymentConfig.h> +#include <events/EventBroker.h> +#include <events/Events.h> + +#include <LoggerService/LoggerService.h> +#include <System/StackLogger.h> +#include <TimestampTimer.h> +#include <configs/DeploymentConfig.h> +#include <events/EventBroker.h> +#include <events/Events.h> + +namespace DeathStackBoard +{ + +DeploymentController::DeploymentController(ServoInterface* ejection_servo) + : FSM(&DeploymentController::state_initialization), + ejection_servo{ejection_servo} +{ + sEventBroker->subscribe(this, TOPIC_DPL); +} + +DeploymentController::~DeploymentController() +{ + sEventBroker->unsubscribe(this); +} + +void DeploymentController::logStatus(DeploymentControllerState current_state) +{ + status.timestamp = TimestampTimer::getTimestamp(); + status.state = current_state; + status.servo_position = ejection_servo->getCurrentPosition(); + if (current_state == DeploymentControllerState::CUTTING) + { + status.cutters_enabled = true; + } + else + { + status.cutters_enabled = false; + } + + LoggerService::getInstance()->log(status); + StackLogger::getInstance()->updateStack(THID_DPL_FSM); +} + +void DeploymentController::state_initialization(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state initialization"); + + ejection_servo->enable(); + ejection_servo->reset(); + + logStatus(DeploymentControllerState::INITIALIZATION); + + transition(&DeploymentController::state_idle); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state initialization"); + break; + } + + default: + { + break; + } + } +} + +void DeploymentController::state_idle(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + logStatus(DeploymentControllerState::IDLE); + + LOG_DEBUG(log, "Entering state idle"); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state idle"); + break; + } + case EV_RESET_SERVO: + { + LOG_DEBUG(log, "Reset servo"); + ejection_servo->reset(); + break; + } + case EV_WIGGLE_SERVO: + { + LOG_DEBUG(log, "Wiggle servo"); + ejection_servo->selfTest(); + break; + } + case EV_NC_OPEN: + { + LOG_DEBUG(log, "Nosecone open event"); + transition(&DeploymentController::state_noseconeEjection); + break; + } + case EV_CUT_DROGUE: + { + LOG_DEBUG(log, "Cut drogue event"); + transition(&DeploymentController::state_cutting); + break; + } + default: + { + break; + } + } +} + +void DeploymentController::state_noseconeEjection(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state nosecone_ejection"); + + ejectNosecone(); + + ev_nc_open_timeout_id = sEventBroker->postDelayed<NC_OPEN_TIMEOUT>( + Event{EV_NC_OPEN_TIMEOUT}, TOPIC_DPL); + + logStatus(DeploymentControllerState::NOSECONE_EJECTION); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state nosecone_ejection"); + break; + } + case EV_NC_DETACHED: + { + LOG_DEBUG(log, "Nosecone detached event"); + sEventBroker->removeDelayed(ev_nc_open_timeout_id); + transition(&DeploymentController::state_idle); + break; + } + case EV_NC_OPEN_TIMEOUT: + { + LOG_DEBUG(log, "Nosecone opening timeout"); + transition(&DeploymentController::state_idle); + break; + } + default: + { + break; + } + } +} + +void DeploymentController::state_cutting(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state cutting"); + + startCutting(); + + ev_nc_cutting_timeout_id = sEventBroker->postDelayed<CUT_DURATION>( + Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL); + + logStatus(DeploymentControllerState::CUTTING); + + break; + } + case EV_EXIT: + { + stopCutting(); + + LOG_DEBUG(log, "Exiting state cutting"); + + break; + } + case EV_CUTTING_TIMEOUT: + { + LOG_DEBUG(log, "Cutter timeout"); + transition(&DeploymentController::state_idle); + break; + } + default: + { + break; + } + } +} + +void DeploymentController::ejectNosecone() +{ + ejection_servo->set(DPL_SERVO_EJECT_POS); +} + +void DeploymentController::startCutting() +{ + PrimaryCutterEna::getPin().high(); + BackupCutterEna::getPin().high(); + CuttersInput::getPin().high(); +} + +void DeploymentController::stopCutting() +{ + PrimaryCutterEna::getPin().low(); + BackupCutterEna::getPin().low(); + CuttersInput::getPin().low(); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Deployment/DeploymentController.h b/src/boards/DeathStack/Deployment/DeploymentController.h new file mode 100644 index 0000000000000000000000000000000000000000..d2cfc650992bba6ab562b2efee2346e6317c0173 --- /dev/null +++ b/src/boards/DeathStack/Deployment/DeploymentController.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Deployment/DeploymentData.h> +#include <Deployment/DeploymentServo.h> +#include <configs/CutterConfig.h> +#include <diagnostic/PrintLogger.h> +#include <drivers/hbridge/HBridge.h> +#include <drivers/servo/servo.h> +#include <events/Events.h> +#include <events/FSM.h> + +using namespace DeathStackBoard::DeploymentConfigs; +using namespace DeathStackBoard::CutterConfig; + +namespace DeathStackBoard +{ + +/** + * @brief Deployment state machine. + */ +class DeploymentController : public FSM<DeploymentController> +{ +public: + DeploymentController( + ServoInterface* ejection_servo = new DeploymentServo()); + ~DeploymentController(); + + void state_initialization(const Event& ev); + void state_idle(const Event& ev); + void state_noseconeEjection(const Event& ev); + void state_cutting(const Event& ev); + + void ejectNosecone(); + void startCutting(); + void stopCutting(); + +private: + DeploymentStatus status; + + ServoInterface* ejection_servo; + + uint16_t ev_nc_open_timeout_id = 0; + uint16_t ev_nc_cutting_timeout_id = 0; + + PrintLogger log = Logging::getLogger("deathstack.fsm.dpl"); + + void logStatus(DeploymentControllerState current_state); +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/DeploymentController/DeploymentData.h b/src/boards/DeathStack/Deployment/DeploymentData.h similarity index 60% rename from src/boards/DeathStack/DeploymentController/DeploymentData.h rename to src/boards/DeathStack/Deployment/DeploymentData.h index 9b0d8a46461caef4c484a05073a5afe73af73b6d..48f504a58207bd224af43336a752feda6bb25da6 100644 --- a/src/boards/DeathStack/DeploymentController/DeploymentData.h +++ b/src/boards/DeathStack/Deployment/DeploymentData.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -23,39 +22,48 @@ #pragma once -#include "Motor/MotorData.h" -#include "ThermalCutter/CutterData.h" +#include <drivers/hbridge/HBridgeData.h> +#include <stdint.h> + +#include <iostream> +#include <string> namespace DeathStackBoard { -enum DeploymentCTRLState : uint8_t +/** + * @brief Enum defining the possibile FSM states. + */ +enum class DeploymentControllerState : uint8_t { - DPL_IDLE = 0, - CUTTING_PRIMARY, - CUTTING_BACKUP, - EJECTING_NC, - TESTING_PRIMARY, - TESTING_BACKUP + INITIALIZATION = 0, + IDLE, + NOSECONE_EJECTION, + CUTTING, + TEST_CUTTING_PRIMARY, + TEST_CUTTING_BACKUP, }; +/** + * @brief Structure defining the overall controller status. + */ struct DeploymentStatus { - long long timestamp; - DeploymentCTRLState state; - CutterStatus cutter_status; - float servo_position; + uint64_t timestamp; + DeploymentControllerState state = DeploymentControllerState::INITIALIZATION; + bool cutters_enabled = false; + float servo_position = 0.0f; static std::string header() { - return "timestamp,state,cutter_state,servo_position\n"; + return "timestamp,state,cutters_enabled,servo_position\n"; } void print(std::ostream& os) const { - os << timestamp << "," << (int)state << "," << (int)cutter_status.state + os << timestamp << "," << (int)state << "," << (int)cutters_enabled << "," << servo_position << "\n"; } }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Deployment/DeploymentServo.h b/src/boards/DeathStack/Deployment/DeploymentServo.h new file mode 100644 index 0000000000000000000000000000000000000000..707c626cc6747e8ba5386d45ac8de3e11f9bc7e7 --- /dev/null +++ b/src/boards/DeathStack/Deployment/DeploymentServo.h @@ -0,0 +1,90 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <ServoInterface.h> +#include <configs/DeploymentConfig.h> +#include <drivers/servo/servo.h> +#include <miosix.h> + +namespace DeathStackBoard +{ + +using namespace DeathStackBoard::DeploymentConfigs; + +class DeploymentServo : public ServoInterface +{ +public: + Servo servo{DPL_SERVO_TIMER}; + + DeploymentServo() + : ServoInterface(DPL_SERVO_MIN_POS, DPL_SERVO_MAX_POS, + DPL_SERVO_RESET_POS) + { + } + + DeploymentServo(float minPosition, float maxPosition, float resetPosition) + : ServoInterface(minPosition, maxPosition, resetPosition) + { + } + + void enable() override + { + servo.setMinPulseWidth(500); + servo.setMaxPulseWidth(2500); + servo.enable(DPL_SERVO_PWM_CH); + servo.start(); + } + + void disable() override + { + servo.stop(); + servo.disable(DPL_SERVO_PWM_CH); + } + + /** + * @brief Perform wiggle around the reset position. + */ + void selfTest() override + { + for (int i = 0; i < 3; i++) + { + set(RESET_POS - DPL_SERVO_WIGGLE_AMPLITUDE); + miosix::Thread::sleep(500); + reset(); + miosix::Thread::sleep(500); + } + } + +protected: + void setPosition(float angle) override + { + currentPosition = angle; + servo.setPosition(DPL_SERVO_PWM_CH, currentPosition / 180.0f); + } + +private: + float anglePrec = DPL_SERVO_RESET_POS; +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Deployment/readme.md b/src/boards/DeathStack/Deployment/readme.md new file mode 100644 index 0000000000000000000000000000000000000000..22794a437f9ec1bc87830603bfb04423dd344d8a --- /dev/null +++ b/src/boards/DeathStack/Deployment/readme.md @@ -0,0 +1,14 @@ +```mermaid +stateDiagram-v2 + [*] --> idle + idle --> nosecone_ejection : EV_NC_OPEN + idle --> cutting_primary : EV_CUT_DROGUE + idle --> test_cutting_primary : EV_TEST_CUT_PRIMARY + idle --> test_cutting_backup : EV_TEST_CUT_BACKUP + nosecone_ejection --> idle : EV_NC_DETACHED + nosecone_ejection --> idle : EV_NC_OPEN_TIMEOUT + cutting_primary --> cutting_backup : EV_CUTTING_TIMEOUT + cutting_backup --> idle : EV_CUTTING_TIMEOUT + test_cutting_primary --> idle : EV_CUTTING_TIMEOUT + test_cutting_backup --> idle : EV_CUTTING_TIMEOUT +``` diff --git a/src/boards/DeathStack/DeploymentController/DeploymentController.cpp b/src/boards/DeathStack/DeploymentController/DeploymentController.cpp deleted file mode 100644 index 24109fe79e436a2f304983a0a0d3126cf44c942c..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/DeploymentController/DeploymentController.cpp +++ /dev/null @@ -1,426 +0,0 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta, Alvise de' Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <stdexcept> - -#include "DeathStack/configs/DeploymentConfig.h" -#include "DeathStack/events/Events.h" -#include "DeploymentController.h" - -#include "Motor/MotorDriver.h" -#include "events/EventBroker.h" - -namespace DeathStackBoard -{ - -using namespace DeploymentConfigs; - -DeploymentController::DeploymentController(Cutter& cutter, - Servo& ejection_servo) - : HSM(&DeploymentController::state_initialization, 4096, 2), - cutters(cutter), ejection_servo(ejection_servo) -{ - memset(&status, 0, sizeof(DeploymentStatus)); - - sEventBroker->subscribe(this, TOPIC_DEPLOYMENT); - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_TC); -} - -DeploymentController::~DeploymentController() -{ - sEventBroker->unsubscribe(this); -} - -State DeploymentController::state_initialization(const Event& ev) -{ - initServo(); - - UNUSED(ev); - return transition(&DeploymentController::state_idle); -} - -State DeploymentController::state_idle(const Event& ev) -{ - State retState = HANDLED; - switch (ev.sig) - { - case EV_ENTRY: - { - logStatus(DeploymentCTRLState::DPL_IDLE); - // Process deferred events - try - { - while (!deferred_events.isEmpty()) - { - postEvent(deferred_events.pop()); - } - } - catch (...) - { - TRACE("Tried to pop empty circularbuffer!\n"); - } - - TRACE("[DPL_CTRL] state_idle ENTRY\n"); - break; - } - case EV_INIT: - { - break; - } - case EV_EXIT: - { - TRACE("[DPL_CTRL] state_idle EXIT\n"); - - break; - } - case EV_NC_OPEN: - { - retState = - transition(&DeploymentController::state_ejectingNosecone); - break; - } - case EV_CUT_DROGUE: - { - cut_backup = true; - retState = transition(&DeploymentController::state_cuttingPrimary); - break; - } - case EV_CUT_PRIMARY: - { - cut_backup = false; - retState = transition(&DeploymentController::state_cuttingPrimary); - break; - } - case EV_CUT_BACKUP: - { - retState = transition(&DeploymentController::state_cuttingBackup); - break; - } - case EV_TEST_CUTTER_PRIMARY: - { - retState = transition(&DeploymentController::state_testingPrimary); - break; - } - case EV_TEST_CUTTER_BACKUP: - { - retState = transition(&DeploymentController::state_testingBackup); - break; - } - case EV_RESET_SERVO: - { - resetServo(); - break; - } - case EV_WIGGLE_SERVO: - { - wiggleServo(); - break; - } - default: - { - retState = tran_super(&DeploymentController::Hsm_top); - break; - } - } - return retState; -} - -State DeploymentController::state_ejectingNosecone(const Event& ev) -{ - State retState = HANDLED; - switch (ev.sig) - { - case EV_ENTRY: - { - ejectNosecone(); - ev_open_timeout_id = sEventBroker->postDelayed<NC_OPEN_TIMEOUT>( - Event{EV_TIMEOUT_NC_OPEN}, TOPIC_DEPLOYMENT); - - logStatus(DeploymentCTRLState::EJECTING_NC); - TRACE("[DPL_CTRL] state_ejectingNosecone ENTRY\n"); - break; - } - case EV_INIT: - { - TRACE("[DPL_CTRL] state_ejectingNosecone INIT\n"); - break; - } - case EV_EXIT: - { - // disableServo(); - sEventBroker->removeDelayed(ev_open_timeout_id); - - TRACE("[DPL_CTRL] state_openingNosecone EXIT\n"); - - break; - } - case EV_CUT_DROGUE: - { - deferred_events.put(ev); - break; - } - case EV_NC_DETACHED: - case EV_TIMEOUT_NC_OPEN: - { - retState = transition(&DeploymentController::state_idle); - break; - } - default: - { - retState = tran_super(&DeploymentController::Hsm_top); - break; - } - } - return retState; -} - -State DeploymentController::state_cuttingPrimary(const Event& ev) -{ - State retState = HANDLED; - switch (ev.sig) - { - case EV_ENTRY: - { - // Cutting drogue - cutters.enablePrimaryCutter(); - - logStatus(DeploymentCTRLState::CUTTING_PRIMARY); - - ev_cut_timeout_id = sEventBroker->postDelayed<CUT_DURATION>( - {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT); - - TRACE("[DPL_CTRL] state_cuttingPrimary ENTRY\n"); - break; - } - case EV_INIT: - { - break; - } - case EV_EXIT: - { - cutters.disablePrimaryCutter(); - - sEventBroker->removeDelayed(ev_cut_timeout_id); - TRACE("[DPL_CTRL] state_cuttingPrimary EXIT\n"); - - break; - } - case EV_TIMEOUT_CUTTING: - { - if (cut_backup) - { - retState = - transition(&DeploymentController::state_cuttingBackup); - } - else - { - retState = transition(&DeploymentController::state_idle); - } - break; - } - default: - { - retState = tran_super(&DeploymentController::Hsm_top); - break; - } - } - return retState; -} - -State DeploymentController::state_cuttingBackup(const Event& ev) -{ - State retState = HANDLED; - switch (ev.sig) - { - case EV_ENTRY: - { - // Cutting rogallina - cutters.enableBackupCutter(); - - logStatus(DeploymentCTRLState::CUTTING_BACKUP); - - ev_cut_timeout_id = sEventBroker->postDelayed<CUT_DURATION>( - {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT); - - TRACE("[DPL_CTRL] state_cuttingBackup ENTRY\n"); - break; - } - case EV_INIT: - { - break; - } - case EV_EXIT: - { - cutters.disableBackupCutter(); - - sEventBroker->removeDelayed(ev_cut_timeout_id); - - TRACE("[DPL_CTRL] state_cuttingBackup EXIT\n"); - - break; - } - case EV_TIMEOUT_CUTTING: - { - retState = transition(&DeploymentController::state_idle); - break; - } - default: - { - retState = tran_super(&DeploymentController::Hsm_top); - break; - } - } - return retState; -} - -State DeploymentController::state_testingPrimary(const Event& ev) -{ - State retState = HANDLED; - switch (ev.sig) - { - case EV_ENTRY: - { - cutters.enableTestPrimaryCutter(); - - logStatus(DeploymentCTRLState::TESTING_PRIMARY); - - ev_cut_timeout_id = sEventBroker->postDelayed<CUT_TEST_DURATION>( - {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT); - - TRACE("[DPL_CTRL] state_testingPrimary ENTRY\n"); - break; - } - case EV_INIT: - { - break; - } - case EV_EXIT: - { - cutters.disablePrimaryCutter(); - - sEventBroker->removeDelayed(ev_cut_timeout_id); - - TRACE("[DPL_CTRL] state_testingPrimary EXIT\n"); - - break; - } - case EV_TIMEOUT_CUTTING: - { - retState = transition(&DeploymentController::state_idle); - break; - } - default: - { - retState = tran_super(&DeploymentController::Hsm_top); - break; - } - } - return retState; -} - -State DeploymentController::state_testingBackup(const Event& ev) -{ - State retState = HANDLED; - switch (ev.sig) - { - case EV_ENTRY: - { - // Cutting rogallina - cutters.enableTestBackupCutter(); - - logStatus(DeploymentCTRLState::TESTING_BACKUP); - - ev_cut_timeout_id = sEventBroker->postDelayed<CUT_TEST_DURATION>( - {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT); - - TRACE("[DPL_CTRL] state_testingBackup ENTRY\n"); - break; - } - case EV_INIT: - { - break; - } - case EV_EXIT: - { - cutters.disableBackupCutter(); - - sEventBroker->removeDelayed(ev_cut_timeout_id); - - TRACE("[DPL_CTRL] state_testingBackup EXIT\n"); - - break; - } - case EV_TIMEOUT_CUTTING: - { - retState = transition(&DeploymentController::state_idle); - break; - } - default: - { - retState = tran_super(&DeploymentController::Hsm_top); - break; - } - } - return retState; -} - -void DeploymentController::initServo() -{ - ejection_servo.enable(SERVO_CHANNEL); - - ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS); - - ejection_servo.start(); -} - -void DeploymentController::resetServo() -{ - ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS); -} - -void DeploymentController::ejectNosecone() -{ - ejection_servo.setPosition(SERVO_CHANNEL, SERVO_EJECT_POS); -} - -void DeploymentController::disableServo() -{ - // Do not reset the position to 0 - - ejection_servo.stop(); - ejection_servo.disable(SERVO_CHANNEL); -} - -void DeploymentController::wiggleServo() -{ - for (int i = 0; i < 3; i++) - { - ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS - SERVO_WIGGLE_AMPLITUDE); - Thread::sleep(500); - ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS); - Thread::sleep(500); - } -} - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/DeploymentController/DeploymentController.h b/src/boards/DeathStack/DeploymentController/DeploymentController.h deleted file mode 100644 index c8d0f9db3859c7997f9c5c3caa663c56d1753acc..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/DeploymentController/DeploymentController.h +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <drivers/servo/servo.h> -#include "DeathStack/LoggerService/LoggerService.h" -#include "DeathStack/System/StackLogger.h" -#include "DeathStack/configs/DeploymentConfig.h" -#include "DeploymentData.h" -#include "Motor/MotorDriver.h" -#include "ThermalCutter/Cutter.h" -#include "events/HSM.h" -#include "utils/collections/CircularBuffer.h" - -class PinObserver; - -namespace DeathStackBoard -{ - -/** - * @brief Deployment Controller State Machine - */ -class DeploymentController : public HSM<DeploymentController> -{ -public: - DeploymentController(Cutter &cutter, Servo &ejection_servo); - ~DeploymentController(); - - State state_initialization(const Event &ev); - - State state_idle(const Event &ev); - - State state_cuttingPrimary(const Event &ev); - State state_cuttingBackup(const Event &ev); - - State state_testingPrimary(const Event &ev); - State state_testingBackup(const Event &ev); - - State state_ejectingNosecone(const Event &ev); - -private: - /** - * @brief Logs the DeploymentStatus struct updating the timestamp and the - * current state - * - * @param current_state - */ - void logStatus(DeploymentCTRLState current_state) - { - status.state = current_state; - logStatus(); - } - /** - * @brief Logs the DeploymentStatus struct updating the timestamp - */ - void logStatus() - { - status.timestamp = miosix::getTick(); - status.cutter_status = cutters.getStatus(); - status.servo_position = - ejection_servo.getPosition(DeploymentConfigs::SERVO_CHANNEL); - - logger.log(status); - StackLogger::getInstance()->updateStack(THID_DPL_FSM); - } - - void initServo(); - void resetServo(); - void ejectNosecone(); - void disableServo(); - - /** - * @brief Wiggle the servo just a bit around the reset position to show it's - * working - */ - void wiggleServo(); - - /** - * Defer an event to be processed when the state machine goes back to - * state_idle - * - * @param ev The event to be defered. - */ - void deferEvent(const Event &ev); - - Cutter &cutters; - Servo &ejection_servo; - - DeploymentStatus status; - - LoggerService &logger = *(LoggerService::getInstance()); - - CircularBuffer<Event, DEFERRED_EVENTS_QUEUE_SIZE> deferred_events; - - bool cut_backup = true; - - uint16_t ev_open_timeout_id = 0; - uint16_t ev_reset_timeout_id = 0; - uint16_t ev_cut_timeout_id = 0; -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp b/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp deleted file mode 100644 index 631800c922bb02e022b73397125b6efd7a72ecee..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise de'Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#include <Common.h> -#include <interfaces-impl/hwmapping.h> - -#include "DeathStack/events/Events.h" -#include "MotorDriver.h" - -using namespace miosix; - -namespace DeathStackBoard -{ - -MotorDriver::MotorDriver() { status.motor_active = 0; } - -MotorDriver::~MotorDriver() { stop(); } - -void MotorDriver::start(MotorDirection direction) -{ - // Ensure the pins are configured in output mode. (The rogallo controller - // may set these pins to "alternate mode" to use PWM to drive the servos) - nosecone::motP1::mode(Mode::OUTPUT); - nosecone::motP2::mode(Mode::OUTPUT); - nosecone::motP1::low(); - nosecone::motP2::low(); - - if (direction == MotorDirection::NORMAL) - { - nosecone::motP1::high(); - nosecone::motP2::low(); - } - else - { - nosecone::motP1::low(); - nosecone::motP2::high(); - } - - /* Activate PWM on both half bridges */ - nosecone::motEn::high(); - - /* Update status */ - status.motor_active = true; - status.motor_last_direction = direction; -} - -void MotorDriver::stop() -{ - nosecone::motP1::low(); - nosecone::motP2::low(); - - Thread::sleep(MOTOR_DISABLE_DELAY_MS); // Wait a short delay - - nosecone::motEn::low(); - - status.motor_active = false; -} - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/DeploymentController/README.md b/src/boards/DeathStack/DeploymentController/README.md deleted file mode 100644 index 8ed3ce0def64db9b85ce94d569f4ac58573e5219..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/DeploymentController/README.md +++ /dev/null @@ -1,25 +0,0 @@ - ```plantuml -skinparam titleFontSize 34 - -title Deployment State Machine - -state Ready { - [*] --> CutterIdle - - state CutterIdle - state CuttingDrogue - state CuttingMain - state C <<choice>> -} - -[*] --> Ready -CutterIdle --> CuttingMain : EV_TC_CUT_MAIN -CutterIdle -r-> CuttingDrogue : EV_CUT_DROGUE - -CuttingDrogue --> C : EV_TIMEOUT_CUTTING -C --> CuttingMain : [cut_main = true] -C --> CutterIdle : [cut_main = false] - -CuttingMain --> CutterIdle : EV_TIMEOUT_CUTTING - - ``` \ No newline at end of file diff --git a/src/boards/DeathStack/DeploymentController/ThermalCutter/Cutter.h b/src/boards/DeathStack/DeploymentController/ThermalCutter/Cutter.h deleted file mode 100644 index 43c2678b6fc27d81bad3719c0b11c8c8d4718b8b..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/DeploymentController/ThermalCutter/Cutter.h +++ /dev/null @@ -1,184 +0,0 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <miosix.h> - -#include "CutterData.h" -#include "DeathStack/configs/CutterConfig.h" -#include "drivers/pwm/pwm.h" - -using miosix::GpioPin; -using miosix::Thread; - -namespace DeathStackBoard -{ - -/** - * @brief Interface class to operate the thermal cutters on the Hermev V1 - * rocket. - * - * Provides the ability to enable a cutter using the "nominal" duty cycle or - * using a "test" duty cycle (lower duty cycle that does not cut the parachute - * but is used to check if current is flowing) - */ -class Cutter -{ -public: - /** - * @brief Create a new Cutter - * - * @param frequency Frequency of the PWM driving the cutters - * @param duty_cycle Duty cycle of the PWM driving the cutters - * @param test_duty_cycle Duty cycle to be used when testing the - * cutters for continuity - */ - Cutter(unsigned int frequency, float duty_cycle, float test_duty_cycle) - : pwm(CUTTER_TIM, frequency), - pin_enable_primary(PrimaryCutterEna::getPin()), - pin_enable_backup(BackupCutterEna::getPin()), - cut_duty_cycle(duty_cycle), test_duty_cycle(test_duty_cycle) - { - pin_enable_primary.low(); - pin_enable_backup.low(); - - // Start PWM with 0 duty cycle to keep IN pins low - pwm.enableChannel(CUTTER_CHANNEL_PRIMARY, 0.0f); - pwm.enableChannel(CUTTER_CHANNEL_BACKUP, 0.0f); - - pwm.start(); - } - - ~Cutter() - { - disablePrimaryCutter(); - disableBackupCutter(); - - pwm.stop(); - } - - /** - * @brief Activates the primary cutter. - */ - void enablePrimaryCutter() - { - enableCutter(CUTTER_CHANNEL_PRIMARY, pin_enable_primary, - cut_duty_cycle); - status.state = CutterState::CUTTING_PRIMARY; - } - - /** - * @brief Deactivates the primary cutter - */ - void disablePrimaryCutter() - { - if (status.state == CutterState::CUTTING_PRIMARY) - { - disableCutter(CUTTER_CHANNEL_PRIMARY, pin_enable_primary); - status.state = CutterState::IDLE; - } - } - - /** - * @brief Activates the backup cutter. - */ - void enableBackupCutter() - { - enableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup, cut_duty_cycle); - status.state = CutterState::CUTTING_BACKUP; - } - - /** - * @brief Deactivates the pbackup cutter - */ - void disableBackupCutter() - { - if (status.state == CutterState::CUTTING_BACKUP) - { - disableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup); - status.state = CutterState::IDLE; - } - } - - /** - * @brief Enables the primary cutter using the "test" duty cycle - * - * call disablePrimaryCutter() to disable - */ - void enableTestPrimaryCutter() - { - enableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup, test_duty_cycle); - status.state = CutterState::TESTING_PRIMARY; - } - - /** - * @brief Enables the backup cutter using the "test" duty cycle - * - * call disableBackupCutter() to disable - */ - void enableTestBackupCutter() - { - enableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup, test_duty_cycle); - status.state = CutterState::TESTING_PRIMARY; - } - - CutterStatus getStatus() { return status; } - -private: - void enableCutter(PWMChannel channel, miosix::GpioPin& ena_pin, - float duty_cycle) - { - // Only enable if the cutter is currently idle - if (status.state == CutterState::IDLE) - { - // Enable PWM Generation - pwm.setDutyCycle(channel, duty_cycle); - - // enable - ena_pin.high(); - } - } - - void disableCutter(PWMChannel channel, miosix::GpioPin& ena_pin) - { - pwm.setDutyCycle(channel, 0.0f); // Set duty cycle to 0 to leave the IN - // pin of the Half-H bridge low - - Thread::sleep(CUTTER_DISABLE_DELAY_MS); // Wait a short delay - - ena_pin.low(); // Disable hbridge - } - - Cutter(const Cutter& c) = delete; - - PWM pwm; - GpioPin pin_enable_primary; - GpioPin pin_enable_backup; - - float cut_duty_cycle; - float test_duty_cycle; - CutterStatus status; -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/DeploymentController/ThermalCutter/PidCutter.h b/src/boards/DeathStack/DeploymentController/ThermalCutter/PidCutter.h deleted file mode 100644 index 6c133acc76df70a818d505a6300d13685394bf68..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/DeploymentController/ThermalCutter/PidCutter.h +++ /dev/null @@ -1,109 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <miosix.h> - -#include "ActiveObject.h" -#include "CutterData.h" -#include "DeathStack/configs/CutterConfig.h" -#include "drivers/pwm/pwm.h" - -using miosix::GpioPin; -using miosix::Thread; - -namespace DeathStackBoard -{ -class PidCutter : public ActiveObject -{ -public: - PidCutter() - : pwm(CUTTER_TIM, CUTTER_PWM_FREQUENCY), - pin_enable_drogue(DrogueCutterEna::getPin()), - pin_enable_main_chute(MainChuteCutterEna::getPin()) - { - pin_enable_drogue.low(); - pin_enable_main_chute.low(); - - // Start PWM with 0 duty cycle to keep IN pins low - pwm.enableChannel(CUTTER_CHANNEL_DROGUE, 0.0f); - pwm.enableChannel(CUTTER_CHANNEL_MAIN_CHUTE, 0.0f); - - pwm.start(); - } - - ~PidCutter() - { - stopCutDrogue(); - stopCutMainChute(); - pwm.stop(); - } - - void startCutDrogue() { cut_drogue = true; } - void stopCutDrogue() { cut_drogue = false; } - void startCutMainChute() { cut_main = true; } - void stopCutMainChute() { cut_main = false; } - - void updateCut1Current(float current) { cut_1_curr = current; } - void updateCut2Current(float current) { cut_2_curr = current; } - -protected: - void run() override - { - for (;;) - { - if (cut_drogue) - { - float err = cut_1_curr - CUT_1_TARGET_CURR; - pwm.setDutyCycle(CUTTER_CHANNEL_DROGUE, err * PID_K); - } - else if (cut_main) - { - float err = cut_2_curr - CUT_2_TARGET_CURR; - pwm.setDutyCycle(CUTTER_CHANNEL_MAIN_CHUTE, err * PID_K); - } - else - { - pwm.setDutyCycle(CUTTER_CHANNEL_DROGUE, 0.0f); - pwm.setDutyCycle(CUTTER_CHANNEL_MAIN_CHUTE, 0.0f); - } - - Thread::sleep(50); - } - } - -private: - float cut_1_curr = 0; - float cut_2_curr = 0; - bool cut_drogue = false; - bool cut_main = false; - - PWM pwm; - - GpioPin pin_enable_drogue; - GpioPin pin_enable_main_chute; - - CutterStatus status; -}; -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/FlightModeManager/FMM.scxml b/src/boards/DeathStack/FlightModeManager/FMM.scxml new file mode 100644 index 0000000000000000000000000000000000000000..ea6ea0cac1cd7311efc8c32c9686ee1dd561a150 --- /dev/null +++ b/src/boards/DeathStack/FlightModeManager/FMM.scxml @@ -0,0 +1,76 @@ +<scxml version="1.0" xmlns="http://www.w3.org/2005/07/scxml" initial="OnGround"> + <state id="OnGround"> <!-- Super-state --> + <transition event="TMTC.EV_TC_RESET_BOARD" target="OnGround">logger.stop(); reboot();</transition> + <transition event="TMTC.EV_TC_LAUNCH" target="Flying"/> + <state id="Init"> + <transition event="FMM.EV_INIT_OK" target="InitDone"></transition> + <transition event="FMM.EV_INIT_ERROR" target="InitError"></transition> + </state> + <state id="InitError"> + <transition event="TMTC.EV_TC_FORCE_INIT" target="InitDone"></transition> + </state> + <state id="InitDone"> + <transition event="TMTC.EV_TC_TEST_MODE" target="TestMode"></transition> + <transition event="TMTC.EV_TC_CALIBRATE_SENSORS" target="SensorsCalibration"></transition> + </state> + <state id="TestMode"> + <transition event="TMTC.EV_TC_NC_OPEN" target="TestMode">post(DPL.EV_NC_OPEN)</transition> + <transition event="TMTC.EV_TC_DPL_WIGGLE_SERVO" target="TestMode">post(DPL.EV_WIGGLE_SERVO)</transition> + <transition event="TMTC.EV_TC_DPL_RESET_SERVO" target="TestMode">post(DPL.EV_RESET_SERVO)</transition> + <transition event="TMTC.EV_TC_CUT_DROGUE" target="TestMode">post(DPL.EV_CUT_DROGUE)</transition> + <transition event="TMTC.EV_TC_ABK_WIGGLE_SERVO" target="TestMode">post(ABK.EV_WIGGLE_SERVO)</transition> + <transition event="TMTC.EV_TC_ABK_RESET_SERVO" target="TestMode">post(ABK.EV_RESET_SERVO)</transition> + <transition event="TMTC.EV_TC_TEST_ABK" target="TestMode">post(ABK.EV_TEST_ABK)</transition> + <transition event="TMTC.EV_TC_CLOSE_LOG" target="TestMode">logger.stop()</transition> + <transition event="TMTC.EV_TC_START_LOG" target="TestMode">DeathStack.startLogger()</transition> + </state> + <state id="SensorsCalibration"> + <transition event="TMTC.EV_TC_CALIBRATE_SENSORS" target="SensorsCalibration"></transition> + <transition event="FLIGHT_EVENTS.EV_SENSORS_READY" target="AlgosCalibration"></transition> + </state> + <state id="AlgosCalibration"> + <onentry>post(ADA.EV_CALIBRATE_ADA); post(NAS.EV_CALIBRATE_NAS)</onentry> + <transition event="TMTC.EV_TC_CALIBRATE_ALGOS" target="AlgosCalibration"></transition> + <transition event="ADA.EV_ADA_READY" target="AlgosCalibration">ada_ready = true</transition> + <transition event="NAS.EV_NAS_READY" target="AlgosCalibration">nas_ready = true</transition> + <transition event="FLIGHT_EVENTS.EV_CALIBRATION_OK" target="Disarmed"></transition> + </state> + <state id="Disarmed"> + <transition event="TMTC.EV_TC_CALIBRATE_SENSORS" target="SensorsCalibration"></transition> + <transition event="TMTC.EV_TC_CALIBRATE_ALGOS" target="AlgosCalibration"></transition> + <transition event="TMTC.EV_TC_ARM" target="Armed"></transition> + </state> + </state> + <state id="Armed"> + <onentry>post(FLIGHT_EVENTS.EV_ARMED)</onentry> + <transition event="TMTC.EV_TC_DISARM" target="Disarmed"></transition> + <transition event="FLIGHT_EVENTS.EV_UMBILICAL_DETACHED" target="Ascending"></transition> + <transition event="TMTC.EV_TC_LAUNCH" target="Ascending"></transition> + </state> + <state id="Flying"> <!-- Super-state --> + <onentry>postD(FMM.EV_TIMEOUT_END_MISSION); post(FLIGHT_EVENTS.EV_LIFTOFF);</onentry> + <transition event="TMTC.EV_TC_END_MISSION" target="Landed"></transition> + <transition event="FMM.EV_TIMEOUT_END_MISSION" target="Landed"></transition> + <transition event="TMTC.EV_TC_NC_OPEN" target="Flying">post(DPL.EV_NC_OPEN)</transition> + <transition event="TMTC.EV_TC_ABK_DISABLE" target="Flying">post(ABK.EV_DISABLE_ABK)</transition> + <state id="Ascending"> + <transition event="ADA.EV_ADA_DISABLE_ABK" target="Ascending">post(ABK.EV_DISABLE_ABK)</transition> + <transition event="ADA.EV_ADA_APOGEE_DETECTED" target="Descending">post(FLIGHT_EVENTS.EV_APOGEE)</transition> + <transition event="TMTC.EV_TC_NC_OPEN" target="Descending">post(FLIGHT_EVENTS.EV_APOGEE)</transition> + </state> + <state id="DrogueDescent"> + <onentry>post(DPL.EV_NC_OPEN)</onentry> + <transition event="ADA.EV_ADA_DPL_ALT_DETECTED" target="TerminalDescent"></transition> + <transition event="TMTC.EV_TC_CUT_DROGUE" target="TerminalDescent"></transition> + <transition event="FLIGHT_EVENTS.EV_NC_DETACHED" target="DrogueDescent">post(FLIGHT_EVENTS.EV_NC_DETACHED)</transition> + </state> + <state id="TerminalDescent"> + <onentry>post(FLIGHT_EVENTS.EV_DPL_ALTITUDE); post(DPL.EV_CUT_DROGUE);</onentry> + <transition event="TMTC.EV_TC_CUT_DROGUE" target="TerminalDescent">post(DPL.EV_CUT_DROGUE)</transition> + </state> + </state> + <state id="Landed"> + <onentry>logger.stop(); post(FLIGHT_EVENTS.EV_LANDED);</onentry> + <transition event="TMTC.EV_TC_RESET_BOARD" target="Landed">reboot()</transition> + </state> +</scxml> diff --git a/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp b/src/boards/DeathStack/FlightModeManager/FMMController.cpp similarity index 56% rename from src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp rename to src/boards/DeathStack/FlightModeManager/FMMController.cpp index 2e50d0c57208df2c9d3c43dbea0c4e1bdf012b1a..145b803425b903f56ea349fd697e61bf60414fcf 100644 --- a/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp +++ b/src/boards/DeathStack/FlightModeManager/FMMController.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,61 +13,60 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <DeathStack/FlightModeManager/FlightModeManager.h> +#include <FlightModeManager/FMMController.h> +#include <System/StackLogger.h> +#include <configs/FMMConfig.h> #include <events/EventBroker.h> - -#include "DeathStack/System/StackLogger.h" -#include "DeathStack/configs/FMMConfig.h" -#include "DeathStack/events/Events.h" -#include "DeathStack/events/Topics.h" - -#include "Debug.h" +#include <events/Events.h> +#include <events/Topics.h> +#include <miosix.h> namespace DeathStackBoard { -FlightModeManager::FlightModeManager() - : HSM(&FlightModeManager::state_initialization, 4096, 2), +FMMController::FMMController() + : HSM(&FMMController::state_initialization, STACK_MIN_FOR_SKYWARD, + FMM_PRIORITY), logger(*(LoggerService::getInstance())) { sEventBroker->subscribe(this, TOPIC_ADA); - sEventBroker->subscribe(this, TOPIC_TC); + sEventBroker->subscribe(this, TOPIC_NAS); + sEventBroker->subscribe(this, TOPIC_TMTC); sEventBroker->subscribe(this, TOPIC_FMM); sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); } -FlightModeManager::~FlightModeManager() +FMMController::~FMMController() { // Unsubscribe from all topics sEventBroker->unsubscribe(this); } -void FlightModeManager::logState(FMMState current_state) +void FMMController::logState(FMMState current_state) { - status.timestamp = miosix::getTick(); + status.timestamp = TimestampTimer::getTimestamp(); status.state = current_state; logger.log(status); StackLogger::getInstance()->updateStack(THID_FMM_FSM); } -State FlightModeManager::state_initialization(const Event& ev) +State FMMController::state_initialization(const Event& ev) { // Nothing to do during initialization UNUSED(ev); - return transition(&FlightModeManager::state_onGround); + return transition(&FMMController::state_onGround); } -/* Handle TC_BOARD_RESET and TC_FORCE_LIFTOFF (super-state) */ -State FlightModeManager::state_onGround(const Event& ev) +State FMMController::state_onGround(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -75,25 +74,25 @@ State FlightModeManager::state_onGround(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::ON_GROUND); - TRACE("[FMM] Entering state_onGround\n"); + LOG_DEBUG(log, "Entering state_onGround"); break; } case EV_INIT: /* This is a super-state, so move to the first sub-state */ { - TRACE("[FMM] Init state_onGround\n"); + LOG_DEBUG(log, "Init state_onGround"); - retState = transition(&FlightModeManager::state_init); + retState = transition(&FMMController::state_init); break; } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exiting state_onGround\n"); + LOG_DEBUG(log, "Exiting state_onGround"); break; } - case EV_TC_BOARD_RESET: + case EV_TC_RESET_BOARD: { logger.stop(); miosix::reboot(); @@ -101,21 +100,20 @@ State FlightModeManager::state_onGround(const Event& ev) } case EV_TC_LAUNCH: { - retState = transition(&FlightModeManager::state_ascending); + retState = transition(&FMMController::state_flying); break; } default: /* If an event is not handled here, try with super-state */ { // Since this is an outer super-state, the parent is HSM_top - retState = tran_super(&FlightModeManager::Hsm_top); + retState = tran_super(&FMMController::Hsm_top); break; } } return retState; } -/* First state, wait for sensors and objects initialization */ -State FlightModeManager::state_init(const Event& ev) +State FMMController::state_init(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -123,7 +121,7 @@ State FlightModeManager::state_init(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::INIT); - TRACE("[FMM] Entering state_init\n"); + LOG_DEBUG(log, "Entering state_init"); break; } case EV_INIT: /* No sub-states */ @@ -132,30 +130,30 @@ State FlightModeManager::state_init(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exit state_init\n"); + LOG_DEBUG(log, "Exit state_init"); break; } case EV_INIT_ERROR: { - retState = transition(&FlightModeManager::state_initError); + retState = transition(&FMMController::state_initError); break; } case EV_INIT_OK: { - retState = transition(&FlightModeManager::state_initDone); + retState = transition(&FMMController::state_initDone); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_onGround); + retState = tran_super(&FMMController::state_onGround); break; } } return retState; } -State FlightModeManager::state_initError(const Event& ev) +State FMMController::state_initError(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -163,7 +161,7 @@ State FlightModeManager::state_initError(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::INIT_ERROR); - TRACE("[FMM] Entering state_initError\n"); + LOG_DEBUG(log, "Entering state_initError"); break; } case EV_INIT: /* No sub-states */ @@ -172,25 +170,25 @@ State FlightModeManager::state_initError(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exit state_initError\n"); + LOG_DEBUG(log, "Exit state_initError"); break; } case EV_TC_FORCE_INIT: { - retState = transition(&FlightModeManager::state_initDone); + retState = transition(&FMMController::state_initDone); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_onGround); + retState = tran_super(&FMMController::state_onGround); break; } } return retState; } -State FlightModeManager::state_initDone(const Event& ev) +State FMMController::state_initDone(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -198,7 +196,7 @@ State FlightModeManager::state_initDone(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::INIT_DONE); - TRACE("[FMM] Entering state_initDone\n"); + LOG_DEBUG(log, "Entering state_initDone"); break; } case EV_INIT: /* No sub-states */ @@ -207,42 +205,87 @@ State FlightModeManager::state_initDone(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exit state_initDone\n"); + LOG_DEBUG(log, "Exit state_initDone"); break; } - case EV_TC_CALIBRATE_ADA: + case EV_TC_CALIBRATE_SENSORS: { - retState = transition(&FlightModeManager::state_calibrating); + retState = transition(&FMMController::state_sensorsCalibration); break; } case EV_TC_TEST_MODE: { - retState = transition(&FlightModeManager::state_testing); + retState = transition(&FMMController::state_testMode); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_onGround); + retState = tran_super(&FMMController::state_onGround); break; } } return retState; } -/* Just wait the ADA event */ -State FlightModeManager::state_calibrating(const Event& ev) +State FMMController::state_sensorsCalibration(const Event& ev) { State retState = HANDLED; switch (ev.sig) { case EV_ENTRY: /* Executed everytime state is entered */ { - logState(FMMState::CALIBRATING); + logState(FMMState::SENSORS_CALIBRATION); + + LOG_DEBUG(log, "Entering sensors_calibration"); + + DeathStack::getInstance()->sensors->calibrate(); + sEventBroker->post({EV_SENSORS_READY}, TOPIC_FLIGHT_EVENTS); + + break; + } + case EV_INIT: /* No sub-state */ + { + break; + } + case EV_EXIT: /* Executed everytime state is exited */ + { + LOG_DEBUG(log, "Exit sensors_calibration"); + + break; + } + case EV_TC_CALIBRATE_SENSORS: + { + retState = transition(&FMMController::state_sensorsCalibration); + break; + } + case EV_SENSORS_READY: + { + retState = transition(&FMMController::state_algosCalibration); + break; + } + default: /* If an event is not handled here, try with super-state */ + { + retState = tran_super(&FMMController::state_onGround); + break; + } + } + return retState; +} + +State FMMController::state_algosCalibration(const Event& ev) +{ + State retState = HANDLED; + switch (ev.sig) + { + case EV_ENTRY: /* Executed everytime state is entered */ + { + logState(FMMState::ALGOS_CALIBRATION); sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_ADA); + sEventBroker->post({EV_CALIBRATE_NAS}, TOPIC_NAS); - TRACE("[FMM] Entering calibration\n"); + LOG_DEBUG(log, "Entering algos_calibration"); break; } case EV_INIT: /* No sub-state */ @@ -251,25 +294,50 @@ State FlightModeManager::state_calibrating(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exit calibration\n"); + LOG_DEBUG(log, "Exit algos_calibration"); break; } + case EV_TC_CALIBRATE_ALGOS: + { + retState = transition(&FMMController::state_algosCalibration); + break; + } case EV_ADA_READY: { - retState = transition(&FlightModeManager::state_disarmed); + ada_ready = true; + if (nas_ready) + { + sEventBroker->post(Event{EV_CALIBRATION_OK}, + TOPIC_FLIGHT_EVENTS); + } + break; + } + case EV_NAS_READY: + { + nas_ready = true; + if (ada_ready) + { + sEventBroker->post(Event{EV_CALIBRATION_OK}, + TOPIC_FLIGHT_EVENTS); + } + break; + } + case EV_CALIBRATION_OK: + { + retState = transition(&FMMController::state_disarmed); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_onGround); + retState = tran_super(&FMMController::state_onGround); break; } } return retState; } -State FlightModeManager::state_disarmed(const Event& ev) +State FMMController::state_disarmed(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -278,7 +346,8 @@ State FlightModeManager::state_disarmed(const Event& ev) { sEventBroker->post({EV_DISARMED}, TOPIC_FLIGHT_EVENTS); logState(FMMState::DISARMED); - TRACE("[FMM] Entering disarmed\n"); + LOG_DEBUG(log, "Entering disarmed"); + break; } case EV_INIT: /* No sub-state */ @@ -287,30 +356,38 @@ State FlightModeManager::state_disarmed(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exiting disarmed\n"); + LOG_DEBUG(log, "Exiting disarmed"); + + break; + } + case EV_TC_CALIBRATE_ALGOS: + { + ada_ready = false; + nas_ready = false; + retState = transition(&FMMController::state_algosCalibration); break; } - case EV_TC_CALIBRATE_ADA: + case EV_TC_CALIBRATE_SENSORS: { - retState = transition(&FlightModeManager::state_calibrating); + retState = transition(&FMMController::state_sensorsCalibration); break; } case EV_TC_ARM: { - retState = transition(&FlightModeManager::state_armed); + retState = transition(&FMMController::state_armed); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_onGround); + retState = tran_super(&FMMController::state_onGround); break; } } return retState; } -State FlightModeManager::state_armed(const Event& ev) +State FMMController::state_armed(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -320,7 +397,7 @@ State FlightModeManager::state_armed(const Event& ev) sEventBroker->post({EV_ARMED}, TOPIC_FLIGHT_EVENTS); logState(FMMState::ARMED); - TRACE("[FMM] Entering armed\n"); + LOG_DEBUG(log, "Entering armed"); break; } case EV_INIT: /* No sub-state */ @@ -329,32 +406,31 @@ State FlightModeManager::state_armed(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exiting armed\n"); + LOG_DEBUG(log, "Exiting armed"); break; } case EV_TC_DISARM: { - retState = transition(&FlightModeManager::state_disarmed); + retState = transition(&FMMController::state_disarmed); break; } case EV_UMBILICAL_DETACHED: case EV_TC_LAUNCH: { - retState = transition(&FlightModeManager::state_ascending); + retState = transition(&FMMController::state_flying); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::Hsm_top); + retState = tran_super(&FMMController::Hsm_top); break; } } return retState; } -// ACTUATORS test -State FlightModeManager::state_testing(const Event& ev) +State FMMController::state_testMode(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -363,9 +439,8 @@ State FlightModeManager::state_testing(const Event& ev) { logState(FMMState::TESTING); - sEventBroker->post({EV_TEST_MODE}, TOPIC_FLIGHT_EVENTS); + LOG_DEBUG(log, "Entering testing"); - TRACE("[FMM] Entering testing\n"); break; } case EV_INIT: /* No sub-state */ @@ -374,65 +449,65 @@ State FlightModeManager::state_testing(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exiting testing\n"); + LOG_DEBUG(log, "Exiting testing"); break; } case EV_TC_NC_OPEN: { - sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL); break; } - case EV_TC_CUT_DROGUE: + case EV_TC_DPL_WIGGLE_SERVO: { - sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_DPL); break; } - case EV_TC_CUT_PRIMARY: + case EV_TC_DPL_RESET_SERVO: { - sEventBroker->post(Event{EV_CUT_PRIMARY}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_DPL); break; } - case EV_TC_CUT_BACKUP: + case EV_TC_CUT_DROGUE: { - sEventBroker->post(Event{EV_CUT_BACKUP}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL); break; } - case EV_TC_TEST_CUTTER_PRIMARY: + case EV_TC_ABK_WIGGLE_SERVO: { - sEventBroker->post(Event{EV_TEST_CUTTER_PRIMARY}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_ABK); break; } - case EV_TC_TEST_CUTTER_BACKUP: + case EV_TC_ABK_RESET_SERVO: { - sEventBroker->post(Event{EV_TEST_CUTTER_BACKUP}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_ABK); break; } - case EV_TC_RESET_SERVO: + case EV_TC_TEST_ABK: { - sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_TEST_ABK}, TOPIC_ABK); break; } - case EV_TC_WIGGLE_SERVO: + case EV_TC_CLOSE_LOG: { - sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_DEPLOYMENT); + logger.stop(); break; } - case EV_TC_CLOSE_LOG: + case EV_TC_START_LOG: { - logger.stop(); + DeathStackBoard::DeathStack::getInstance()->startLogger(); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_onGround); + retState = tran_super(&FMMController::state_onGround); break; } } return retState; } -State FlightModeManager::state_flying(const Event& ev) +State FMMController::state_flying(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -444,22 +519,22 @@ State FlightModeManager::state_flying(const Event& ev) sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); // Start timeout for closing file descriptors end_mission_d_event_id = - sEventBroker->postDelayed<TIMEOUT_FMM_END_MISSION>( + sEventBroker->postDelayed<TIMEOUT_END_MISSION>( {EV_TIMEOUT_END_MISSION}, TOPIC_FMM); - TRACE("[FMM] Entering flying\n"); + LOG_DEBUG(log, "Entering flying"); break; } case EV_INIT: { - TRACE("[FMM] Init flying\n"); + LOG_DEBUG(log, "Init flying"); - retState = transition(&FlightModeManager::state_ascending); + retState = transition(&FMMController::state_ascending); break; } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exiting flying\n"); + LOG_DEBUG(log, "Exiting flying"); sEventBroker->removeDelayed(end_mission_d_event_id); break; @@ -468,25 +543,32 @@ State FlightModeManager::state_flying(const Event& ev) { // Open nosecone command sent by GS in case of problems // during flight - sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL); + break; + } + case EV_TC_ABK_DISABLE: + { + // Disable airbrakes command sent by GS in case of problems + // during flight + sEventBroker->post(Event{EV_DISABLE_ABK}, TOPIC_ABK); break; } case EV_TC_END_MISSION: case EV_TIMEOUT_END_MISSION: { - retState = transition(&FlightModeManager::state_landed); + retState = transition(&FMMController::state_landed); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::Hsm_top); + retState = tran_super(&FMMController::Hsm_top); break; } } return retState; } -State FlightModeManager::state_ascending(const Event& ev) +State FMMController::state_ascending(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -494,7 +576,7 @@ State FlightModeManager::state_ascending(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::ASCENDING); - TRACE("[FMM] Entering ascending\n"); + LOG_DEBUG(log, "Entering ascending"); break; } case EV_INIT: /* No sub-state */ @@ -503,7 +585,7 @@ State FlightModeManager::state_ascending(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exit ascending\n"); + LOG_DEBUG(log, "Exit ascending"); break; } @@ -513,19 +595,27 @@ State FlightModeManager::state_ascending(const Event& ev) // Notify of apogee all components sEventBroker->post(Event{EV_APOGEE}, TOPIC_FLIGHT_EVENTS); - retState = transition(&FlightModeManager::state_drogueDescent); + retState = transition(&FMMController::state_drogueDescent); + break; + } + case EV_ADA_DISABLE_ABK: + { + // Send disable airbrakes + sEventBroker->post(Event{EV_DISABLE_ABK}, TOPIC_ABK); + + retState = transition(&FMMController::state_ascending); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_flying); + retState = tran_super(&FMMController::state_flying); break; } } return retState; } -State FlightModeManager::state_drogueDescent(const Event& ev) +State FMMController::state_drogueDescent(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -533,10 +623,10 @@ State FlightModeManager::state_drogueDescent(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { // Open nosecone - sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL); logState(FMMState::DROGUE_DESCENT); - TRACE("[FMM] Entering drogueDescent\n"); + LOG_DEBUG(log, "Entering drogueDescent"); break; } case EV_INIT: /* No sub-state */ @@ -545,26 +635,26 @@ State FlightModeManager::state_drogueDescent(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - TRACE("[FMM] Exiting drogueDescent\n"); + LOG_DEBUG(log, "Exiting drogueDescent"); break; } case EV_ADA_DPL_ALT_DETECTED: case EV_TC_CUT_DROGUE: { - retState = transition(&FlightModeManager::state_terminalDescent); + retState = transition(&FMMController::state_terminalDescent); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_flying); + retState = tran_super(&FMMController::state_flying); break; } } return retState; } -State FlightModeManager::state_terminalDescent(const Event& ev) +State FMMController::state_terminalDescent(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -572,12 +662,12 @@ State FlightModeManager::state_terminalDescent(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { sEventBroker->post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS); - - sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DEPLOYMENT); + + sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL); logState(FMMState::TERMINAL_DESCENT); - TRACE("[FMM] Entering terminalDescent\n"); + LOG_DEBUG(log, "Entering terminalDescent"); break; } case EV_INIT: @@ -590,19 +680,19 @@ State FlightModeManager::state_terminalDescent(const Event& ev) } case EV_TC_CUT_DROGUE: // if you want to repeat cutting { - sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DEPLOYMENT); + sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::state_flying); + retState = tran_super(&FMMController::state_flying); break; } } return retState; } -State FlightModeManager::state_landed(const Event& ev) +State FMMController::state_landed(const Event& ev) { State retState = HANDLED; switch (ev.sig) @@ -615,7 +705,7 @@ State FlightModeManager::state_landed(const Event& ev) sEventBroker->post(Event{EV_LANDED}, TOPIC_FLIGHT_EVENTS); logger.stop(); - TRACE("[FMM] Entering landed\n"); + LOG_DEBUG(log, "Entering landed"); break; } case EV_INIT: @@ -628,7 +718,7 @@ State FlightModeManager::state_landed(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FlightModeManager::Hsm_top); + retState = tran_super(&FMMController::Hsm_top); break; } } diff --git a/src/boards/DeathStack/FlightModeManager/FMMController.h b/src/boards/DeathStack/FlightModeManager/FMMController.h new file mode 100644 index 0000000000000000000000000000000000000000..bddf2e7157360e134606c9edfe487ac72c4880f6 --- /dev/null +++ b/src/boards/DeathStack/FlightModeManager/FMMController.h @@ -0,0 +1,138 @@ +/* Copyright (c) 2015-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <DeathStack.h> +#include <FlightModeManager/FMMStatus.h> +#include <LoggerService/LoggerService.h> +#include <Singleton.h> +#include <events/Events.h> +#include <events/HSM.h> +#include <miosix.h> + +using miosix::FastMutex; +using miosix::Lock; + +namespace DeathStackBoard +{ + +/** + * @brief Implementation of the Flight Mode Manager Finite State Machine. + */ +class FMMController : public HSM<FMMController> +{ +public: + FMMController(); + ~FMMController(); + + State state_initialization(const Event& ev); + + /** + * @brief Handle TC_BOARD_RESET and TC_FORCE_LIFTOFF (super-state). + */ + State state_onGround(const Event& ev); + + /** + * @brief First state, wait for sensors and objects initialization. + */ + State state_init(const Event& ev); + + /** + * @brief "Low power" state, log only if requested by TC. + */ + State state_initDone(const Event& ev); + + /** + * @brief Init error, get stuck. + */ + State state_initError(const Event& ev); + + /** + * @brief Test mode, listen to serial and print stuff on serial. + */ + State state_testMode(const Event& ev); + + /** + * @brief Calibrating sensors. + */ + State state_sensorsCalibration(const Event& ev); + + /** + * @brief Calibrating ADA and NAS. + */ + State state_algosCalibration(const Event& ev); + + /** + * @brief All good, waiting for arm. + */ + State state_disarmed(const Event& ev); + + /** + * @brief Ready to launch, listening detachment pin (or command). + */ + State state_armed(const Event& ev); + + /** + * @brief Handle TC_OPEN and END_MISSION (super-state). + */ + State state_flying(const Event& ev); + + /** + * @brief Liftoff. + */ + State state_ascending(const Event& ev); + + /** + * @brief Apogee reached, deploy drogue and wait half altitude (or manual + * mode). + */ + State state_drogueDescent(const Event& ev); + + /** + * @brief Descent super-state. + */ + State state_terminalDescent(const Event& ev); + + /** + * @brief Close file descriptors. + */ + State state_landed(const Event& ev); + + FMMStatus getStatus() { return status; } + +private: + void logState(FMMState current_state); + + LoggerService& logger; + + FMMStatus status; + + uint16_t end_mission_d_event_id = 0; + + bool ada_ready = false; + bool nas_ready = false; + + PrintLogger log = Logging::getLogger("deathstack.fsm.fmm"); +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/FlightModeManager/FMMStatus.h b/src/boards/DeathStack/FlightModeManager/FMMStatus.h index 4b25824aa7954659c24364dde44413fe77d02893..5299dc4fcb4346a92ebc13ad51d124e60987e946 100644 --- a/src/boards/DeathStack/FlightModeManager/FMMStatus.h +++ b/src/boards/DeathStack/FlightModeManager/FMMStatus.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -20,19 +19,23 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#ifndef SRC_SHARED_BOARDS_HOMEONE_FLIGHTMODEMANAGER_FMMSTATUS_H -#define SRC_SHARED_BOARDS_HOMEONE_FLIGHTMODEMANAGER_FMMSTATUS_H + +#pragma once #include <cstdint> #include <ostream> +namespace DeathStackBoard +{ + enum class FMMState : uint8_t { ON_GROUND, INIT, INIT_DONE, INIT_ERROR, - CALIBRATING, + SENSORS_CALIBRATION, + ALGOS_CALIBRATION, DISARMED, ARMED, TESTING, @@ -40,14 +43,12 @@ enum class FMMState : uint8_t ASCENDING, DROGUE_DESCENT, TERMINAL_DESCENT, - ROGALLO_DESCENT, - MANUAL_DESCENT, LANDED }; struct FMMStatus { - long long timestamp; + uint64_t timestamp; FMMState state = FMMState::ON_GROUND; static std::string header() { return "timestamp,state\n"; } @@ -58,4 +59,4 @@ struct FMMStatus } }; -#endif /* SRC_SHARED_BOARDS_HOMEONE_FLIGHTMODEMANAGER_FMMSTATUS_H */ +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/FlightModeManager/FlightModeManager.h b/src/boards/DeathStack/FlightModeManager/FlightModeManager.h deleted file mode 100644 index 9c6828e67de8983d93709259e27ac945b68c6530..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/FlightModeManager/FlightModeManager.h +++ /dev/null @@ -1,107 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#ifndef SRC_SHARED_BOARDS_HOMEONE_FLIGHTMODEMANAGER_FSM_H -#define SRC_SHARED_BOARDS_HOMEONE_FLIGHTMODEMANAGER_FSM_H - -#include "Singleton.h" - -#include "FMMStatus.h" -#include "events/Event.h" -#include "events/HSM.h" -#include "DeathStack/LoggerService/LoggerService.h" - -#include <miosix.h> - -using miosix::FastMutex; -using miosix::Lock; - -namespace DeathStackBoard -{ - -/** - * Implementation of the Flight Mode Manager Finite State Machine - */ -class FlightModeManager : public HSM<FlightModeManager> -{ -public: - FlightModeManager(); - ~FlightModeManager(); - - State state_initialization(const Event& ev); - - /// ON-GROUND - - /* Handle TC_BOARD_RESET and TC_FORCE_LIFTOFF (super-state) */ - State state_onGround(const Event& ev); - - /* First state, wait for sensors and objects initialization */ - State state_init(const Event& ev); - /* "Low power" state, log only if requested by TC */ - State state_initDone(const Event& ev); - /* Init error, get stuck */ - State state_initError(const Event& ev); - - /* Test mode, listen to serial and print stuff on serial */ - State state_testing(const Event& ev); - - /* Calibrating ADA with pressure samples */ - State state_calibrating(const Event& ev); - /* All good, waiting for arm */ - State state_disarmed(const Event& ev); - - // ARMED - /* Ready to launch, listening detachment pin (or command) */ - State state_armed(const Event& ev); - - /// FLYING - - /* Handle TC_OPEN and END_MISSION (super-state) */ - State state_flying(const Event& ev); // super-state - /* Liftoff */ - State state_ascending(const Event& ev); - /* Apogee reached, deploy drogue and wait half altitude (or manual mode) */ - State state_drogueDescent(const Event& ev); - - /* Descent super-state */ - State state_terminalDescent(const Event& ev); - - /// LANDED - /* Close file descriptors */ - State state_landed(const Event& ev); - - FMMStatus getStatus() - { - return status; - } -private: - void logState(FMMState current_state); - - LoggerService& logger; - - FMMStatus status; - - uint16_t end_mission_d_event_id = 0; -}; - -} // namespace DeathStackBoard - -#endif /* SRC_SHARED_BOARDS_HOMEONE_FLIGHTMODEMANAGER_FSM_H */ diff --git a/src/boards/DeathStack/FlightModeManager/README.md b/src/boards/DeathStack/FlightModeManager/README.md deleted file mode 100644 index 54a4469ab4a0fb013af56402822d3242d1578ff7..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/FlightModeManager/README.md +++ /dev/null @@ -1,58 +0,0 @@ - ```plantuml - -skinparam titleFontSize 34 - -title Flight Mode Manager State Machine -top to bottom direction -state Init -state Testing -state Error -state Disarmed -state Armed -state Launching -state Ascending -state FirstDescentPhase -state SecondDescentPhase -state ManualDescent -state Landed - -[*] --> Init -Init -u-> Error : EV_INIT_ERROR -Init --> Disarmed : EV_INIT_OK - -Testing --> Testing : EV_TC_BOARD_RESET - -Error --> Error : EV_TC_BOARD_RESET - -Disarmed -u-> Error : EV_NC_OFFLINE -Disarmed -u-> Error : EV_IGN_OFFLINE -Disarmed -u-> Error : EV_NCEV_IGN_ABORTED_OFFLINE -Disarmed --> Armed : EV_TC_ARM -Disarmed --> Testing : EV_TC_TEST_MODE - - -Armed -u-> Error : EV_NC_OFFLINE -Armed -u-> Error : EV_IGN_OFFLINE -Armed -u-> Error : EV_NC_OFFLINE - -Armed --> Disarmed : EV_TC_DISARM -Armed --> Disarmed : EV_TIMEOUT_ARM -Armed --> Launching : EV_TC_LAUNCH - -Launching --> Ascending : EV_UMBILICAL_DETACHED -Launching --> Disarmed : EV_TC_DISARM -Launching -u-> Error : EV_IGN_ABORTED - -Ascending --> FirstDescentPhase : EV_ADA_APOGEE_DETECTED -Ascending --> FirstDescentPhase : EV_TIMEOUT_APOGEE - -FirstDescentPhase --> SecondDescentPhase : EV_ADA_DPL_ALT_DETECTED -FirstDescentPhase --> SecondDescentPhase : EV_TIMEOUT_DPL_ALT -FirstDescentPhase --> ManualDescent : EV_TC_MANUAL_MODE - -SecondDescentPhase --> Landed : EV_TC_END_MISSION -SecondDescentPhase --> Landed : EV_TIMEOUT_END_MISSION - -ManualDescent --> Landed : EV_TC_END_MISSION -ManualDescent --> Landed : EV_TIMEOUT_END_MISSION - ``` \ No newline at end of file diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSR.scxml b/src/boards/DeathStack/FlightStatsRecorder/FSR.scxml new file mode 100644 index 0000000000000000000000000000000000000000..2d12318f690d94cada8dd77e50489f2c921ca78f --- /dev/null +++ b/src/boards/DeathStack/FlightStatsRecorder/FSR.scxml @@ -0,0 +1,30 @@ +<?xml version="1.0" encoding="UTF-8"?> +<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="idle" version="1.0"> + <state id="idle"> + <onentry>processDeferred()</onentry> + <transition event="FLIGHT_EVENTS.EV_DPL_ALTITUDE" target="mainDLP"/> + <transition event="FLIGHT_EVENTS.EV_LIFTOFF" target="liftoff"/> + </state> + <state id="mainDLP"> + <onentry>postD(STATS.EV_STATS_TIMEOUT)</onentry> + <onexit>logMainDplStats()</onexit> + <transition event="STATS.EV_STATS_TIMEOUT" target="idle"/> + </state> + <state id="liftoff"> + <onentry>postD(STATS.EV_STATS_TIMEOUT)</onentry> + <onexit>logLiftoffStats()</onexit> + <transition event="STATS.EV_STATS_TIMEOUT" target="ascent"/> + </state> + <state id="ascent"> + <onexit>logApogeeStats()</onexit> + <transition event="FLIGHT_EVENTS.EV_APOGEE" target="drogueDPL"/> + </state> + <state id="drogueDPL"> + <onentry>postD(STATS.EV_STATS_TIMEOUT)</onentry> + <onexit>logDrougeDplStats()</onexit> + <transition event="FLIGHT_EVENTS.EV_DPL_ALTITUDE" target="drogueDPL"> + defer() + </transition> + <transition event="STATS.EV_STATS_TIMEOUT" target="idle"/> + </state> +</scxml> diff --git a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp similarity index 54% rename from src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp rename to src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp index 5ff3a97a318331fbfe6225de529dd5773daa4a6e..5fbad791b71fe16c3fff018e026c08dd81e4e505 100644 --- a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,20 +13,21 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include "FlightStatsRecorder.h" -#include <cmath> +#include <FlightStatsRecorder/FSRController.h> +#include <LoggerService/LoggerService.h> +#include <System/StackLogger.h> +#include <configs/SensorsConfig.h> +#include <events/EventBroker.h> +#include <events/Events.h> -#include "DeathStack/System/StackLogger.h" -#include "DeathStack/events/Events.h" -#include "LoggerService.h" -#include "events/EventBroker.h" +#include <cmath> namespace DeathStackBoard { @@ -36,26 +36,26 @@ FlightStatsRecorder::FlightStatsRecorder() : FSM(&FlightStatsRecorder::state_idle) { sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_DEPLOYMENT); + sEventBroker->subscribe(this, TOPIC_DPL); sEventBroker->subscribe(this, TOPIC_STATS); } FlightStatsRecorder::~FlightStatsRecorder() { sEventBroker->unsubscribe(this); } -void FlightStatsRecorder::update(const KalmanState& t) +void FlightStatsRecorder::update(const ADAKalmanState& t) { switch (state) { - case State::LIFTOFF: + case FSRState::LIFTOFF: { - apogee_stats.kalman_min_pressure = t.x0; + apogee_stats.ada_min_pressure = t.x0; break; } - case State::ASCENDING: + case FSRState::ASCENDING: { - if (t.x0 < apogee_stats.kalman_min_pressure) + if (t.x0 < apogee_stats.ada_min_pressure) { - apogee_stats.kalman_min_pressure = t.x0; + apogee_stats.ada_min_pressure = t.x0; } break; } @@ -64,38 +64,45 @@ void FlightStatsRecorder::update(const KalmanState& t) } } -void FlightStatsRecorder::update(const CurrentSenseData& t) -{ - switch (state) - { - case State::TESTING_CUTTER: - { - ++cutter_stats.n_samples; - cutter_stats.cutter_1_avg += t.current_1; - cutter_stats.cutter_2_avg += t.current_2; - break; - } - default: - break; - } -} +// void FlightStatsRecorder::update(const CurrentSensorData& t) +// { +// switch (state) +// { +// case FSRState::TESTING_CUTTER: +// { +// if (t.channel_id == SensorConfigs::ADC_CS_CUTTER_PRIMARY) +// { +// ++cutters_stats.n_samples_1; +// cutters_stats.cutter_1_avg += t.current; +// } +// else if (t.channel_id == SensorConfigs::ADC_CS_CUTTER_BACKUP) +// { +// ++cutters_stats.n_samples_2; +// cutters_stats.cutter_2_avg += t.current; +// } +// break; +// } +// default: +// break; +// } +// } void FlightStatsRecorder::update(const ADAData& t) { switch (state) { - case State::LIFTOFF: + case FSRState::LIFTOFF: { - if (t.acc_vert_speed > liftoff_stats.vert_speed_max) + if (t.vert_speed > liftoff_stats.vert_speed_max) { - liftoff_stats.vert_speed_max = t.acc_vert_speed; + liftoff_stats.vert_speed_max = t.vert_speed; liftoff_stats.T_max_speed = static_cast<uint32_t>(miosix::getTick()); liftoff_stats.altitude_max_speed = t.msl_altitude; } break; } - case State::ASCENDING: + case FSRState::ASCENDING: { if (t.msl_altitude > apogee_stats.baro_max_altitude) { @@ -103,7 +110,7 @@ void FlightStatsRecorder::update(const ADAData& t) } break; } - case State::MAIN_DPL: + case FSRState::MAIN_DPL: { // Only set it one time if (main_dpl_stats.altitude_dpl == 0) @@ -118,19 +125,15 @@ void FlightStatsRecorder::update(const ADAData& t) } } -void FlightStatsRecorder::update(const AD7994WrapperData& t) +void FlightStatsRecorder::update(const AirSpeedPitot& t) { switch (state) { - case State::ASCENDING: + case FSRState::LIFTOFF: { - if (t.nxp_baro_pressure < apogee_stats.nxp_min_pressure) + if (fabs(t.airspeed) > liftoff_stats.airspeed_pitot_max) { - apogee_stats.nxp_min_pressure = t.nxp_baro_pressure; - } - if (t.honeywell_baro_pressure < apogee_stats.hw_min_pressure) - { - apogee_stats.hw_min_pressure = t.honeywell_baro_pressure; + liftoff_stats.airspeed_pitot_max = fabs(t.airspeed); } break; } @@ -138,35 +141,33 @@ void FlightStatsRecorder::update(const AD7994WrapperData& t) break; } } -void FlightStatsRecorder::update(const MPU9250Data& t) + +void FlightStatsRecorder::update(const MS5803Data& t) { switch (state) { - case State::LIFTOFF: + case FSRState::ASCENDING: { - if (fabs(t.accel.getZ()) > liftoff_stats.acc_max) + if (t.press < apogee_stats.digital_min_pressure) { - liftoff_stats.T_max_acc = - static_cast<uint32_t>(miosix::getTick()); - liftoff_stats.acc_max = fabs(t.accel.getZ()); + apogee_stats.digital_min_pressure = t.press; } break; } - case State::DROGUE_DPL: - { - if (fabs(t.accel.getZ()) > fabs(drogue_dpl_stats.max_dpl_acc)) - { - drogue_dpl_stats.max_dpl_acc = t.accel.getZ(); - drogue_dpl_stats.T_dpl = - static_cast<uint32_t>(miosix::getTick()); - } + default: break; - } - case State::MAIN_DPL: + } +} + +void FlightStatsRecorder::update(const MPXHZ6130AData& t) +{ + switch (state) + { + case FSRState::ASCENDING: { - if (fabs(t.accel.getZ()) > fabs(main_dpl_stats.max_dpl_acc)) + if (t.press < apogee_stats.static_min_pressure) { - main_dpl_stats.max_dpl_acc = t.accel.getZ(); + apogee_stats.static_min_pressure = t.press; } break; } @@ -175,19 +176,15 @@ void FlightStatsRecorder::update(const MPU9250Data& t) } } -void FlightStatsRecorder::update(const PiksiData& t) +void FlightStatsRecorder::update(const SSCDANN030PAAData& t) { switch (state) { - case State::ASCENDING: + case FSRState::ASCENDING: { - if (t.gps_data.height > apogee_stats.gps_max_altitude) + if (t.press > drogue_dpl_stats.max_dpl_vane_pressure) { - apogee_stats.gps_max_altitude = t.gps_data.height; - apogee_stats.lat_apogee = - static_cast<float>(t.gps_data.latitude); - apogee_stats.lon_apogee = - static_cast<float>(t.gps_data.longitude); + drogue_dpl_stats.max_dpl_vane_pressure = t.press; } break; } @@ -196,82 +193,134 @@ void FlightStatsRecorder::update(const PiksiData& t) } } -void FlightStatsRecorder::state_idle(const Event& ev) +void FlightStatsRecorder::update(const BMX160WithCorrectionData& t) { - switch (ev.sig) + // take acceleration on x axis since it is in body frame + switch (state) { - case EV_ENTRY: + case FSRState::LIFTOFF: { - TRACE("[FlightStats] Entering IDLE state\n"); - state = State::IDLE; - - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + if (fabs(t.accel_x) > liftoff_stats.acc_max) + { + liftoff_stats.T_max_acc = + static_cast<uint32_t>(miosix::getTick()); + liftoff_stats.acc_max = fabs(t.accel_x); + } break; } - case EV_EXIT: + case FSRState::DROGUE_DPL: { - TRACE("[FlightStats] Exiting IDLE state\n"); - + if (fabs(t.accel_x) > fabs(drogue_dpl_stats.max_dpl_acc)) + { + drogue_dpl_stats.max_dpl_acc = t.accel_x; + drogue_dpl_stats.T_dpl = + static_cast<uint32_t>(miosix::getTick()); + } break; } - case EV_LIFTOFF: + case FSRState::MAIN_DPL: { - transition(&FlightStatsRecorder::state_liftOff); + if (fabs(t.accel_x) > fabs(main_dpl_stats.max_dpl_acc)) + { + main_dpl_stats.max_dpl_acc = t.accel_x; + } break; } - case EV_TEST_CUTTER_BACKUP: - case EV_TEST_CUTTER_PRIMARY: - { - transition(&FlightStatsRecorder::state_testing_cutters); + default: break; - } - case EV_DPL_ALTITUDE: + } +} + +void FlightStatsRecorder::update(const UbloxGPSData& t) +{ + switch (state) + { + case FSRState::ASCENDING: { - transition(&FlightStatsRecorder::state_mainDeployment); + if (fabs(t.height) > apogee_stats.gps_max_altitude) + { + apogee_stats.gps_max_altitude = t.height; + apogee_stats.lat_apogee = static_cast<float>(t.latitude); + apogee_stats.lon_apogee = static_cast<float>(t.longitude); + } break; } default: - { break; - } } } -void FlightStatsRecorder::state_testing_cutters(const Event& ev) +#ifdef HARDWARE_IN_THE_LOOP +void FlightStatsRecorder::update(const HILImuData& t) +{ + BMX160Data d; + d.accel_timestamp = t.accel_timestamp; + d.accel_x = t.accel_x; + d.accel_y = t.accel_y; + d.accel_z = t.accel_z; + d.gyro_timestamp = t.gyro_timestamp; + d.gyro_x = t.gyro_x; + d.gyro_y = t.gyro_y; + d.gyro_z = t.gyro_z; + d.mag_timestamp = t.mag_timestamp; + d.mag_x = t.mag_x; + d.mag_y = t.mag_y; + d.mag_z = t.mag_z; + this->update(d); +} + +void FlightStatsRecorder::update(const HILBaroData& t) +{ + MS5803Data d; + d.press_timestamp = t.press_timestamp; + d.press = t.press; + this->update(d); +} + +void FlightStatsRecorder::update(const HILGpsData& t) +{ + UbloxGPSData d; + d.latitude = t.latitude; + d.longitude = t.longitude; + d.height = t.height; + d.velocity_north = t.velocity_north; + d.velocity_east = t.velocity_east; + d.velocity_down = t.velocity_down; + d.speed = t.speed; + d.fix = (uint8_t)t.fix; + d.track = t.track; + d.num_satellites = t.num_satellites; + this->update(d); +} +#endif + +void FlightStatsRecorder::state_idle(const Event& ev) { switch (ev.sig) { case EV_ENTRY: { - cutter_stats = CutterTestStats{}; - - state = State::TESTING_CUTTER; - - ev_timeout_id = - sEventBroker - ->postDelayed<FlightStatsConfig::TIMEOUT_CUTTER_TEST_STATS>( - {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS); + LOG_DEBUG(log, "Entering IDLE state"); + + state = FSRState::IDLE; StackLogger::getInstance()->updateStack(THID_STATS_FSM); - TRACE("[FlightStats] Entering CUTTER_TEST state\n"); break; } case EV_EXIT: { - cutter_stats.cutter_1_avg = - cutter_stats.cutter_1_avg / cutter_stats.n_samples; - cutter_stats.cutter_2_avg = - cutter_stats.cutter_2_avg / cutter_stats.n_samples; - - LoggerService::getInstance()->log(cutter_stats); - sEventBroker->removeDelayed(ev_timeout_id); + LOG_DEBUG(log, "Exiting IDLE state"); - TRACE("[FlightStats] Exiting CUTTER_TEST state\n"); break; } - case EV_FLIGHTSTATS_TIMEOUT: + case EV_LIFTOFF: { - transition(&FlightStatsRecorder::state_idle); + transition(&FlightStatsRecorder::state_liftOff); + break; + } + case EV_DPL_ALTITUDE: + { + transition(&FlightStatsRecorder::state_mainDeployment); break; } default: @@ -287,14 +336,15 @@ void FlightStatsRecorder::state_liftOff(const Event& ev) { case EV_ENTRY: { - TRACE("[FlightStats] Entering LIFTOFF state\n"); - state = State::LIFTOFF; + LOG_DEBUG(log, "Entering LIFTOFF state"); + + state = FSRState::LIFTOFF; // Collect liftoff stats until this event is received ev_timeout_id = sEventBroker ->postDelayed<FlightStatsConfig::TIMEOUT_LIFTOFF_STATS>( - {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS); + {EV_STATS_TIMEOUT}, TOPIC_STATS); // Save liftoff time liftoff_stats.T_liftoff = static_cast<uint32_t>(miosix::getTick()); @@ -304,14 +354,14 @@ void FlightStatsRecorder::state_liftOff(const Event& ev) } case EV_EXIT: { - TRACE("[FlightStats] Exiting LIFTOFF state\n"); + LOG_DEBUG(log, "Exiting LIFTOFF state"); LoggerService::getInstance()->log(liftoff_stats); sEventBroker->removeDelayed(ev_timeout_id); break; } - case EV_FLIGHTSTATS_TIMEOUT: + case EV_STATS_TIMEOUT: { transition(&FlightStatsRecorder::state_ascending); break; @@ -322,21 +372,23 @@ void FlightStatsRecorder::state_liftOff(const Event& ev) } } } + void FlightStatsRecorder::state_ascending(const Event& ev) { switch (ev.sig) { case EV_ENTRY: { - TRACE("[FlightStats] Entering ASCENDING state\n"); + LOG_DEBUG(log, "Entering ASCENDING state"); + + state = FSRState::ASCENDING; - state = State::ASCENDING; StackLogger::getInstance()->updateStack(THID_STATS_FSM); break; } case EV_EXIT: { - TRACE("[FlightStats] Exiting ASCENDING state\n"); + LOG_DEBUG(log, "Exiting ASCENDING state"); LoggerService::getInstance()->log(apogee_stats); @@ -353,10 +405,10 @@ void FlightStatsRecorder::state_ascending(const Event& ev) ev_timeout_id = sEventBroker ->postDelayed<FlightStatsConfig::TIMEOUT_APOGEE_STATS>( - {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS); + {EV_STATS_TIMEOUT}, TOPIC_STATS); break; } - case EV_FLIGHTSTATS_TIMEOUT: + case EV_STATS_TIMEOUT: { // Drogue deployment occurs just after apogee transition(&FlightStatsRecorder::state_drogueDeployment); @@ -375,29 +427,29 @@ void FlightStatsRecorder::state_drogueDeployment(const Event& ev) { case EV_ENTRY: { - TRACE("[FlightStats] Entering DROGUE_DPL state\n"); + LOG_DEBUG(log, "Entering DROGUE_DPL state"); - state = State::DROGUE_DPL; + state = FSRState::DROGUE_DPL; // Collect stats until this event is received ev_timeout_id = sEventBroker ->postDelayed<FlightStatsConfig::TIMEOUT_DROGUE_DPL_STATS>( - {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS); + {EV_STATS_TIMEOUT}, TOPIC_STATS); StackLogger::getInstance()->updateStack(THID_STATS_FSM); break; } case EV_EXIT: { - TRACE("[FlightStats] Entering EXITING state\n"); + LOG_DEBUG(log, "Exiting DROGUE_DPL state"); LoggerService::getInstance()->log(drogue_dpl_stats); sEventBroker->removeDelayed(ev_timeout_id); break; } - case EV_FLIGHTSTATS_TIMEOUT: + case EV_STATS_TIMEOUT: { transition(&FlightStatsRecorder::state_idle); break; @@ -415,9 +467,9 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev) { case EV_ENTRY: { - TRACE("[FlightStats] Entering MAIN DPL state\n"); + LOG_DEBUG(log, "Entering MAIN_DPL state"); - state = State::MAIN_DPL; + state = FSRState::MAIN_DPL; // Save deployment timestamp main_dpl_stats.T_dpl = static_cast<uint32_t>(miosix::getTick()); @@ -426,21 +478,21 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev) ev_timeout_id = sEventBroker ->postDelayed<FlightStatsConfig::TIMEOUT_MAIN_DPL_STATS>( - {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS); + {EV_STATS_TIMEOUT}, TOPIC_STATS); StackLogger::getInstance()->updateStack(THID_STATS_FSM); break; } case EV_EXIT: { - TRACE("[FlightStats] Exiting MAIN DPL state\n"); + LOG_DEBUG(log, "Exiting MAIN_DPL state"); LoggerService::getInstance()->log(main_dpl_stats); sEventBroker->removeDelayed(ev_timeout_id); break; } - case EV_FLIGHTSTATS_TIMEOUT: + case EV_STATS_TIMEOUT: { transition(&FlightStatsRecorder::state_idle); break; @@ -451,4 +503,51 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev) } } } + +/* +void FlightStatsRecorder::state_testingCutters(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + cutters_stats = CutterTestStats{}; + + state = FSRState::TESTING_CUTTER; + + const int timeout = CutterConfig::CUT_TEST_DURATION; + + ev_timeout_id = sEventBroker->postDelayed<timeout>( + {EV_STATS_TIMEOUT}, TOPIC_STATS); + + StackLogger::getInstance()->updateStack(THID_STATS_FSM); + LOG_DEBUG(log, "Entering CUTTER_TEST state"); + break; + } + case EV_EXIT: + { + cutters_stats.cutter_1_avg = + cutters_stats.cutter_1_avg / cutters_stats.n_samples_1; + cutters_stats.cutter_2_avg = + cutters_stats.cutter_2_avg / cutters_stats.n_samples_2; + + LoggerService::getInstance()->log(cutters_stats); + sEventBroker->removeDelayed(ev_timeout_id); + + LOG_DEBUG(log, "Exiting CUTTER_TEST state"); + break; + } + case EV_STATS_TIMEOUT: + { + transition(&FlightStatsRecorder::state_idle); + break; + } + default: + { + break; + } + } +} +*/ + } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h new file mode 100644 index 0000000000000000000000000000000000000000..1a6e511ee8ef8bf7393518570453edf405e569cb --- /dev/null +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h @@ -0,0 +1,114 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <ApogeeDetectionAlgorithm/ADAData.h> +#include <FlightStatsRecorder/FSRData.h> +#include <Main/SensorsData.h> +#include <configs/FlightStatsConfig.h> +#include <diagnostic/PrintLogger.h> +#include <drivers/gps/ublox/UbloxGPSData.h> +#include <events/FSM.h> +#include <sensors/BMX160/BMX160WithCorrectionData.h> +#include <sensors/MS5803/MS5803Data.h> +#include <sensors/analog/current/CurrentSensorData.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAAData.h> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL_sensors/HILSensors.h> +#endif + +namespace DeathStackBoard +{ + +/** + * @brief Records statistics about the flight. + * + * Statistics includes maximum acceleration during liftoff, maximum altitude, + * maximum speed etc. In order to do so, we need to know in which stage of the + * flight we are in, and we do so using a state machine and receiving events + * from the topic FLIGHT_EVENTS. + */ +class FlightStatsRecorder : public FSM<FlightStatsRecorder> +{ +public: + FlightStatsRecorder(); + ~FlightStatsRecorder(); + + void update(const ADAKalmanState& t); + void update(const ADAData& t); + void update(const UbloxGPSData& t); + void update(const BMX160WithCorrectionData& t); + //void update(const CurrentSensorData& t); + void update(const MS5803Data& t); // digitl baro + void update(const MPXHZ6130AData& t); // static ports baro + void update(const SSCDANN030PAAData& t); // DPL vane baro + void update(const AirSpeedPitot& t); + +#ifdef HARDWARE_IN_THE_LOOP + void update(const HILImuData& t); + void update(const HILBaroData& t); + void update(const HILGpsData& t); +#endif + + /** + * @brief Wait for liftoff or deployment. + */ + void state_idle(const Event& ev); + + /** + * @brief Record stats of the first few seconds of flight. + */ + void state_liftOff(const Event& ev); + + /** + * @brief Record stats for the apogee part of the flight. + */ + void state_ascending(const Event& ev); + + /** + * @brief Record stats during drogue deployment. + */ + void state_drogueDeployment(const Event& ev); + + /** + * @brief Stats during main deployment. + */ + void state_mainDeployment(const Event& ev); + +private: + LiftOffStats liftoff_stats{}; + ApogeeStats apogee_stats{}; + DrogueDPLStats drogue_dpl_stats{}; + MainDPLStats main_dpl_stats{}; + //CutterTestStats cutters_stats{}; + FSRState state = FSRState::IDLE; + long long T_liftoff = 0; + + uint16_t ev_timeout_id = 0; + + PrintLogger log = Logging::getLogger("deathstack.fsm.flightstatsrecorder"); +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/LoggerService/FlightStatsData.h b/src/boards/DeathStack/FlightStatsRecorder/FSRData.h similarity index 60% rename from src/boards/DeathStack/LoggerService/FlightStatsData.h rename to src/boards/DeathStack/FlightStatsRecorder/FSRData.h index da51396fb03e3bce5ccf990cdf5424218d285c4a..8d5e5897b7099657354ebdfe8c21ff9f4bfe9cd9 100644 --- a/src/boards/DeathStack/LoggerService/FlightStatsData.h +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRData.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -28,37 +27,49 @@ namespace DeathStackBoard { + +enum class FSRState +{ + IDLE = 0, + LIFTOFF, + ASCENDING, + DROGUE_DPL, + MAIN_DPL +}; + struct LiftOffStats { - uint32_t T_liftoff = 0; + uint64_t T_liftoff = 0; - uint32_t T_max_acc = 0; + uint64_t T_max_acc = 0; float acc_max = 0.0f; - uint32_t T_max_speed = 0; + uint64_t T_max_speed = 0; float vert_speed_max = 0.0f; + float airspeed_pitot_max = 0.0f; float altitude_max_speed = 0.0f; static std::string header() { return "T_liftoff,T_max_acc,acc_max,T_max_speed,vert_speed_max," - "altitude_max_speed\n"; + "airspeed_pitot_max,altitude_max_speed\n"; } void print(std::ostream& os) const { os << T_liftoff << "," << T_max_acc << "," << acc_max << "," - << T_max_speed << "," << vert_speed_max << "," << altitude_max_speed - << "\n"; + << T_max_speed << "," << vert_speed_max << "," << airspeed_pitot_max + << "," << altitude_max_speed << "\n"; } }; -struct CutterTestStats +/*struct CutterTestStats { - uint32_t timestamp = 0; - float cutter_1_avg = 0; - float cutter_2_avg = 0; - uint32_t n_samples = 0; + uint64_t timestamp = 0; + float cutter_1_avg = 0; + float cutter_2_avg = 0; + uint32_t n_samples_1 = 0; + uint32_t n_samples_2 = 0; static std::string header() { return "timestamp,cutter_1_avg,cutter_2_avg\n"; @@ -68,14 +79,13 @@ struct CutterTestStats { os << timestamp << "," << cutter_1_avg << "," << cutter_2_avg << "\n"; } -}; +};*/ struct ApogeeStats { - uint32_t T_apogee = 0; - float nxp_min_pressure = 200000.0f; - float hw_min_pressure = 200000.0f; - float kalman_min_pressure = 200000.0f; + uint64_t T_apogee = 0; + float static_min_pressure = 200000.0f; + float ada_min_pressure = 200000.0f; float digital_min_pressure = 200000.0f; float baro_max_altitude = 0.0f; @@ -86,36 +96,41 @@ struct ApogeeStats static std::string header() { - return "T_apogee,nxp_min_pressure,hw_min_pressure,kalman_min_pressure," - "digital_min_pressure,baro_max_altitude,gps_max_altitude,lat_" - "apogee,lon_apogee\n"; + return "T_apogee,static_min_pressure,ada_min_pressure,digital_min_" + "pressure,baro_max_altitude,gps_max_altitude,lat_apogee,lon_" + "apogee\n"; } void print(std::ostream& os) const { - os << T_apogee << "," << nxp_min_pressure << "," << hw_min_pressure - << "," << kalman_min_pressure << "," << digital_min_pressure << "," - << baro_max_altitude << "," << gps_max_altitude << "," << lat_apogee - << "," << lon_apogee << "\n"; + os << T_apogee << "," << static_min_pressure << "," << ada_min_pressure + << "," << digital_min_pressure << "," << baro_max_altitude << "," + << gps_max_altitude << "," << lat_apogee << "," << lon_apogee + << "\n"; } }; struct DrogueDPLStats { - uint32_t T_dpl = 0; - float max_dpl_acc = 0.0f; + uint64_t T_dpl = 0; + float max_dpl_acc = 0.0f; + float max_dpl_vane_pressure = 0.0f; - static std::string header() { return "T_dpl,max_dpl_acc\n"; } + static std::string header() + { + return "T_dpl,max_dpl_acc,max_dpl_vane_pressure\n"; + } void print(std::ostream& os) const { - os << T_dpl << "," << max_dpl_acc << "\n"; + os << T_dpl << "," << max_dpl_acc << "," << max_dpl_vane_pressure + << "\n"; } }; struct MainDPLStats { - uint32_t T_dpl = 0; + uint64_t T_dpl = 0; float max_dpl_acc = 0.0f; float altitude_dpl = 0.0f; float vert_speed_dpl = 0.0f; diff --git a/src/boards/DeathStack/IgnitionController/IgnitionController.cpp b/src/boards/DeathStack/IgnitionController/IgnitionController.cpp deleted file mode 100644 index 144b5089e495db40045c0e02adbceff61bafcdfd..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/IgnitionController/IgnitionController.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "IgnitionController.h" -#include "DeathStack/configs/IgnitionConfig.h" -#include "boards/CanInterfaces.h" -#include "DeathStack/events/Events.h" - -namespace DeathStackBoard -{ - -using namespace CanInterfaces; - -IgnitionController::IgnitionController(CanProxy* canbus) - : FSM(&IgnitionController::stateIdle), canbus(canbus) -{ - // Receive self posted events - sEventBroker->subscribe(this, TOPIC_IGNITION); - // Receive global board events, e.g. liftoff - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - // Receive telecommands to be forwarded on the CAN - sEventBroker->subscribe(this, TOPIC_TC); - // Receive Can messages - sEventBroker->subscribe(this, TOPIC_CAN); - - memset(&status, 0, sizeof(IgnCtrlStatus)); - memset(&loggable_board_status, 0, sizeof(IgnBoardLoggableStatus)); -} - -/** - * Status - */ -void IgnitionController::logStatus() -{ - status.timestamp = miosix::getTick(); - logger.log(status); -} - - -bool IgnitionController::updateIgnBoardStatus(const Event& ev) -{ - const CanbusEvent& cev = static_cast<const CanbusEvent&>(ev); - - // Check event - if (cev.canTopic == CAN_TOPIC_IGNITION && - cev.len == sizeof(IgnitionBoardStatus)) - { - // Update internal board status struct - memcpy(&(loggable_board_status.board_status), cev.payload, - sizeof(IgnitionBoardStatus)); - - // Log internal board status struct - loggable_board_status.timestamp = miosix::getTick(); - logger.log(loggable_board_status); - - return true; - } - - // Event not directed to me - return false; -} - - -/** - * States - */ -void IgnitionController::stateIdle(const Event& ev) -{ - status.last_event = ev.sig; - - switch (ev.sig) - { - case EV_ENTRY: - status.fsm_state = IgnitionControllerState::IGN_IDLE; - logStatus(); - - // Schedule IGN_OFFLINE event: every time a message is received, the - // event is rescheduled - ign_offline_delayed_id = sEventBroker->postDelayed( - {EV_IGN_OFFLINE}, TOPIC_FLIGHT_EVENTS, TIMEOUT_IGN_OFFLINE); - - // Send first getstatus event, which will be periodically rescheduled - sEventBroker->post({EV_IGN_GETSTATUS}, TOPIC_IGNITION); - - TRACE("IGNCTRL: Entering stateIdle\n"); - break; - - case EV_EXIT: - // Remove GET_STATUS scheduled event - sEventBroker->removeDelayed(get_status_delayed_id); - TRACE("IGNCTRL: Exiting stateIdle\n"); - break; - - case EV_IGN_GETSTATUS: - { - status.n_sent_messages++; - - // Send status request on the CAN bus - uint8_t cmd = CAN_MSG_REQ_IGN_STATUS; - canbus->send( CAN_TOPIC_HOMEONE, &cmd, sizeof(uint8_t)); - - // Post next GETSTATUS event - get_status_delayed_id = sEventBroker->postDelayed( - {EV_IGN_GETSTATUS}, TOPIC_IGNITION, INTERVAL_IGN_GET_STATUS); - break; - } - - case EV_NEW_CAN_MSG: - { - // Check that the event is an ignition status: if so, update internal - // status - if (updateIgnBoardStatus(ev)) - { - status.n_rcv_messages++; - - // Reset the ignition offline timeout - sEventBroker->removeDelayed(ign_offline_delayed_id); - // Reschedule offline event - ign_offline_delayed_id = sEventBroker->postDelayed( - {EV_IGN_OFFLINE}, TOPIC_FLIGHT_EVENTS, - TIMEOUT_IGN_OFFLINE); - - static const uint16_t ABORT_BITMASK = 0x0707; - uint16_t status_bytes; - memcpy(&status_bytes, &loggable_board_status.board_status, sizeof(IgnitionBoardStatus)); - if (status_bytes & ABORT_BITMASK) - { - // We've had an abort. - status.abort_rcv = 1; - transition(&IgnitionController::stateAborted); - } - - // Log ignition controller status - logStatus(); - } - break; - } - - case EV_LIFTOFF: - transition(&IgnitionController::stateEnd); - break; - - case EV_GS_OFFLINE: - case EV_TC_ABORT_LAUNCH: - { - status.n_sent_messages++; - status.abort_sent = 1; - - // Send an ABORT message to the board - // NOTE: this does not cause the controller to go in abort state yet: this - // state wll be reached only when we have confirmation of the ABORT from the board - uint8_t cmd = CAN_MSG_ABORT; - canbus->send((uint16_t)CAN_TOPIC_HOMEONE, &cmd, sizeof(uint8_t)); - break; - } - - case EV_LAUNCH: - { - // Send an LAUNCH command to the board - // NOTE: this does not cause the controller to change state: the state will - // change only when we have confirmation from the board and the detachment pin - // signaled the LIFTOFF event. - const LaunchEvent& lev = static_cast<const LaunchEvent&>(ev); - status.n_sent_messages++; - status.launch_sent = 1; - - canbus->send((uint16_t)CAN_TOPIC_LAUNCH, (uint8_t*)&(lev.launchCode), sizeof(uint64_t)); - break; - } - - default: - TRACE("IGNCTRL stateIdle: Event %d not handled.\n", ev.sig); - break; - } -} - -void IgnitionController::stateAborted(const Event& ev) -{ - status.last_event = ev.sig; - - switch (ev.sig) - { - case EV_ENTRY: - status.fsm_state = IgnitionControllerState::IGN_ABORTED; - logStatus(); - - // Send first getstatus event, which will be periodically rescheduled - sEventBroker->post({EV_IGN_GETSTATUS}, TOPIC_IGNITION); - // Signal abort to rest of the board - sEventBroker->post({EV_IGN_ABORTED}, TOPIC_FLIGHT_EVENTS); - TRACE("IGNCTRL: Entering stateAborted\n"); - break; - - case EV_EXIT: - TRACE("IGNCTRL: Exiting stateAborted\n"); - break; - - case EV_IGN_GETSTATUS: - { - status.n_sent_messages++; - - // Send status request on the CAN bus - uint8_t cmd = CAN_MSG_REQ_IGN_STATUS; - canbus->send( CAN_TOPIC_HOMEONE, &cmd, sizeof(uint8_t)); - - // Post next GETSTATUS event - get_status_delayed_id = sEventBroker->postDelayed( - {EV_IGN_GETSTATUS}, TOPIC_IGNITION, INTERVAL_IGN_GET_STATUS); - break; - } - - // Still handle the abort, just in case we want to send it again - case EV_TC_ABORT_LAUNCH: - { - status.n_sent_messages++; - status.abort_sent = 1; - - uint8_t cmd = CAN_MSG_ABORT; - canbus->send((uint16_t)CAN_TOPIC_HOMEONE, &cmd, sizeof(uint8_t)); - break; - } - - case EV_NEW_CAN_MSG: - { - if (updateIgnBoardStatus(ev)) - { - status.n_rcv_messages++; - - // Reset the ignition offline timeout - sEventBroker->removeDelayed(ign_offline_delayed_id); - // Reschedule offline event - ign_offline_delayed_id = sEventBroker->postDelayed( - {EV_IGN_OFFLINE}, TOPIC_FLIGHT_EVENTS, - TIMEOUT_IGN_OFFLINE); - - logStatus(); - } - break; - } - - default: - TRACE("IGNCTRL stateAborted: Event %d not handled.\n", ev.sig); - break; - } -} - -void IgnitionController::stateEnd(const Event& ev) -{ - status.last_event = ev.sig; - - switch (ev.sig) - { - case EV_ENTRY: - sEventBroker->removeDelayed(ign_offline_delayed_id); - status.fsm_state = IgnitionControllerState::IGN_END; - logStatus(); - TRACE("IGNCTRL: Entering stateEnd\n"); - break; - - case EV_EXIT: - TRACE("IGNCTRL: Exiting stateEnd\n"); - break; - - // No event handled here - - default: - TRACE("IGNCTRL stateEnd: Event %d not handled.\n", ev.sig); - break; - } -} - -} // namespace HomeoneBoard diff --git a/src/boards/DeathStack/IgnitionController/IgnitionController.h b/src/boards/DeathStack/IgnitionController/IgnitionController.h deleted file mode 100644 index 2a885e2ee11cfd714d5ffa091efb531306273a1d..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/IgnitionController/IgnitionController.h +++ /dev/null @@ -1,83 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include "IgnitionStatus.h" - -#include <events/FSM.h> -#include <DeathStack/LoggerService/LoggerService.h> -#include <DeathStack/Canbus/CanProxy.h> - -namespace DeathStackBoard -{ - -class IgnitionController : public FSM<IgnitionController> -{ - -public: - /** - * @brief Initialize the controller and subscribe to EventBroker topics - */ - IgnitionController(CanProxy* canbus); - ~IgnitionController() {} - -private: - void stateIdle(const Event& ev); - void stateAborted(const Event& ev); - void stateEnd(const Event& ev); - - /** - * @brief Put timestamp and log the component's status - * @param None - * @return None - */ - void logStatus(); - - /** - * @brief Updates the status of the ignition board if received on the canbus - * and logs it on the logger. - * - * @param ev The NEW_CAN_MSG event received - * @return true the received event contained an IGN_STATUS message: updated - * internal board status struct - * @return false the received message was not directed to me: not handled - */ - bool updateIgnBoardStatus(const Event& ev); - - // Internal stats - IgnCtrlStatus status; - // Status received from the board, with timestamp - IgnBoardLoggableStatus loggable_board_status; - - CanProxy* canbus; - LoggerService& logger = *(LoggerService::getInstance()); - - // Id of the IGN_OFFLINE delayed event posted in the Event Broker - // NOTE: this is needed to cancel the delayed event - uint16_t ign_offline_delayed_id = 0; - // Id of the GET_IGN_STATUS delayed event posted in the Event Broker' - // NOTE: this is needed to cancel the delayed event - uint16_t get_status_delayed_id = 0; -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h b/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h deleted file mode 100644 index f2b72c7f0c3fdf3ae5da66da744d047c0ecdde69..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h +++ /dev/null @@ -1,99 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <events/FSM.h> - -#include "TmRepository.h" - -#include "DeathStack/ADA/ADAStatus.h" -#include "DeathStack/SensorManager/Sensors/AD7994WrapperData.h" -#include "DeathStack/SensorManager/Sensors/PiksiData.h" -#include "DeathStack/configs/FlightStatsConfig.h" -#include "FlightStatsData.h" -#include "sensors/MPU9250/MPU9250Data.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapperData.h" - -namespace DeathStackBoard -{ - -/** - * @brief Records statistics about the flight such as maximum acceleration during - * liftoff, maximum altitude, maximum speed etc. In order to do so, we need to - * know in which stage of the flight we are in, and we do so using a state - * machine and receiving events from the topic FLIGHT_EVENTS - */ -class FlightStatsRecorder : public FSM<FlightStatsRecorder> -{ -public: - FlightStatsRecorder(); - ~FlightStatsRecorder(); - - void update(const KalmanState& t); - void update(const CurrentSenseData& t); - void update(const ADAData& t); - void update(const AD7994WrapperData& t); - void update(const MPU9250Data& t); - void update(const PiksiData& t); - - // Wait for liftoff or deployment - void state_idle(const Event& ev); - - void state_testing_cutters(const Event& ev); - - // Record stats of the first few seconds of flight - void state_liftOff(const Event& ev); - - // Record stats for the apogee part of the flight - void state_ascending(const Event& ev); - - // Record stats during drogue deployment - void state_drogueDeployment(const Event& ev); - - // Stats during main deployment - void state_mainDeployment(const Event& ev); - -private: - enum class State - { - IDLE, - TESTING_CUTTER, - LIFTOFF, - ASCENDING, - DROGUE_DPL, - MAIN_DPL - }; - - LiftOffStats liftoff_stats{}; - ApogeeStats apogee_stats{}; - DrogueDPLStats drogue_dpl_stats{}; - MainDPLStats main_dpl_stats{}; - CutterTestStats cutter_stats{}; - State state = State::IDLE; - long long T_liftoff = 0; - - uint16_t ev_timeout_id = 0; -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/LoggerService/LoggerService.cpp b/src/boards/DeathStack/LoggerService/LoggerService.cpp deleted file mode 100644 index 482643515615093d0aaf1b9a99161e0b0969cdbb..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/LoggerService/LoggerService.cpp +++ /dev/null @@ -1,646 +0,0 @@ -/* Copyright (c) 2015-2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron, Alessio Galluccio - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#include "LoggerService.h" -#include "FlightStatsRecorder.h" -#include "TmRepository.h" - -#include "DeathStack/ADA/ADAStatus.h" -#include "DeathStack/DeathStackStatus.h" -#include "DeathStack/DeploymentController/DeploymentData.h" -#include "DeathStack/FlightModeManager/FMMStatus.h" -#include "DeathStack/IgnitionController/IgnitionStatus.h" -#include "DeathStack/PinHandler/PinHandlerData.h" -#include "DeathStack/SensorManager/SensorManagerData.h" -#include "DeathStack/SensorManager/Sensors/AD7994WrapperData.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapperData.h" -#include "DeathStack/SensorManager/Sensors/PiksiData.h" - -#include "drivers/canbus/CanUtils.h" -#include "drivers/mavlink/MavlinkStatus.h" -#include "scheduler/TaskSchedulerData.h" -#include "sensors/ADIS16405/ADIS16405Data.h" -#include "sensors/MPU9250/MPU9250Data.h" - -namespace DeathStackBoard -{ -/** - * @brief Each function here is an implementation of the log() method for a - * specific status struct or class. - * The rationale is that, whenever a status struct is logged, the corresponding - * telemetry struct is updated synchronously in the telemetry repo. - * Telemetry requests from ground will therefore return the latest logged struct - * from each component. - */ - -template <> -LogResult LoggerService::log<DeathStackStatus>(const DeathStackStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.sys_tm.death_stack = t.death_stack; - tm_repository.sys_tm.logger = t.logger; - tm_repository.sys_tm.ev_broker = t.ev_broker; - tm_repository.sys_tm.pin_obs = t.pin_obs; - tm_repository.sys_tm.fmm = t.pin_obs; - tm_repository.sys_tm.sensor_manager = t.sensor_manager; - tm_repository.sys_tm.ada = t.ada; - tm_repository.sys_tm.tmtc = t.tmtc; - tm_repository.sys_tm.ign = t.ign; - tm_repository.sys_tm.dpl = t.dpl; - } - return logger.log(t); -} - -/* Flight Mode Manager */ -template <> -LogResult LoggerService::log<FMMStatus>(const FMMStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.fmm_tm.state = static_cast<uint8_t>(t.state); - tm_repository.hr_tm.fmm_state = static_cast<uint8_t>(t.state); - } - return logger.log(t); -} - -/* Launch and Nosecone detachment pins */ -template <> -LogResult LoggerService::log<PinStatus>(const PinStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - switch (t.pin) - { - - case ObservedPin::LAUNCH: - { - tm_repository.fmm_tm.pin_launch_last_change = - t.last_state_change / 1000; - tm_repository.fmm_tm.pin_launch_num_changes = - t.num_state_changes; - tm_repository.fmm_tm.pin_launch_state = t.state; - // HR TM - tm_repository.hr_tm.pin_launch = t.state; - break; - } - case ObservedPin::NOSECONE: - { - tm_repository.fmm_tm.pin_nosecone_last_change = - t.last_state_change / 1000; - tm_repository.fmm_tm.pin_nosecone_num_changes = - t.num_state_changes; - tm_repository.fmm_tm.pin_nosecone_state = t.state; - - // HR TM - tm_repository.hr_tm.pin_nosecone = t.state; - break; - } - case ObservedPin::MOTOR: - { - // No time to change telemetries, since we have no GPS use gps - // telemetry to send motor pin state. - tm_repository.gps_tm.n_satellites = t.num_state_changes; - tm_repository.gps_tm.lat = t.last_state_change / 1000; - tm_repository.hr_tm.gps_fix = t.state; - break; - } - default: - break; - } - } - return logger.log(t); -} - -/* Ignition Board */ -template <> -LogResult LoggerService::log<IgnBoardLoggableStatus>( - const IgnBoardLoggableStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - uint16_t bs; - memcpy(&bs, &t.board_status, sizeof(bs)); - - tm_repository.ign_tm.avr_bitfield = bs & 0xFF; - tm_repository.ign_tm.avr_bitfield = bs >> 8; - } - - return logger.log(t); -} - -/* Ignition Controller */ -template <> -LogResult LoggerService::log<IgnCtrlStatus>(const IgnCtrlStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.ign_tm.fsm_state = t.fsm_state; - tm_repository.ign_tm.last_event = t.last_event; - - tm_repository.ign_tm.n_rcv_message = t.n_rcv_messages; - tm_repository.ign_tm.n_sent_messages = t.n_sent_messages; - - // Bitfield - tm_repository.ign_tm.cmd_bitfield = 0; - tm_repository.ign_tm.cmd_bitfield |= t.launch_sent; - tm_repository.ign_tm.cmd_bitfield |= (t.abort_sent << 1); - tm_repository.ign_tm.cmd_bitfield |= (t.abort_rcv << 2); - } - return logger.log(t); -} - -/* Logger */ -template <> -LogResult LoggerService::log<LogStats>(const LogStats& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.logger_tm.statLogNumber = t.logNumber; - tm_repository.logger_tm.statTooLargeSamples = t.statTooLargeSamples; - tm_repository.logger_tm.statDroppedSamples = t.statDroppedSamples; - tm_repository.logger_tm.statQueuedSamples = t.statQueuedSamples; - tm_repository.logger_tm.statBufferFilled = t.statBufferFilled; - tm_repository.logger_tm.statBufferWritten = t.statBufferWritten; - tm_repository.logger_tm.statWriteFailed = t.statWriteFailed; - tm_repository.logger_tm.statWriteError = t.statWriteError; - tm_repository.logger_tm.statWriteTime = t.statWriteTime; - tm_repository.logger_tm.statMaxWriteTime = t.statMaxWriteTime; - } - return logger.log(t); -} - -/* TMTCManager (Mavlink) */ -template <> -LogResult LoggerService::log<MavlinkStatus>(const MavlinkStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - // mavchannel stats - tm_repository.tmtc_tm.n_send_queue = t.n_send_queue; - tm_repository.tmtc_tm.max_send_queue = t.max_send_queue; - tm_repository.tmtc_tm.n_send_errors = t.n_send_errors; - tm_repository.tmtc_tm.msg_received = t.mav_stats.msg_received; - // mav stats - tm_repository.tmtc_tm.buffer_overrun = t.mav_stats.buffer_overrun; - tm_repository.tmtc_tm.parse_error = t.mav_stats.parse_error; - tm_repository.tmtc_tm.parse_state = t.mav_stats.parse_state; - tm_repository.tmtc_tm.packet_idx = t.mav_stats.packet_idx; - tm_repository.tmtc_tm.current_rx_seq = t.mav_stats.current_rx_seq; - tm_repository.tmtc_tm.current_tx_seq = t.mav_stats.current_tx_seq; - tm_repository.tmtc_tm.packet_rx_success_count = - t.mav_stats.packet_rx_success_count; - tm_repository.tmtc_tm.packet_rx_drop_count = - t.mav_stats.packet_rx_drop_count; - } - return logger.log(t); -} - -/* Sensor Manager */ -template <> -LogResult LoggerService::log<SensorManagerStatus>(const SensorManagerStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.sm_tm.sensor_state = t.sensor_status; - tm_repository.sm_tm.state = (uint8_t)t.state; - } - return logger.log(t); -} - -/* Deployment Controller */ -template <> -LogResult LoggerService::log<DeploymentStatus>(const DeploymentStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.dpl_tm.fsm_state = (uint8_t)t.state; - tm_repository.dpl_tm.cutter_state = (uint8_t)t.cutter_status.state; - - // HR TM - tm_repository.hr_tm.dpl_state = t.state; - } - return logger.log(t); -} - -/* ADA state machine */ -template <> -LogResult LoggerService::log<ADAControllerStatus>(const ADAControllerStatus& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.ada_tm.state = (uint8_t)t.state; - } - return logger.log(t); -} - -/* ADA target dpl pressure */ -template <> -LogResult LoggerService::log<TargetDeploymentAltitude>( - const TargetDeploymentAltitude& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.ada_tm.target_dpl_altitude = t.deployment_altitude; - } - return logger.log(t); -} - -/* ADA kalman filter values */ -template <> -LogResult LoggerService::log<KalmanState>(const KalmanState& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.ada_tm.kalman_x0 = t.x0; - tm_repository.ada_tm.kalman_x1 = t.x1; - tm_repository.ada_tm.kalman_x2 = t.x2; - - tm_repository.ada_tm.kalman_acc_x0 = t.x0_acc; - tm_repository.ada_tm.kalman_acc_x1 = t.x1_acc; - tm_repository.ada_tm.kalman_acc_x2 = t.x2_acc; - } - - flight_stats.update(t); - - return logger.log(t); -} - -/* ADA kalman altitude values */ -template <> -LogResult LoggerService::log<ADAData>(const ADAData& t) -{ - { - miosix::PauseKernelLock kLock; - tm_repository.hr_tm.msl_altitude = t.msl_altitude; - tm_repository.hr_tm.agl_altitude = t.dpl_altitude; - tm_repository.hr_tm.vert_speed = t.vert_speed; - tm_repository.hr_tm.vert_speed_2 = t.acc_vert_speed; - } - flight_stats.update(t); - - return logger.log(t); -} - -template <> -LogResult LoggerService::log<ReferenceValues>(const ReferenceValues& t) -{ - { - miosix::PauseKernelLock kLock; - tm_repository.ada_tm.msl_pressure = t.msl_pressure; - tm_repository.ada_tm.msl_temperature = t.msl_temperature; - - tm_repository.ada_tm.ref_altitude = t.ref_altitude; - tm_repository.ada_tm.ref_pressure = t.ref_pressure; - tm_repository.ada_tm.ref_temperature = t.ref_temperature; - } - - return logger.log(t); -} - -// template <> -// LogResult LoggerService::log<ADACalibrationData>(const ADACalibrationData& t) -// { -// { -// miosix::PauseKernelLock kLock; - -// tm_repository.ada_tm.ref_pressure_mean = t.pressure_calib.mean; -// tm_repository.ada_tm.ref_pressure_stddev = t.pressure_calib.stdev; -// tm_repository.ada_tm.ref_pressure_nsamples = -// t.pressure_calib.nSamples; -// } -// return logger.log(t); -// } - -/* Canbus stats */ -template <> -LogResult LoggerService::log<CanStatus>(const CanStatus& t) -{ - { - miosix::PauseKernelLock kLock; - tm_repository.can_tm.n_sent = t.n_sent; - tm_repository.can_tm.n_rcv = t.n_rcv; - tm_repository.can_tm.last_sent = t.last_sent; - tm_repository.can_tm.last_rcv = t.last_rcv; - tm_repository.can_tm.last_sent_ts = t.last_sent_ts; - tm_repository.can_tm.last_rcv_ts = t.last_rcv_ts; - } - - return logger.log(t); -} - -/* Main Barometer */ -template <> -LogResult LoggerService::log<AD7994WrapperData>(const AD7994WrapperData& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.adc_tm.nxp_baro_volt = t.nxp_baro_volt; - tm_repository.adc_tm.nxp_baro_flag = t.nxp_baro_flag; - tm_repository.adc_tm.nxp_baro_pressure = t.nxp_baro_pressure; - - tm_repository.adc_tm.hw_baro_volt = t.honeywell_baro_volt; - tm_repository.adc_tm.hw_baro_flag = t.honeywell_baro_flag; - tm_repository.adc_tm.hw_baro_pressure = t.honeywell_baro_pressure; - - // HR TM - tm_repository.hr_tm.pressure_ada = t.nxp_baro_pressure; - - // Test tm - tm_repository.test_tm.pressure_hw = t.honeywell_baro_pressure; - } - - flight_stats.update(t); - return logger.log(t); -} - -/* Battery status, sampled by internal ADC */ -template <> -LogResult LoggerService::log<BatteryVoltageData>(const BatteryVoltageData& t) -{ - tm_repository.adc_tm.battery_voltage = t.volt; - tm_repository.test_tm.battery_volt = t.volt; - - return logger.log(t); -} - -/* Current sense, sampled by internal ADC */ -template <> -LogResult LoggerService::log<CurrentSenseData>(const CurrentSenseData& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.adc_tm.current_sense_1 = t.current_1; - tm_repository.adc_tm.current_sense_2 = t.current_2; - - tm_repository.test_tm.th_cut_1 = t.current_1; - tm_repository.test_tm.th_cut_2 = t.current_2; - } - return logger.log(t); -} - -template <> -LogResult LoggerService::log<MS5803Data>(const MS5803Data& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.hr_tm.pressure_digi = t.pressure; - } - return logger.log(t); -} - -/* ADIS imu */ -template <> -LogResult LoggerService::log<ADIS16405Data>(const ADIS16405Data& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.adis_tm.acc_x = t.xaccl_out; - tm_repository.adis_tm.acc_y = t.yaccl_out; - tm_repository.adis_tm.acc_z = t.zaccl_out; - tm_repository.adis_tm.gyro_x = t.xgyro_out; - tm_repository.adis_tm.gyro_y = t.ygyro_out; - tm_repository.adis_tm.gyro_z = t.zgyro_out; - tm_repository.adis_tm.compass_x = t.xmagn_out; - tm_repository.adis_tm.compass_y = t.ymagn_out; - tm_repository.adis_tm.compass_z = t.zmagn_out; - tm_repository.adis_tm.temp = t.temp_out; - tm_repository.adis_tm.supply_out = t.supply_out; - tm_repository.adis_tm.aux_adc = t.aux_adc; - - // HR TM - // tm_repository.hr_tm.z_acc = t.zaccl_out; - } - - return logger.log(t); -} - -/* MPU imu */ -template <> -LogResult LoggerService::log<MPU9250Data>(const MPU9250Data& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.mpu_tm.acc_x = t.accel.getX(); - tm_repository.mpu_tm.acc_y = t.accel.getY(); - tm_repository.mpu_tm.acc_z = t.accel.getZ(); - tm_repository.mpu_tm.gyro_x = t.gyro.getX(); - tm_repository.mpu_tm.gyro_y = t.gyro.getY(); - tm_repository.mpu_tm.gyro_z = t.gyro.getZ(); - tm_repository.mpu_tm.compass_x = t.compass.getX(); - tm_repository.mpu_tm.compass_y = t.compass.getY(); - tm_repository.mpu_tm.compass_z = t.compass.getZ(); - tm_repository.mpu_tm.temp = t.temp; - - // HR TM - tm_repository.hr_tm.acc_x = t.accel.getX(); - tm_repository.hr_tm.acc_y = t.accel.getY(); - tm_repository.hr_tm.acc_z = t.accel.getZ(); - - tm_repository.hr_tm.gyro_x = t.gyro.getX(); - tm_repository.hr_tm.gyro_y = t.gyro.getY(); - tm_repository.hr_tm.gyro_z = t.gyro.getZ(); - } - - flight_stats.update(t); - return logger.log(t); -} - -/* LM75b temperature */ -template <> -LogResult LoggerService::log<LM75BData>(const LM75BData& t) -{ - { - miosix::PauseKernelLock kLock; - tm_repository.test_tm.temp_analog = t.temp_analog; - tm_repository.hr_tm.temperature = t.temp_imu; - tm_repository.test_tm.temp_imu = t.temp_imu; - } - - return logger.log(t); -} - -/* GPS */ -template <> -LogResult LoggerService::log<PiksiData>(const PiksiData& t) -{ - { - miosix::PauseKernelLock kLock; - - // GPS_TM - // tm_repository.gps_tm.lat = t.gps_data.latitude; - // tm_repository.gps_tm.lon = t.gps_data.longitude; - // tm_repository.gps_tm.altitude = t.gps_data.height; - // tm_repository.gps_tm.vel_north = t.gps_data.velocityNorth; - // tm_repository.gps_tm.vel_east = t.gps_data.velocityEast; - // tm_repository.gps_tm.vel_down = t.gps_data.velocityDown; - // tm_repository.gps_tm.vel_mag = t.gps_data.speed; - // tm_repository.gps_tm.fix = (uint8_t)t.fix; - // tm_repository.gps_tm.n_satellites = t.gps_data.numSatellites; - - // HR TM - tm_repository.hr_tm.gps_lat = t.gps_data.latitude; - tm_repository.hr_tm.gps_lon = t.gps_data.longitude; - tm_repository.hr_tm.gps_alt = t.gps_data.height; - // tm_repository.hr_tm.gps_fix = t.fix; - - // TEST TM - tm_repository.test_tm.gps_nsats = t.gps_data.numSatellites; - } - - flight_stats.update(t); - - return logger.log(t); -} - -template <> -LogResult LoggerService::log<TaskStatResult>(const TaskStatResult& t) -{ - { - miosix::PauseKernelLock kLock; - switch (t.id) - { - case static_cast<uint8_t>(SensorSamplerId::SIMPLE_20HZ): - tm_repository.sm_tm.task_20hz_max = t.periodStats.maxValue; - tm_repository.sm_tm.task_20hz_min = t.periodStats.minValue; - tm_repository.sm_tm.task_20hz_mean = t.periodStats.mean; - tm_repository.sm_tm.task_20hz_stddev = t.periodStats.stdev; - break; - case static_cast<uint8_t>(SensorSamplerId::GPS): - tm_repository.sm_tm.task_10hz_max = t.periodStats.maxValue; - tm_repository.sm_tm.task_10hz_min = t.periodStats.minValue; - tm_repository.sm_tm.task_10hz_mean = t.periodStats.mean; - tm_repository.sm_tm.task_10hz_stddev = t.periodStats.stdev; - break; - case static_cast<uint8_t>(SensorSamplerId::SIMPLE_250HZ): - tm_repository.sm_tm.task_250hz_max = t.periodStats.maxValue; - tm_repository.sm_tm.task_250hz_min = t.periodStats.minValue; - tm_repository.sm_tm.task_250hz_mean = t.periodStats.mean; - tm_repository.sm_tm.task_250hz_stddev = t.periodStats.stdev; - break; - - default: - break; - } - } - return logger.log(t); -} - -template <> -LogResult LoggerService::log<LiftOffStats>(const LiftOffStats& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.lr_tm.liftoff_ts = t.T_liftoff; - - tm_repository.lr_tm.liftoff_max_acc_ts = t.T_max_acc; - tm_repository.lr_tm.liftoff_max_acc = t.acc_max; - - tm_repository.lr_tm.max_zspeed_ts = t.T_max_speed; - tm_repository.lr_tm.max_zspeed = t.vert_speed_max; - tm_repository.lr_tm.max_speed_altitude = t.altitude_max_speed; - } - - return logger.log(t); -} - -template <> -LogResult LoggerService::log<ApogeeStats>(const ApogeeStats& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.lr_tm.apogee_ts = t.T_apogee; - - tm_repository.lr_tm.nxp_min_pressure = t.nxp_min_pressure; - tm_repository.lr_tm.hw_min_pressure = t.hw_min_pressure; - tm_repository.lr_tm.kalman_min_pressure = t.kalman_min_pressure; - tm_repository.lr_tm.digital_min_pressure = t.digital_min_pressure; - - tm_repository.lr_tm.baro_max_altitutde = t.baro_max_altitude; - tm_repository.lr_tm.gps_max_altitude = t.gps_max_altitude; - - tm_repository.lr_tm.apogee_lat = t.lat_apogee; - tm_repository.lr_tm.apogee_lon = t.lon_apogee; - } - - return logger.log(t); -} - -template <> -LogResult LoggerService::log<DrogueDPLStats>(const DrogueDPLStats& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.lr_tm.drogue_dpl_ts = t.T_dpl; - tm_repository.lr_tm.drogue_dpl_max_acc = t.max_dpl_acc; - } - - return logger.log(t); -} - -template <> -LogResult LoggerService::log<MainDPLStats>(const MainDPLStats& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.lr_tm.main_dpl_ts = t.T_dpl; - tm_repository.lr_tm.main_dpl_acc = t.max_dpl_acc; - tm_repository.lr_tm.main_dpl_altitude = t.altitude_dpl; - tm_repository.lr_tm.main_dpl_zspeed = t.vert_speed_dpl; - } - return logger.log(t); -} - -template <> -LogResult LoggerService::log<CutterTestStats>(const CutterTestStats& t) -{ - { - miosix::PauseKernelLock kLock; - - tm_repository.dpl_tm.cutter_1_test_current = t.cutter_1_avg; - tm_repository.dpl_tm.cutter_2_test_current = t.cutter_2_avg; - } - - return logger.log(t); -} - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/LoggerService/LoggerService.h b/src/boards/DeathStack/LoggerService/LoggerService.h index 3eb367a317590f3ae6c147739c9515a5d8645209..c94f2cc8fe9bf35aafee3ab5e37bc6a90087d5e5 100644 --- a/src/boards/DeathStack/LoggerService/LoggerService.h +++ b/src/boards/DeathStack/LoggerService/LoggerService.h @@ -1,5 +1,5 @@ /* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,29 +22,9 @@ #pragma once -#include "Singleton.h" -#include "logger/Logger.h" - -#include "FlightStatsRecorder.h" -#include "TmRepository.h" - -#include "DeathStack/ADA/ADAStatus.h" -#include "DeathStack/DeathStackStatus.h" -#include "DeathStack/DeploymentController/DeploymentData.h" -#include "DeathStack/FlightModeManager/FMMStatus.h" -#include "DeathStack/IgnitionController/IgnitionStatus.h" -#include "DeathStack/PinHandler/PinHandlerData.h" -#include "DeathStack/SensorManager/SensorManagerData.h" -#include "DeathStack/SensorManager/Sensors/AD7994WrapperData.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapperData.h" -#include "DeathStack/SensorManager/Sensors/PiksiData.h" - -#include "drivers/canbus/CanUtils.h" -#include "drivers/mavlink/MavlinkStatus.h" -#include "scheduler/TaskSchedulerData.h" -#include "sensors/ADIS16405/ADIS16405Data.h" -#include "sensors/MPU9250/MPU9250Data.h" -#include "sensors/MS580301BA07/MS580301BA07Data.h" +#include <Singleton.h> +#include <TelemetriesTelecommands/TmRepository.h> +#include <logger/Logger.h> namespace DeathStackBoard { @@ -60,25 +40,24 @@ class LoggerService : public Singleton<LoggerService> friend class Singleton<LoggerService>; public: - LoggerService() : logger(Logger::instance()) - { - initTelemetries(); - flight_stats.start(); - } - - ~LoggerService() {} - - /* Generic log function, to be implemented for each loggable struct */ + /** + * @brief Generic log function, to be implemented for each loggable struct. + */ template <typename T> inline LogResult log(const T& t) { + { + miosix::PauseKernelLock kLock; + tmRepo.update(t); + } return logger.log(t); } /** - * Blocking call. May take a long time. + * WARNING: Blocking call. May take a long time. + * + * @brief Call this function to start the logger. * - * Call this function to start the logger. * When this function returns, the logger is started, and subsequent calls * to log will actually log the data. * @@ -88,127 +67,32 @@ public: int start() { return logger.start(); } /** - * Blocking call. May take a very long time (seconds). + * WARNING: Blocking call. May take a very long time (seconds). + * + * @brief Call this function to stop the logger. * - * Call this function to stop the logger. * When this function returns, all log buffers have been flushed to disk, * and it is safe to power down the board without losing log data or * corrupting the filesystem. */ void stop() { logger.stop(); } -private: - Logger& logger; // SD logger - FlightStatsRecorder flight_stats{}; -}; - -template <> -LogResult LoggerService::log<DeathStackStatus>(const DeathStackStatus& t); - -/* Flight Mode Manager */ -template <> -LogResult LoggerService::log<FMMStatus>(const FMMStatus& t); - -/* Launch and Nosecone detachment pins */ -template <> -LogResult LoggerService::log<PinStatus>(const PinStatus& t); - -/* Ignition Board */ -template <> -LogResult LoggerService::log<IgnBoardLoggableStatus>( - const IgnBoardLoggableStatus& t); - -/* Ignition Controller */ -template <> -LogResult LoggerService::log<IgnCtrlStatus>(const IgnCtrlStatus& t); - -/* Logger */ -template <> -LogResult LoggerService::log<LogStats>(const LogStats& t); - -/* TMTCManager (Mavlink) */ -template <> -LogResult LoggerService::log<MavlinkStatus>(const MavlinkStatus& t); - -/* Sensor Manager */ -template <> -LogResult LoggerService::log<SensorManagerStatus>(const SensorManagerStatus& t); - -/* Deployment Controller */ -template <> -LogResult LoggerService::log<DeploymentStatus>(const DeploymentStatus& t); - -/* ADA state machine */ -template <> -LogResult LoggerService::log<ADAControllerStatus>(const ADAControllerStatus& t); + Logger& getLogger() { return logger; } -/* ADA target dpl pressure */ -template <> -LogResult LoggerService::log<TargetDeploymentAltitude>( - const TargetDeploymentAltitude& t); - -/* ADA kalman filter values */ -template <> -LogResult LoggerService::log<KalmanState>(const KalmanState& t); - -/* ADA kalman altitude values */ -template <> -LogResult LoggerService::log<ADAData>(const ADAData& t); - -template <> -LogResult LoggerService::log<ReferenceValues>(const ReferenceValues& t); - -/* Canbus stats */ -template <> -LogResult LoggerService::log<CanStatus>(const CanStatus& t); - -/* Main Barometer */ -template <> -LogResult LoggerService::log<AD7994WrapperData>(const AD7994WrapperData& t); - -/* Battery status, sampled by internal ADC */ -template <> -LogResult LoggerService::log<BatteryVoltageData>(const BatteryVoltageData& t); - -/* Motor current sense, sampled by internal ADC */ -template <> -LogResult LoggerService::log<CurrentSenseData>(const CurrentSenseData& t); - -template <> -LogResult LoggerService::log<MS5803Data>(const MS5803Data& t); - -/* ADIS imu */ -template <> -LogResult LoggerService::log<ADIS16405Data>(const ADIS16405Data& t); - -/* MPU imu */ -template <> -LogResult LoggerService::log<MPU9250Data>(const MPU9250Data& t); - -/* GPS */ -template <> -LogResult LoggerService::log<PiksiData>(const PiksiData& t); - -/* LM75b temperature */ -template <> -LogResult LoggerService::log<LM75BData>(const LM75BData& t); - -template <> -LogResult LoggerService::log<TaskStatResult>(const TaskStatResult& t); - -template <> -LogResult LoggerService::log<LiftOffStats>(const LiftOffStats& t); - -template <> -LogResult LoggerService::log<ApogeeStats>(const ApogeeStats& t); - -template <> -LogResult LoggerService::log<DrogueDPLStats>(const DrogueDPLStats& t); +private: + /** + * @brief Private constructor to enforce the singleton + */ + LoggerService() + : logger(Logger::instance()), tmRepo(*(TmRepository::getInstance())) + { + } -template <> -LogResult LoggerService::log<MainDPLStats>(const MainDPLStats& t); + ~LoggerService() {} -template <> -LogResult LoggerService::log<CutterTestStats>(const CutterTestStats& t); + Logger& logger; // SD loggers + // FlightStatsRecorder flight_stats{}; + TmRepository& tmRepo; +}; } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/LoggerService/TmRepository.cpp b/src/boards/DeathStack/LoggerService/TmRepository.cpp deleted file mode 100644 index c23f3eb501d1fe1f3f520ea7e777843f406e3c08..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/LoggerService/TmRepository.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "TmRepository.h" - -namespace DeathStackBoard -{ - -/* Global struct containing all telemetry packets. */ -TmRepository_t tm_repository; - -void initTelemetries() -{ - memset(&tm_repository, 0, sizeof(tm_repository)); -} - -} \ No newline at end of file diff --git a/src/boards/DeathStack/LoggerService/TmRepository.h b/src/boards/DeathStack/LoggerService/TmRepository.h deleted file mode 100644 index c0fe11aa68a05788a31f6edab6626384000df84b..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/LoggerService/TmRepository.h +++ /dev/null @@ -1,124 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h> - -/** - * @brief This file contains all telemetry packets in the form of mavlink - * structs. These packets are updated by the LoggerService and read by the - * TMTCManager. - * - * Notice that, if the LoggerService is not active, the value inside tm packets - * WILL NOT BE UPDATED. - */ -namespace DeathStackBoard -{ - -struct HighRateTM_t -{ - long long timestamp; - float pressure_ada; - float pressure_digi; - float msl_altitude; - float agl_altitude; - float vert_speed; - float vert_speed_2; - float acc_x; - float acc_y; - float acc_z; - float gyro_x; - float gyro_y; - float gyro_z; - float gps_lat; - float gps_lon; - float gps_alt; - float temperature; - uint8_t fmm_state; - uint8_t dpl_state; - uint8_t pin_launch; - uint8_t pin_nosecone; - uint8_t gps_fix; -}; - -struct LowRateTM_t -{ - long long liftoff_ts; - long long liftoff_max_acc_ts; - float liftoff_max_acc; - long long max_zspeed_ts; - float max_zspeed; - float max_speed_altitude; - long long apogee_ts; - float nxp_min_pressure; - float hw_min_pressure; - float kalman_min_pressure; - float digital_min_pressure; - float baro_max_altitutde; - float gps_max_altitude; - float apogee_lat; - float apogee_lon; - long long drogue_dpl_ts; - float drogue_dpl_max_acc; - long long main_dpl_ts; - float main_dpl_altitude; - float main_dpl_zspeed; - float main_dpl_acc; -}; - -/* Struct containing all tms in the form of mavlink structs */ -struct TmRepository_t -{ - mavlink_sys_tm_t sys_tm; - mavlink_fmm_tm_t fmm_tm; - mavlink_logger_tm_t logger_tm; - mavlink_tmtc_tm_t tmtc_tm; - - mavlink_sm_tm_t sm_tm; - mavlink_ign_tm_t ign_tm; - mavlink_dpl_tm_t dpl_tm; - mavlink_ada_tm_t ada_tm; - - mavlink_can_tm_t can_tm; - - mavlink_adc_tm_t adc_tm; - mavlink_adis_tm_t adis_tm; - mavlink_mpu_tm_t mpu_tm; - mavlink_gps_tm_t gps_tm; - - HighRateTM_t hr_tm; - LowRateTM_t lr_tm; - - mavlink_test_tm_t test_tm; -}; - -/* Forward declaration of the global struct. */ -extern TmRepository_t tm_repository; - -/** - * @brief This function initializes all the packets with 0s. - */ -void initTelemetries(); - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/DeploymentController/Motor/MotorData.h b/src/boards/DeathStack/Main/Actuators.h similarity index 66% rename from src/boards/DeathStack/DeploymentController/Motor/MotorData.h rename to src/boards/DeathStack/Main/Actuators.h index 81b81c32b19e0046aba58abe04f70423a5b531b2..8ec201723cc2167680595ddc999ded8974e9d4bd 100644 --- a/src/boards/DeathStack/DeploymentController/Motor/MotorData.h +++ b/src/boards/DeathStack/Main/Actuators.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,34 +13,25 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION\ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - #pragma once -#include <cstdint> +#include <AirBrakes/AirBrakesServo.h> +#include <Deployment/DeploymentServo.h> namespace DeathStackBoard { - -/** - * @brief Motor direction - */ -enum class MotorDirection : uint8_t -{ - NORMAL, - REVERSE -}; -struct MotorStatus +struct Actuators { - bool motor_active = false; - MotorDirection motor_last_direction = MotorDirection::NORMAL; + AirBrakesServo* airbrakes; + DeploymentServo* dpl_servo; }; -} +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/Bus.h b/src/boards/DeathStack/Main/Bus.h new file mode 100644 index 0000000000000000000000000000000000000000..c78b29ae93a5bb675b7d95926e63956df46840f0 --- /dev/null +++ b/src/boards/DeathStack/Main/Bus.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/spi/SPIBus.h> +#include <miosix.h> + +namespace DeathStackBoard +{ + +struct Bus +{ + SPIBusInterface* spi1 = new SPIBus(SPI1); + SPIBusInterface* spi2 = new SPIBus(SPI2); +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/Radio.cpp b/src/boards/DeathStack/Main/Radio.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ab53a6863ea9a250fbfb6bd0e0395debecf72e3b --- /dev/null +++ b/src/boards/DeathStack/Main/Radio.cpp @@ -0,0 +1,175 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <LoggerService/LoggerService.h> +#include <Main/Radio.h> +#include <TelemetriesTelecommands/TCHandler.h> +#include <TelemetriesTelecommands/TMTCController.h> +#include <TelemetriesTelecommands/TmRepository.h> +#include <drivers/Xbee/APIFramesLog.h> +#include <drivers/Xbee/ATCommands.h> +#include <drivers/interrupt/external_interrupts.h> +#include <interfaces-impl/hwmapping.h> + +#include <functional> + +// using std::function; +using std::bind; +using namespace std::placeholders; + +// Xbee ATTN interrupt +void __attribute__((used)) EXTI10_IRQHandlerImpl() +{ + using namespace DeathStackBoard; + + if (DeathStack::getInstance()->radio->xbee != nullptr) + { + DeathStack::getInstance()->radio->xbee->handleATTNInterrupt(); + } +} + +namespace DeathStackBoard +{ + +Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus) +{ + SPIBusConfig xbee_cfg{}; + + xbee_cfg.clock_div = SPIClockDivider::DIV16; + + xbee = new Xbee::Xbee(xbee_bus, xbee_cfg, miosix::xbee::cs::getPin(), + miosix::xbee::attn::getPin(), + miosix::xbee::reset::getPin()); + xbee->setOnFrameReceivedListener( + bind(&Radio::onXbeeFrameReceived, this, _1)); + + Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT); + + mav_driver = new MavDriver(xbee, handleMavlinkMessage, SLEEP_AFTER_SEND, + MAV_OUT_BUFFER_MAX_AGE); + + tmtc_manager = new TMTCController(); + + tm_repo = TmRepository::getInstance(); + + enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::FALLING_EDGE); +} + +Radio::~Radio() +{ + tmtc_manager->stop(); + mav_driver->stop(); + + delete tmtc_manager; + delete mav_driver; + delete xbee; +} + +bool Radio::start() { return mav_driver->start() && tmtc_manager->start(); } + +void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) +{ + LoggerService& logger = *LoggerService::getInstance(); + + using namespace Xbee; + bool logged = false; + switch (frame.frame_type) + { + case FTYPE_AT_COMMAND: + { + ATCommandFrameLog dest; + logged = ATCommandFrameLog::toFrameType(frame, &dest); + if (logged) + { + logger.log(dest); + } + break; + } + case FTYPE_AT_COMMAND_RESPONSE: + { + ATCommandResponseFrameLog dest; + logged = ATCommandResponseFrameLog::toFrameType(frame, &dest); + if (logged) + { + logger.log(dest); + } + break; + } + case FTYPE_MODEM_STATUS: + { + ModemStatusFrameLog dest; + logged = ModemStatusFrameLog::toFrameType(frame, &dest); + if (logged) + { + logger.log(dest); + } + break; + } + case FTYPE_TX_REQUEST: + { + TXRequestFrameLog dest; + logged = TXRequestFrameLog::toFrameType(frame, &dest); + if (logged) + { + logger.log(dest); + } + break; + } + case FTYPE_TX_STATUS: + { + TXStatusFrameLog dest; + logged = TXStatusFrameLog::toFrameType(frame, &dest); + if (logged) + { + logger.log(dest); + } + break; + } + case FTYPE_RX_PACKET_FRAME: + { + RXPacketFrameLog dest; + logged = RXPacketFrameLog::toFrameType(frame, &dest); + if (logged) + { + logger.log(dest); + } + break; + } + } + + if (!logged) + { + APIFrameLog api; + APIFrameLog::fromAPIFrame(frame, &api); + logger.log(api); + } +} + +void Radio::logStatus() +{ + MavlinkStatus status = mav_driver->getStatus(); + status.timestamp = TimestampTimer::getTimestamp(); + LoggerService::getInstance()->log(status); + LoggerService::getInstance()->log(xbee->getStatus()); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/Radio.h b/src/boards/DeathStack/Main/Radio.h new file mode 100644 index 0000000000000000000000000000000000000000..1058392888795cbfe3f898fc1ec89fcf31326bf0 --- /dev/null +++ b/src/boards/DeathStack/Main/Radio.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <TelemetriesTelecommands/Mavlink.h> +#include <drivers/Xbee/Xbee.h> + +namespace DeathStackBoard +{ + +class TMTCController; +class TmRepository; + +class Radio +{ +public: + TMTCController* tmtc_manager; + TmRepository* tm_repo; + Xbee::Xbee* xbee; + MavDriver* mav_driver; + + Radio(SPIBusInterface& xbee_bus); + ~Radio(); + + bool start(); + + void logStatus(); + +private: + void onXbeeFrameReceived(Xbee::APIFrame& frame); + + SPIBusInterface& xbee_bus; +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/Sensors.cpp b/src/boards/DeathStack/Main/Sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fae04a186488f9d6976a14f7cd2939f534888cc7 --- /dev/null +++ b/src/boards/DeathStack/Main/Sensors.cpp @@ -0,0 +1,573 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <DeathStack.h> +#include <Debug.h> +#include <LoggerService/LoggerService.h> +#include <TimestampTimer.h> +#include <configs/SensorsConfig.h> +#include <drivers/interrupt/external_interrupts.h> +#include <interfaces-impl/hwmapping.h> +#include <sensors/Sensor.h> +#include <utils/aero/AeroUtils.h> + +#include <functional> +#include <utility> + +using std::bind; +using std::function; + +// BMX160 Watermark interrupt +void __attribute__((used)) EXTI5_IRQHandlerImpl() +{ + using namespace DeathStackBoard; + + if (DeathStack::getInstance()->sensors->imu_bmx160 != nullptr) + { + DeathStack::getInstance()->sensors->imu_bmx160->IRQupdateTimestamp( + TimestampTimer::getTimestamp()); + } +} + +namespace DeathStackBoard +{ +using namespace SensorConfigs; + +Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) + : spi1_bus(spi1_bus) +{ + // sensors are added to the map ordered by increasing period + ADS1118Init(); // 6 ms + pressDigiInit(); // 10 ms + magLISinit(); + imuBMXInit(); // 23 ms + imuBMXWithCorrectionInit(); + pressPitotInit(); // 24 ms + pressDPLVaneInit(); + pressStaticInit(); + gpsUbloxInit(); // 40 ms + internalAdcInit(); // 50 ms + batteryVoltageInit(); +#ifdef HARDWARE_IN_THE_LOOP + hilImuInit(); + hilBarometerInit(); + hilGpsInit(); +#endif + + sensor_manager = new SensorManager(scheduler, sensors_map); +} + +Sensors::~Sensors() +{ + delete imu_bmx160; + delete press_digital; + delete gps_ublox; + delete internal_adc; + delete cs_cutter_primary; + delete cs_cutter_backup; + delete battery_voltage; + delete adc_ads1118; + delete press_pitot; + delete press_dpl_vane; + delete press_static_port; + delete mag_lis3mdl; +#ifdef HARDWARE_IN_THE_LOOP + delete hil_imu; + delete hil_baro; + delete hil_gps; +#endif + + sensor_manager->stop(); + delete sensor_manager; +} + +bool Sensors::start() +{ + GpioPin int_pin = miosix::sensors::bmx160::intr::getPin(); + enableExternalInterrupt(int_pin.getPort(), int_pin.getNumber(), + InterruptTrigger::FALLING_EDGE); + + gps_ublox->start(); + + bool sm_start_result = sensor_manager->start(); + + // if not init ok, set failing sensors in sensors status + if (!sm_start_result) + { + updateSensorsStatus(); + } + + LoggerService::getInstance()->log(status); + + return sm_start_result; +} + +void Sensors::calibrate() +{ + imu_bmx160_with_correction->calibrate(); + LoggerService::getInstance()->log( + imu_bmx160_with_correction->getGyroscopeBiases()); + + press_pitot->calibrate(); + // wait pitot calibration end + while (press_pitot->isCalibrating()) + { + Thread::sleep(10); + } +} + +void Sensors::internalAdcInit() +{ + internal_adc = new InternalADC(*ADC3, INTERNAL_ADC_VREF); + + internal_adc->enableChannel(ADC_BATTERY_VOLTAGE); + + SensorInfo info("InternalADC", SAMPLE_PERIOD_INTERNAL_ADC, + bind(&Sensors::internalAdcCallback, this), false, true); + sensors_map.emplace(std::make_pair(internal_adc, info)); + + LOG_INFO(log, "InternalADC setup done!"); +} + +void Sensors::batteryVoltageInit() +{ + function<ADCData()> voltage_fun( + bind(&InternalADC::getVoltage, internal_adc, ADC_BATTERY_VOLTAGE)); + battery_voltage = + new BatteryVoltageSensor(voltage_fun, BATTERY_VOLTAGE_COEFF); + + SensorInfo info("BatterySensor", SAMPLE_PERIOD_INTERNAL_ADC, + bind(&Sensors::batteryVoltageCallback, this), false, true); + + sensors_map.emplace(std::make_pair(battery_voltage, info)); + + LOG_INFO(log, "Battery voltage sensor setup done!"); +} + +void Sensors::pressDigiInit() +{ + SPIBusConfig spi_cfg{}; + spi_cfg.clock_div = SPIClockDivider::DIV16; + + press_digital = new MS5803(spi1_bus, miosix::sensors::ms5803::cs::getPin(), + spi_cfg, TEMP_DIVIDER_PRESS_DIGITAL); + + SensorInfo info("DigitalBarometer", SAMPLE_PERIOD_PRESS_DIGITAL, + bind(&Sensors::pressDigiCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_digital, info)); + + LOG_INFO(log, "MS5803 pressure sensor setup done!"); +} + +void Sensors::ADS1118Init() +{ + SPIBusConfig spi_cfg = ADS1118::getDefaultSPIConfig(); + spi_cfg.clock_div = SPIClockDivider::DIV64; + + ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; + ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; + + adc_ads1118 = new ADS1118(spi1_bus, miosix::sensors::ads1118::cs::getPin(), + ads1118Config, spi_cfg); + + adc_ads1118->enableInput(ADC_CH_STATIC_PORT, ADC_DR_STATIC_PORT, + ADC_PGA_STATIC_PORT); + + adc_ads1118->enableInput(ADC_CH_PITOT_PORT, ADC_DR_PITOT_PORT, + ADC_PGA_PITOT_PORT); + adc_ads1118->enableInput(ADC_CH_DPL_PORT, ADC_DR_DPL_PORT, + ADC_PGA_DPL_PORT); + + adc_ads1118->enableInput(ADC_CH_VREF, ADC_DR_VREF, ADC_PGA_VREF); + + SensorInfo info("ADS1118", SAMPLE_PERIOD_ADC_ADS1118, + bind(&Sensors::ADS1118Callback, this), false, true); + sensors_map.emplace(std::make_pair(adc_ads1118, info)); + + LOG_INFO(log, "ADS1118 setup done!"); +} + +void Sensors::pressPitotInit() +{ + function<ADCData()> voltage_fun( + bind(&ADS1118::getVoltage, adc_ads1118, ADC_CH_PITOT_PORT)); + press_pitot = new SSCDRRN015PDA(voltage_fun, REFERENCE_VOLTAGE); + + SensorInfo info("PitotBarometer", SAMPLE_PERIOD_PRESS_PITOT, + bind(&Sensors::pressPitotCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_pitot, info)); + + LOG_INFO(log, "Pitot pressure sensor setup done!"); +} + +void Sensors::pressDPLVaneInit() +{ + function<ADCData()> voltage_fun( + bind(&ADS1118::getVoltage, adc_ads1118, ADC_CH_DPL_PORT)); + press_dpl_vane = new SSCDANN030PAA(voltage_fun, REFERENCE_VOLTAGE); + + SensorInfo info("DeploymentBarometer", SAMPLE_PERIOD_PRESS_DPL, + bind(&Sensors::pressDPLVaneCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_dpl_vane, info)); + + LOG_INFO(log, "DPL pressure sensor setup done!"); +} + +void Sensors::pressStaticInit() +{ + function<ADCData()> voltage_fun( + bind(&ADS1118::getVoltage, adc_ads1118, ADC_CH_STATIC_PORT)); + press_static_port = new MPXHZ6130A(voltage_fun, REFERENCE_VOLTAGE); + + SensorInfo info("StaticPortsBarometer", SAMPLE_PERIOD_PRESS_STATIC, + bind(&Sensors::pressStaticCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_static_port, info)); + + LOG_INFO(log, "Static pressure sensor setup done!"); +} + +void Sensors::imuBMXInit() +{ + SPIBusConfig spi_cfg; + spi_cfg.clock_div = SPIClockDivider::DIV8; + + BMX160Config bmx_config; + bmx_config.fifo_mode = BMX160Config::FifoMode::HEADER; + bmx_config.fifo_watermark = IMU_BMX_FIFO_WATERMARK; + bmx_config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; + + bmx_config.temp_divider = IMU_BMX_TEMP_DIVIDER; + + bmx_config.acc_range = IMU_BMX_ACC_FULLSCALE_ENUM; + bmx_config.gyr_range = IMU_BMX_GYRO_FULLSCALE_ENUM; + + bmx_config.acc_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.gyr_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.mag_odr = IMU_BMX_MAG_ODR_ENUM; + + bmx_config.gyr_unit = BMX160Config::GyroscopeMeasureUnit::RAD; + + imu_bmx160 = new BMX160(spi1_bus, miosix::sensors::bmx160::cs::getPin(), + bmx_config, spi_cfg); + + SensorInfo info("BMX160", SAMPLE_PERIOD_IMU_BMX, + bind(&Sensors::imuBMXCallback, this), false, true); + + sensors_map.emplace(std::make_pair(imu_bmx160, info)); + + LOG_INFO(log, "BMX160 Setup done!"); +} + +void Sensors::imuBMXWithCorrectionInit() +{ + // Read the correction parameters + BMX160CorrectionParameters correctionParameters = + BMX160WithCorrection::readCorrectionParametersFromFile( + BMX160_CORRECTION_PARAMETERS_FILE); + + // Print the calibration parameters + TRACE("[Sensors] Current accelerometer bias vector\n"); + TRACE("[Sensors] b = [ % 2.5f % 2.5f % 2.5f ]\n", + correctionParameters.accelParams(0, 1), + correctionParameters.accelParams(1, 1), + correctionParameters.accelParams(2, 1)); + TRACE("[Sensors] Matrix to be multiplied to the input vector\n"); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n", + correctionParameters.accelParams(0, 0), 0.f, 0.f); + TRACE("[Sensors] M = | % 2.5f % 2.5f % 2.5f |\n", 0.f, + correctionParameters.accelParams(1, 0), 0.f); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n\n", 0.f, 0.f, + correctionParameters.accelParams(2, 0)); + TRACE("[Sensors] Current magnetometer bias vector\n"); + TRACE("[Sensors] b = [ % 2.5f % 2.5f % 2.5f ]\n", + correctionParameters.magnetoParams(0, 1), + correctionParameters.magnetoParams(1, 1), + correctionParameters.magnetoParams(2, 1)); + TRACE("[Sensors] Matrix to be multiplied to the input vector\n"); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n", + correctionParameters.magnetoParams(0, 0), 0.f, 0.f); + TRACE("[Sensors] M = | % 2.5f % 2.5f % 2.5f |\n", 0.f, + correctionParameters.magnetoParams(1, 0), 0.f); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n\n", 0.f, 0.f, + correctionParameters.magnetoParams(2, 0)); + TRACE( + "[Sensors] The current minimun number of gyroscope samples for " + "calibration is %d\n", + correctionParameters.minGyroSamplesForCalibration); + + imu_bmx160_with_correction = new BMX160WithCorrection( + imu_bmx160, correctionParameters, BMX160_AXIS_ROTATION); + + SensorInfo info("BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX, + bind(&Sensors::imuBMXWithCorrectionCallback, this), false, + true); + + sensors_map.emplace(std::make_pair(imu_bmx160_with_correction, info)); + + LOG_INFO(log, "BMX160WithCorrection Setup done!"); +} + +void Sensors::magLISinit() +{ + SPIBusConfig busConfig; + busConfig.clock_div = SPIClockDivider::DIV32; + + LIS3MDL::Config config; + config.odr = MAG_LIS_ODR_ENUM; + config.scale = MAG_LIS_FULLSCALE; + config.temperatureDivider = 1; + + mag_lis3mdl = new LIS3MDL(spi1_bus, miosix::sensors::lis3mdl::cs::getPin(), + busConfig, config); + + SensorInfo info("LIS3MDL", SAMPLE_PERIOD_MAG_LIS, + bind(&Sensors::magLISCallback, this), false, true); + + sensors_map.emplace(std::make_pair(mag_lis3mdl, info)); + + LOG_INFO(log, "LIS3MDL Setup done!"); +} + +void Sensors::gpsUbloxInit() +{ + gps_ublox = new UbloxGPS(GPS_BAUD_RATE, GPS_SAMPLE_RATE); + + SensorInfo info("UbloxGPS", GPS_SAMPLE_PERIOD, + bind(&Sensors::gpsUbloxCallback, this), false, true); + + sensors_map.emplace(std::make_pair(gps_ublox, info)); + + LOG_INFO(log, "Ublox GPS Setup done!"); +} + +void Sensors::internalAdcCallback() +{ + // LoggerService::getInstance()->log(internal_adc->getLastSample()); +} + +void Sensors::batteryVoltageCallback() +{ + static float v = battery_voltage->getLastSample().bat_voltage; + if (v < BATTERY_MIN_SAFE_VOLTAGE) + { + battery_critical_counter++; + // every 30 seconds to avoid filling the log (if debug disabled) + if (battery_critical_counter % 30 == 0) + { + LOG_CRIT(log, "*** LOW BATTERY *** ---> Voltage = {:02f}", v); + battery_critical_counter = 0; + } + } + + LoggerService::getInstance()->log(battery_voltage->getLastSample()); +} + +#ifdef HARDWARE_IN_THE_LOOP +void Sensors::hilBarometerInit() +{ + HILTransceiver* simulator = HIL::getInstance()->simulator; + + hil_baro = new HILBarometer(simulator, N_DATA_BARO); + + SensorInfo info_baro("HILBaro", HIL_BARO_PERIOD, + bind(&Sensors::hilBaroCallback, this), false, true); + + sensors_map.emplace(std::make_pair(hil_baro, info_baro)); + + LOG_INFO(log, "HIL barometer setup done!"); +} +void Sensors::hilImuInit() +{ + HILTransceiver* simulator = HIL::getInstance()->simulator; + + hil_imu = new HILImu(simulator, N_DATA_IMU); + + SensorInfo info_imu("HILImu", HIL_IMU_PERIOD, + bind(&Sensors::hilIMUCallback, this), false, true); + + sensors_map.emplace(std::make_pair(hil_imu, info_imu)); + + LOG_INFO(log, "HIL IMU setup done!"); +} + +void Sensors::hilGpsInit() +{ + HILTransceiver* simulator = HIL::getInstance()->simulator; + + hil_gps = new HILGps(simulator, N_DATA_GPS); + + SensorInfo info_gps("HILGps", HIL_GPS_PERIOD, + bind(&Sensors::hilGPSCallback, this), false, true); + + sensors_map.emplace(std::make_pair(hil_gps, info_gps)); + + LOG_INFO(log, "HIL GPS setup done!"); +} +#endif + +void Sensors::pressDigiCallback() +{ + LoggerService::getInstance()->log(press_digital->getLastSample()); +} + +void Sensors::ADS1118Callback() +{ + LoggerService::getInstance()->log(adc_ads1118->getLastSample()); +} + +void Sensors::pressPitotCallback() +{ + SSCDRRN015PDAData d = press_pitot->getLastSample(); + LoggerService::getInstance()->log(d); + + ADAReferenceValues rv = + DeathStack::getInstance() + ->state_machines->ada_controller->getReferenceValues(); + + float rel_density = aeroutils::relDensity( + press_digital->getLastSample().press, rv.ref_pressure, rv.ref_altitude, + rv.ref_temperature); + if (rel_density != 0.0f) + { + float airspeed = sqrtf(2 * fabs(d.press) / rel_density); + + AirSpeedPitot aspeed_data{TimestampTimer::getTimestamp(), airspeed}; + LoggerService::getInstance()->log(aspeed_data); + } +} + +void Sensors::pressDPLVaneCallback() +{ + LoggerService::getInstance()->log(press_dpl_vane->getLastSample()); +} + +void Sensors::pressStaticCallback() +{ + LoggerService::getInstance()->log(press_static_port->getLastSample()); +} + +void Sensors::imuBMXCallback() +{ + // static uint64_t max_t = 0; + // static unsigned int counter = 0; + + uint8_t fifo_size = imu_bmx160->getLastFifoSize(); + auto& fifo = imu_bmx160->getLastFifo(); + + LoggerService::getInstance()->log(imu_bmx160->getTemperature()); + + for (uint8_t i = 0; i < fifo_size; ++i) + { + BMX160Data d = fifo.at(i); + + // counter++; + // if (counter > 1000) + // { + // if (d.accel_timestamp > max_t) + // { + // max_t = d.accel_timestamp; + // } + // else + // { + // TRACE("\nBUG!!!\n\n"); + // } + // } + + LoggerService::getInstance()->log(d); + } + + LoggerService::getInstance()->log(imu_bmx160->getFifoStats()); +} + +void Sensors::imuBMXWithCorrectionCallback() +{ + + LoggerService::getInstance()->log( + imu_bmx160_with_correction->getLastSample()); +} + +void Sensors::magLISCallback() +{ + LoggerService::getInstance()->log(mag_lis3mdl->getLastSample()); +} + +void Sensors::gpsUbloxCallback() +{ + LoggerService::getInstance()->log(gps_ublox->getLastSample()); +} + +#ifdef HARDWARE_IN_THE_LOOP +void Sensors::hilIMUCallback() +{ + LoggerService::getInstance()->log(hil_imu->getLastSample()); +} +void Sensors::hilBaroCallback() +{ + LoggerService::getInstance()->log(hil_baro->getLastSample()); +} +void Sensors::hilGPSCallback() +{ + LoggerService::getInstance()->log(hil_gps->getLastSample()); +} +#endif + +void Sensors::updateSensorsStatus() +{ + SensorInfo info; + + info = sensor_manager->getSensorInfo(imu_bmx160); + if (!info.is_initialized) + { + status.bmx160 = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(mag_lis3mdl); + if (!info.is_initialized) + { + status.lis3mdl = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(gps_ublox); + if (!info.is_initialized) + { + status.gps = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(internal_adc); + if (!info.is_initialized) + { + status.internal_adc = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(adc_ads1118); + if (!info.is_initialized) + { + status.ads1118 = SensorDriverStatus::DRIVER_ERROR; + } +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/Sensors.h b/src/boards/DeathStack/Main/Sensors.h new file mode 100644 index 0000000000000000000000000000000000000000..9ae12cd4e43c5cfe674c5ceb9a69128d80fede89 --- /dev/null +++ b/src/boards/DeathStack/Main/Sensors.h @@ -0,0 +1,150 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <diagnostic/PrintLogger.h> +#include <drivers/adc/ADS1118/ADS1118.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/spi/SPIBusInterface.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/BMX160/BMX160WithCorrection.h> +#include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/MS5803/MS5803.h> +#include <sensors/SensorData.h> +#include <sensors/SensorManager.h> +#include <sensors/analog/battery/BatteryVoltageSensor.h> +#include <sensors/analog/current/CurrentSensor.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h> + +#include <map> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL.h> +#include <hardware_in_the_loop/HIL_sensors/HILSensors.h> +#endif + +namespace DeathStackBoard +{ + +/** + * @brief Initializes all the sensors on the death stack + * + */ +class Sensors +{ +public: + SensorManager* sensor_manager = nullptr; + + InternalADC* internal_adc = nullptr; + BatteryVoltageSensor* battery_voltage = nullptr; + CurrentSensor* cs_cutter_primary = nullptr; + CurrentSensor* cs_cutter_backup = nullptr; + + MS5803* press_digital = nullptr; + + ADS1118* adc_ads1118 = nullptr; + SSCDRRN015PDA* press_pitot = nullptr; + SSCDANN030PAA* press_dpl_vane = nullptr; + MPXHZ6130A* press_static_port = nullptr; + + BMX160* imu_bmx160 = nullptr; + BMX160WithCorrection* imu_bmx160_with_correction = nullptr; + LIS3MDL* mag_lis3mdl = nullptr; + UbloxGPS* gps_ublox = nullptr; + +#ifdef HARDWARE_IN_THE_LOOP + HILImu* hil_imu = nullptr; + HILBarometer* hil_baro = nullptr; + HILGps* hil_gps = nullptr; +#endif + + Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler); + + ~Sensors(); + + bool start(); + + void calibrate(); + +private: + // PrintLogger log = Logging::getLogger("deathstack.sensors"); + + void internalAdcInit(); + void internalAdcCallback(); + + void batteryVoltageInit(); + void batteryVoltageCallback(); + + void pressDigiInit(); + void pressDigiCallback(); + + void ADS1118Init(); + void ADS1118Callback(); + + void pressPitotInit(); + void pressPitotCallback(); + + void pressDPLVaneInit(); + void pressDPLVaneCallback(); + + void pressStaticInit(); + void pressStaticCallback(); + + void imuBMXInit(); + void imuBMXCallback(); + + void imuBMXWithCorrectionInit(); + void imuBMXWithCorrectionCallback(); + + void magLISinit(); + void magLISCallback(); + + void gpsUbloxInit(); + void gpsUbloxCallback(); + +#ifdef HARDWARE_IN_THE_LOOP + void hilBarometerInit(); + void hilImuInit(); + void hilGpsInit(); + void hilIMUCallback(); + void hilBaroCallback(); + void hilGPSCallback(); +#endif + + void updateSensorsStatus(); + + SPIBusInterface& spi1_bus; + + SensorManager::SensorMap_t sensors_map; + + SensorsStatus status; + + PrintLogger log = Logging::getLogger("sensors"); + + unsigned int battery_critical_counter = 0; +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/SensorManager/Sensors/PiksiData.h b/src/boards/DeathStack/Main/SensorsData.h similarity index 58% rename from src/boards/DeathStack/SensorManager/Sensors/PiksiData.h rename to src/boards/DeathStack/Main/SensorsData.h index 14ba778c96c8f5a384877b84666215ee1d994b22..7ff9615e975deabea4899b6c8ccfb7b4ba7c285c 100644 --- a/src/boards/DeathStack/SensorManager/Sensors/PiksiData.h +++ b/src/boards/DeathStack/Main/SensorsData.h @@ -1,6 +1,6 @@ /** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -23,28 +23,48 @@ #pragma once -#include <drivers/piksi/piksi_data.h> -#include <ostream> - namespace DeathStackBoard { -struct PiksiData + +struct AirSpeedPitot +{ + uint64_t timestamp; + float airspeed; + + static std::string header() { return "timestamp,airspeed\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << airspeed << "\n"; + } +}; + +enum SensorDriverStatus +{ + DRIVER_ERROR = 0, + DRIVER_OK = 1 +}; + +struct SensorsStatus { - GPSData gps_data; - bool fix = false; + uint8_t bmx160 = DRIVER_OK; + uint8_t ms5803 = DRIVER_OK; + uint8_t lis3mdl = DRIVER_OK; + uint8_t gps = DRIVER_OK; + uint8_t internal_adc = DRIVER_OK; + uint8_t ads1118 = DRIVER_OK; static std::string header() { - return "timestamp,lat,lon,alt,vel_n,vel_e,vel_d,speed,num_sat,fix\n"; + return "bmx160,ms5803,lis3mdl,gps,internal_adc,ads1118\n"; } void print(std::ostream& os) const { - os << gps_data.timestamp << "," << gps_data.latitude << "," - << gps_data.longitude << "," << gps_data.height << "," - << gps_data.velocityNorth << "," << gps_data.velocityEast << "," - << gps_data.velocityDown << "," << gps_data.speed << "," - << gps_data.numSatellites << "," << (int)fix << "\n"; + os << (int)bmx160 << "," << (int)ms5803 << "," << (int)lis3mdl << "," + << (int)gps << "," << (int)internal_adc << "," << (int)ads1118 + << "\n"; } }; + } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/StateMachines.cpp b/src/boards/DeathStack/Main/StateMachines.cpp new file mode 100644 index 0000000000000000000000000000000000000000..546687c0b5006ede1369e00a3ddd66e5cbfc02f4 --- /dev/null +++ b/src/boards/DeathStack/Main/StateMachines.cpp @@ -0,0 +1,105 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <AirBrakes/AirBrakesController.h> +#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <Deployment/DeploymentController.h> +#include <FlightModeManager/FMMController.h> +#include <FlightStatsRecorder/FSRController.h> +#include <Main/StateMachines.h> +#include <NavigationAttitudeSystem/NASController.h> +#include <System/TaskID.h> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL.h> +#endif + +namespace DeathStackBoard +{ +StateMachines::StateMachines(IMUType& imu, PressType& press, GPSType& gps, + TaskScheduler* scheduler) +{ + ada_controller = new ADAControllerType(press, gps); + dpl_controller = new DeploymentController(); + nas_controller = new NASControllerType(imu, press, gps); + arb_controller = new AirBrakesControllerType(nas_controller->getNAS()); + fmm = new FMMController(); + +#ifdef HARDWARE_IN_THE_LOOP + HIL::getInstance()->setNAS(&nas_controller->getNAS()); +#endif + + addAlgorithmsToScheduler(scheduler); +} + +StateMachines::~StateMachines() +{ + delete ada_controller; + delete dpl_controller; + delete nas_controller; + delete arb_controller; + delete fmm; +} + +bool StateMachines::start() +{ + return fmm->start() && dpl_controller->start() && ada_controller->start() && + nas_controller->start() && arb_controller->start(); +} + +void StateMachines::addAlgorithmsToScheduler(TaskScheduler* scheduler) +{ + uint64_t start_time = miosix::getTick() + 10; + + scheduler->add(std::bind(&ADAControllerType::update, ada_controller), + ADA_UPDATE_PERIOD, TASK_ADA_ID, start_time); + + scheduler->add(std::bind(&NASControllerType::update, nas_controller), + NAS_UPDATE_PERIOD, TASK_NAS_ID, start_time); + + scheduler->add(std::bind(&AirBrakesControllerType::update, arb_controller), + ABK_UPDATE_PERIOD, TASK_ABK_ID, start_time); +} + +void StateMachines::setReferenceTemperature(float t) +{ + ada_controller->setReferenceTemperature(t); + nas_controller->setReferenceTemperature(t); +} + +void StateMachines::setInitialOrientation(float roll, float pitch, float yaw) +{ + nas_controller->setInitialOrientation(roll, pitch, yaw); +} + +void StateMachines::setReferenceAltitude(float altitude) +{ + ada_controller->setReferenceAltitude(altitude); + nas_controller->setReferenceAltitude(altitude); +} + +void StateMachines::setInitialCoordinates(float latitude, float longitude) +{ + nas_controller->setInitialCoordinates(latitude, longitude); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/StateMachines.h b/src/boards/DeathStack/Main/StateMachines.h new file mode 100644 index 0000000000000000000000000000000000000000..a7184d7728451a3bf9fcf4c4c4a4a7ef6caf2f3c --- /dev/null +++ b/src/boards/DeathStack/Main/StateMachines.h @@ -0,0 +1,106 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <LoggerService/LoggerService.h> +#include <NavigationAttitudeSystem/NASData.h> +#include <drivers/gps/ublox/UbloxGPS.h> +#include <scheduler/TaskScheduler.h> +#include <sensors/BMX160/BMX160WithCorrection.h> +#include <sensors/MS5803/MS5803.h> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL_sensors/HILSensors.h> +#endif + +namespace DeathStackBoard +{ + +class FMMController; +class DeploymentController; + +template <typename Press, typename GPS> +class ADAController; + +template <typename IMU, typename Press, typename GPS> +class NASController; + +template <typename T> +class AirBrakesController; + +class StateMachines +{ +public: +#ifdef HARDWARE_IN_THE_LOOP + using IMUType = HILImu; + using IMUDataType = HILImuData; + + using PressType = HILBarometer; + using PressDataType = HILBaroData; + + using GPSType = HILGps; + using GPSDataType = HILGpsData; +#else + using IMUType = BMX160WithCorrection; + using IMUDataType = BMX160WithCorrectionData; + + using PressType = MS5803; + using PressDataType = MS5803Data; + + using GPSType = UbloxGPS; + using GPSDataType = UbloxGPSData; +#endif + + using ADAControllerType = ADAController<PressDataType, GPSDataType>; + using NASControllerType = + NASController<IMUDataType, PressDataType, GPSDataType>; + using AirBrakesControllerType = AirBrakesController<NASData>; + + DeploymentController* dpl_controller; + FMMController* fmm; + ADAControllerType* ada_controller; + NASControllerType* nas_controller; + AirBrakesControllerType* arb_controller; + + StateMachines(IMUType& imu, PressType& press, GPSType& gps, + TaskScheduler* scheduler); + + ~StateMachines(); + + bool start(); + + void setReferenceTemperature(float t); + + void setInitialOrientation(float roll, float pitch, float yaw); + + void setInitialCoordinates(float latitude, float longitude); + + void setReferenceAltitude(float altitude); + +private: + void addAlgorithmsToScheduler(TaskScheduler* scheduler); + + LoggerService& logger = *(LoggerService::getInstance()); +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/lh_circles_test_data.cpp b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h similarity index 58% rename from src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/lh_circles_test_data.cpp rename to src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h index e7243ed01c133a1215325ab5e2539b6f984ac108..bfdf5e9274f9a605c0be48fccf8854ea182f4c56 100644 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/tests/lh_circles_test_data.cpp +++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h @@ -1,47 +1,53 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Alessandro Del Duca, Luca Conterio + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ - -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.804075 +#pragma once -#include "lh_circles_test_data.h" +#include <Eigen/Dense> -namespace launchhazard -{ -namespace test +namespace DeathStackBoard { -const double test_latitudes[] = {}; -const double test_longitudes[] = {}; +using namespace Eigen; + +// function with 2 vectors as parameters +typedef std::function<MatrixXf(VectorXf, VectorXf)> function_2v; +// function with 1 vector as parameter +typedef std::function<MatrixXf(VectorXf)> function_v; + +struct ExtendedKalmanConfig +{ + uint8_t n; + uint8_t m; + uint8_t p; + Matrix<float, 12, 12> P; + Matrix<float, 6, 6> Q; + Vector<float, 13> x; + function_v h; + function_2v dfdx; + function_v dhdx; +}; -} -} // namespace elevationmap \ No newline at end of file +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1a70e01556be798d64d6f96173176f7a66349914 --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp @@ -0,0 +1,280 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <NavigationAttitudeSystem/ExtendedKalmanEigen.h> + +namespace DeathStackBoard +{ + +using namespace NASConfigs; + +ExtendedKalmanEigen::ExtendedKalmanEigen() +{ + eye6.setIdentity(); + eye4.setIdentity(); + eye3.setIdentity(); + eye2.setIdentity(); + + // clang-format off + + R_bar << SIGMA_BAR; + R_mag << SIGMA_MAG * eye3; + R_gps << eye4 * SIGMA_GPS / SATS_NUM; + Q_mag << (SIGMA_W * SIGMA_W * T + + (1.0F / 3.0F) * SIGMA_BETA * SIGMA_BETA * T * T * T) * + eye3, + (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3, + (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3, + (SIGMA_BETA * SIGMA_BETA * T) * eye3; + + P_pos = eye3 * P_POS; + P_vel = eye3 * P_VEL; + P_att = eye3 * P_ATT; + P_bias = eye3 * P_BIAS; + P << P_pos, MatrixXf::Zero(3, N - 4), MatrixXf::Zero(3, 3), P_vel, + MatrixXf::Zero(3, N - 7), MatrixXf::Zero(3, 6), P_att, + MatrixXf::Zero(3, N - 10), MatrixXf::Zero(3, 9), P_bias; + + Q_pos = eye3 * SIGMA_POS; + Q_vel = eye3 * SIGMA_VEL; + Q_lin << Q_pos, MatrixXf::Zero(3, NL - 3), MatrixXf::Zero(3, 3), Q_vel; + + F << 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, + MatrixXf::Zero(3, NL); + F = eye6 + T * F; + Ftr = F.transpose(); + + H_gps << 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F; + H_gpstr = H_gps.transpose(); + + Fatt << -eye3, -eye3 * T, Matrix3f::Zero(3, 3), eye3; + Fatttr = Fatt.transpose(); + + Gatt << -eye3, Matrix3f::Zero(3, 3), + Matrix3f::Zero(3, 3), eye3; + Gatttr = Gatt.transpose(); + + g << 0.0F, 0.0F, aeroutils::constants::g; + + // clang-format on +} + +void ExtendedKalmanEigen::predict(const Vector3f& u) +{ + Matrix3f A; + Vector3f a; + VectorNf x_dot; + + Plin = P.block<NL, NL>(0, 0); + P.block<NL, NL>(0, 0) = F * Plin * Ftr + Q_lin; + + float q1 = + x(NL); // Since the index begins from 0, the first quaternion + // component will be at index = # linear states before itself + float q2 = x(NL + 1); + float q3 = x(NL + 2); + float q4 = x(NL + 3); + + A << powf(q1, 2) - powf(q2, 2) - powf(q3, 2) + powf(q4, 2), + 2.0F * q1 * q2 + 2.0F * q3 * q4, 2.0F * q1 * q3 - 2.0F * q2 * q4, + 2.0F * q1 * q2 - 2.0F * q3 * q4, + -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2), + 2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4, + 2.0F * q2 * q3 - 2.0F * q1 * q4, + -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2); + + a = A.transpose() * u + + g; // Real accelerometers don't measure g during the flight + + x_dot << x(3), x(4), x(5), a, + MatrixXf::Zero( + NATT, + 1); // The quaternions and the biases don't need to be updated + + x = x + T * x_dot; +} + +void ExtendedKalmanEigen::correctBaro(const float y) +{ + Matrix<float, NL, NBAR> K_bar; + Matrix<float, NBAR, NL> H_bar; + Matrix<float, NBAR, NBAR> S_bar; + + Plin = P.block<NL, NL>(0, 0); + + float temp = aeroutils::mslTemperature(MSL_TEMPERATURE, x(2)); + float p = aeroutils::constants::a * aeroutils::constants::n * MSL_PRESSURE * + powf(1 - aeroutils::constants::a * x(2) / temp, + -aeroutils::constants::n - 1) / + temp; + + H_bar << 0.0F, 0.0F, p, + MatrixXf::Zero(1, N - 3 - NATT); // Gradient of h_bar + + S_bar = H_bar * Plin * H_bar.transpose() + R_bar; + + K_bar = Plin * H_bar.transpose() * S_bar.inverse(); + + P.block<NL, NL>(0, 0) = (eye6 - K_bar * H_bar) * Plin; + + float h_bar = aeroutils::mslPressure(MSL_PRESSURE, MSL_TEMPERATURE, x(2)); + + x.head(NL) = x.head(NL) + K_bar * (y - h_bar); + + // float res_bar = y - h_bar; +} + +void ExtendedKalmanEigen::correctGPS(const Vector4f& y, const uint8_t sats_num) +{ + Matrix<float, NGPS, 1> h_gps; + Matrix<float, NL, NGPS> K_gps; + Matrix<float, NGPS, 1> res_gps; + Matrix<float, NGPS, NGPS> S_gps; + + float xnord = y(0); + float yest = y(1); + float velnord = y(2); + float velest = y(3); + + Matrix<float, NGPS, 1> yned(xnord, yest, velnord, velest); + + R_gps = eye4 * SIGMA_GPS / sqrtf(sats_num); + + Plin = P.block<NL, NL>(0, 0); + + S_gps = H_gps * Plin * H_gpstr + R_gps; + + K_gps = Plin * H_gpstr * S_gps.inverse(); + + P.block<NL, NL>(0, 0) = (eye6 - K_gps * H_gps) * Plin; + + h_gps << x(0), x(1), x(3), x(4); + + x.head(NL) = x.head(NL) + K_gps * (yned - h_gps); + + res_gps = y - h_gps; +} + +const VectorNf& ExtendedKalmanEigen::getState() { return x; } + +void ExtendedKalmanEigen::setX(const VectorNf& x) { this->x = x; } + +/* + MULTIPLICATIVE EXTENDED KALMAN FILTER +*/ +void ExtendedKalmanEigen::predictMEKF(const Vector3f& u) +{ + Vector3f omega; + Vector3f prev_bias; + Matrix4f omega_mat; + Matrix3f omega_submat; + + q << x(NL), x(NL + 1), x(NL + 2), x(NL + 3); + prev_bias << x(NL + 4), x(NL + 5), x(NL + 6); + + omega = u - prev_bias; + + omega_submat << 0.0F, -omega(2), omega(1), omega(2), 0.0F, -omega(0), + -omega(1), omega(0), 0.0F; + + omega_mat << -omega_submat, omega, -omega.transpose(), 0.0F; + + q = (eye4 + 0.5F * omega_mat * T) * q; + q.normalize(); + + x.tail(NATT) << q, prev_bias; + + Patt = P.block<NMEKF, NMEKF>( + NL, NL); // The block of P related to the attitude starts at (NL, NL) + // because the index starts from 0. The block is of size + // NMEKF x NMEKF. + P.block<NMEKF, NMEKF>(NL, NL) = + Fatt * Patt * Fatttr + + Gatt * Q_mag * Gatttr; // Update only the attitude related part of P +} + +void ExtendedKalmanEigen::correctMEKF(const Vector3f& y) +{ + Matrix3f A; + Vector3f z; + Vector4f r; + Matrix3f z_mat; + Vector4f u_att; + Vector3f r_sub; + Matrix<float, NMAG, NMAG> Satt; + Matrix<float, NMAG, NMEKF> Hatt; + Matrix<float, NMEKF, NMAG> Katt; + Matrix<float, NMEKF, 1> delta_x; + + float q1 = x(NL); + float q2 = x(NL + 1); + float q3 = x(NL + 2); + float q4 = x(NL + 3); + + A << powf(q1, 2) - powf(q2, 2) - powf(q3, 2) + powf(q4, 2), + 2.0F * q1 * q2 + 2.0F * q3 * q4, 2.0F * q1 * q3 - 2.0F * q2 * q4, + 2.0F * q1 * q2 - 2.0F * q3 * q4, + -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2), + 2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4, + 2.0F * q2 * q3 - 2.0F * q1 * q4, + -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2); + + z = A * NED_MAG; + + z_mat << 0.0F, -z(2), z(1), z(2), 0.0F, -z(0), -z(1), z(0), 0.0F; + + Hatt << z_mat, Matrix3f::Zero(3, 3); + + Patt = P.block<NMEKF, NMEKF>(NL, NL); + Satt = Hatt * Patt * Hatt.transpose() + R_mag; + + Katt = Patt * Hatt.transpose() * Satt.inverse(); + + delta_x = Katt * (y - z); + + r_sub << delta_x(0), delta_x(1), delta_x(2); + + r << 0.5F * r_sub, sqrtf(1.0F - 0.25F * r_sub.transpose() * r_sub); + + q << q1, q2, q3, q4; + u_att = quater.quatProd(r, q); + float u_norm = u_att.norm(); + + x(NL) = u_att(0) / u_norm; + x(NL + 1) = u_att(1) / u_norm; + x(NL + 2) = u_att(2) / u_norm; + x(NL + 3) = u_att(3) / u_norm; + x(NL + 4) = x(NL + 4) + delta_x(3); + x(NL + 5) = x(NL + 5) + delta_x(4); + x(NL + 6) = x(NL + 6) + delta_x(5); + + P.block<NMEKF, NMEKF>(NL, NL) = + (eye6 - Katt * Hatt) * Patt * (eye6 - Katt * Hatt).transpose() + + Katt * R_mag * Katt.transpose(); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h new file mode 100644 index 0000000000000000000000000000000000000000..ce89a4a91ff55e1b43169490874170670c847bf8 --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h @@ -0,0 +1,135 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <configs/NASConfig.h> +#include <math/SkyQuaternion.h> +#include <utils/aero/AeroUtils.h> + +#include <Eigen/Dense> + +using namespace Eigen; + +namespace DeathStackBoard +{ + +using namespace NASConfigs; + +using VectorNf = Matrix<float, N, 1>; + +class ExtendedKalmanEigen +{ + +public: + ExtendedKalmanEigen(); + + /** + * @brief Prediction step of the EKF. + * + * @param u 3x1 Vector of the accelerometer readings [ax ay az]. + */ + void predict(const Vector3f& u); + + /** + * @brief EKF correction of the barometer data. + * + * @param y Pressure read from the barometer [Pa] + * @param ref_press Pressure at ground level [Pa] + * @param ref_temp Temperature at ground level [Kelvin] + */ + void correctBaro(const float y); + + /** + * @brief EKF correction of the gps readings. + * + * @param y 4x1 Vector of the gps readings [longitude, latitude, + * gps_nord_vel, gps_east_vel]. + * @param sats_num Number of satellites available + */ + void correctGPS(const Vector4f& y, const uint8_t sats_num); + + /** + * @brief Prediction step of the Multiplicative EKF. + * + * @param u 3x1 Vector of the gyroscope readings [wx wy wz]. + */ + void predictMEKF(const Vector3f& u); + + /** + * @brief MEKF correction of the magnetometer readings. + * + * @param y 3x1 Vector of the magnetometer readings [mx my mz]. + */ + void correctMEKF(const Vector3f& y); + + /** + * @return 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz]. + */ + const VectorNf& getState(); + + /** + * @param x 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz]. + */ + void setX(const VectorNf& x); + +private: + VectorNf x; + Matrix<float, NP, NP> P; + Matrix<float, NL, NL> F; + Matrix<float, NL, NL> Ftr; + + Matrix3f P_pos; + Matrix3f P_vel; + Matrix3f P_att; + Matrix3f P_bias; + Matrix<float, NL, NL> Plin; + + Matrix3f Q_pos; + Matrix3f Q_vel; + Matrix<float, NL, NL> Q_lin; + + Vector3f g; + Matrix2f eye2; + Matrix3f eye3; + Matrix4f eye4; + Matrix<float, 6, 6> eye6; + + Matrix<float, NBAR, NBAR> R_bar; + + Matrix<float, NGPS, NGPS> R_gps; + Matrix<float, NGPS, NL> H_gps; + Matrix<float, NL, NGPS> H_gpstr; + + Vector4f q; + Matrix<float, NMAG, NMAG> R_mag; + Matrix<float, NMEKF, NMEKF> Q_mag; + Matrix<float, NMEKF, NMEKF> Fatt; + Matrix<float, NMEKF, NMEKF> Fatttr; + Matrix<float, NMEKF, NMEKF> Gatt; + Matrix<float, NMEKF, NMEKF> Gatttr; + Matrix<float, NMEKF, NMEKF> Patt; + + SkyQuaternion quater; +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h new file mode 100644 index 0000000000000000000000000000000000000000..3cfd264a72902dd7f6acc2ca5e4e559b32e08773 --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h @@ -0,0 +1,200 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/** + * INITIALIZATION OF ATTITUDE, NOT TO USE WHILE THE ROCKET IS FLYING + * ecompass reference: https://de.mathworks.com/help/fusion/ref/ecompass.html + * triad reference: + * https://www.air.iitb.ac.in/satelliteWiki/index.php/Triad_Algorithm + */ + +#pragma once + +#include <Common.h> +#include <configs/NASConfig.h> +#include <diagnostic/PrintLogger.h> +#include <math/SkyQuaternion.h> +#include <utils/aero/AeroUtils.h> + +#include <Eigen/Dense> + +using namespace Eigen; + +namespace DeathStackBoard +{ + +using namespace NASConfigs; + +class InitStates +{ + +public: + InitStates(); + + /** + * @brief ecompass algorithm to estimate the attitude before the liftoff. + * + * @param acc 3x1 accelerometer readings [ax ay az]. + * @param mag 3x1 magnetometer readings [mx my mz]. + * + */ + void eCompass(const Vector3f acc, const Vector3f mag); + + /** + * @brief triad algorithm to estimate the attitude before the liftoff. + * + * @param acc 3x1 accelerometer readings [ax ay az]. + * @param mag 3x1 magnetometer readings [mx my mz]. + */ + const Vector3f& triad(Vector3f& acc, Vector3f& mag); + + /** + * @brief Initialization of the positions before the liftoff. + * @param press Reference pressure [Pa]. + */ + void positionInit(const float press); //, const float temp); + /** + * @brief Initialization of the velocities before the liftoff. + * + */ + void velocityInit(); + + /** + * @brief Initialization of the biases before the liftoff. + * + */ + void biasInit(); + + Matrix<float, N, 1> getInitX(); + +private: + Matrix<float, N, 1> x_init; + Matrix3f R, Rb, Ri; + SkyQuaternion quat; + Vector4f x_quat; + Vector3f eul; + + PrintLogger log = Logging::getLogger("deathstack.nas.initstates"); +}; + +InitStates::InitStates() { x_init << MatrixXf::Zero(N, 1); } + +void InitStates::eCompass(const Vector3f acc, const Vector3f mag) +{ + // ndr: since this method runs only when the rocket is stationary, there's + // no need to add the gravity vector because the accelerometers already + // measure it. This is not true if we consider the flying rocket. + + Vector3f am(acc.cross(mag)); + + R << am.cross(acc), am, acc; + R.col(0).normalize(); + R.col(1).normalize(); + R.col(2).normalize(); + + x_quat = quat.rotm2quat(R); + eul = quat.quat2eul(x_quat); + + x_init(NL) = x_quat(0); + x_init(NL + 1) = x_quat(1); + x_init(NL + 2) = x_quat(2); + x_init(NL + 3) = x_quat(3); +} + +const Vector3f& InitStates::triad(Vector3f& acc, Vector3f& mag) +{ + LOG_DEBUG(log, "Executing TRIAD"); + + // The coulmns of the the triad matrices. b:body, i:inertial + Vector3f t1b, t2b, t3b, t1i, t2i, t3i; + + // vettore gravità "vero" in NED, normalizzato : + // -1 su asse "down", perché da fermo l'accelerometro + // misura la reazione vincolare (rivolta verso l'alto) + Vector3f g_norm(0.0F, 0.0F, -1.0F); + + acc.normalize(); + Vector3f w1 = acc; + mag.normalize(); + Vector3f w2 = mag; + + Vector3f v1 = g_norm; + Vector3f v2 = NED_MAG; + + Vector3f Ou1 = w1; + Vector3f Ou2 = w1.cross(w2); + Ou2.normalize(); + Vector3f Ou3 = Ou2.cross(Ou1); + + Vector3f R1 = v1; + Vector3f R2 = v1.cross(v2); + R2.normalize(); + Vector3f R3 = R2.cross(R1); + + Matrix3f Mou; + Mou << Ou1, Ou2, Ou3; + + Matrix3f Mr; + Mr << R1, R2, R3; + + R = Mr * Mou.transpose(); + + x_quat = quat.rotm2quat(R); + + eul = quat.quat2eul(x_quat); + + x_init(NL) = x_quat(0); + x_init(NL + 1) = x_quat(1); + x_init(NL + 2) = x_quat(2); + x_init(NL + 3) = x_quat(3); + + return eul; +} + +void InitStates::positionInit(const float press) +{ + x_init(0) = 0.0; + x_init(1) = 0.0; + x_init(2) = -aeroutils::relAltitude( + press, MSL_PRESSURE, + MSL_TEMPERATURE); // msl altitude (in NED, so negative) +} + +void InitStates::velocityInit() +{ + x_init(3) = 0.0F; + x_init(4) = 0.0F; + x_init(5) = 0.0F; +} + +void InitStates::biasInit() +{ + // gyro biases set to zero since the sensor performs + // self-calibration and the measurements are already compensated + x_init(NL + 4) = 0.0F; + x_init(NL + 5) = 0.0F; + x_init(NL + 6) = 0.0F; +} + +Matrix<float, N, 1> InitStates::getInitX() { return x_init; } + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h new file mode 100644 index 0000000000000000000000000000000000000000..f943ee2664bb080f28bf64110ebc533cdd9248ab --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h @@ -0,0 +1,429 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/* STATE VECTOR: px + * py + * pz + * vx + * vy + * vz + * x = qx + * qy + * qz + * qw + * bx + * by + * bz + */ + +#pragma once + +#include <NavigationAttitudeSystem/ExtendedKalmanEigen.h> +#include <NavigationAttitudeSystem/InitStates.h> +#include <NavigationAttitudeSystem/NASData.h> +#include <TimestampTimer.h> +#include <diagnostic/PrintLogger.h> +#include <math/SkyQuaternion.h> +#include <sensors/Sensor.h> + +namespace DeathStackBoard +{ + +using namespace NASConfigs; + +template <typename IMU, typename Press, typename GPS> +class NAS : public Sensor<NASData> +{ + +public: + NAS(Sensor<IMU>& imu, Sensor<Press>& baro, Sensor<GPS>& gps); + + /** + * @brief Initialization of the state vector before the liftoff + * + * @return boolean indicating if the operation succeded or not + */ + bool init(); + + /** + * @brief Self-test here does nothing. + */ + bool selfTest(); + + /** + * @brief Samples the sensors' readings, filters them through the EKF + * and computes the state vector + * + * @return Struct with the states used by the airbrakes subsystem + */ + NASData sampleImpl(); + + /** + * @return Struct containing initial positions and orientation estimated + * using the triad algorithm. + */ + NASTriadResult getTriadResult(); + + /** + * @return Struct containing the kalman state. + */ + NASKalmanState getKalmanState(); + + /** + * @param ref_v Struct of NASReferenceValues to be set + */ + void setReferenceValues(const NASReferenceValues& ref_v); + + const NASReferenceValues& getReferenceValues(); + + void resetReferenceValues(); + + void setInitialOrientation(float roll, float pitch, float yaw); + +private: + /** + * @brief Copy the kalman state information in the NASData output struct. + */ + void updateNASData(); + + /** + * @brief Convert GPS coordinates to NED frame. + */ + Vector3f geodetic2NED(const Vector3f& gps_data); + + SkyQuaternion quat; /**< Auxiliary functions for quaternions */ + + Matrix<float, N, 1> x; /**< Kalman state vector */ + + NASData nas_data; + + Sensor<IMU>& imu; + Sensor<Press>& barometer; + Sensor<GPS>& gps; + + ExtendedKalmanEigen filter; + InitStates states_init; + NASReferenceValues ref_values; + Vector3f triad_result_eul; + + uint64_t last_gps_timestamp = 0; + uint64_t last_accel_timestamp = 0; + uint64_t last_gyro_timestamp = 0; + uint64_t last_mag_timestamp = 0; + uint64_t last_press_timestamp = 0; + + bool initialized = false; + + PrintLogger log = Logging::getLogger("deathstack.fsm.nas"); + +#ifdef DEBUG + unsigned int counter = 0; +#endif +}; + +template <typename IMU, typename Press, typename GPS> +NAS<IMU, Press, GPS>::NAS(Sensor<IMU>& imu, Sensor<Press>& baro, + Sensor<GPS>& gps) + : imu(imu), barometer(baro), gps(gps) +{ + x = Matrix<float, N, 1>::Zero(); +} + +template <typename IMU, typename Press, typename GPS> +bool NAS<IMU, Press, GPS>::init() +{ + states_init.positionInit(ref_values.ref_pressure); + + states_init.velocityInit(); + + Vector3f acc_init(ref_values.ref_accel_x, ref_values.ref_accel_y, + ref_values.ref_accel_z); + Vector3f mag_init(ref_values.ref_mag_x, ref_values.ref_mag_y, + ref_values.ref_mag_z); + + triad_result_eul = states_init.triad(acc_init, mag_init); + + states_init.biasInit(); + + x = states_init.getInitX(); + + filter.setX(x); + + updateNASData(); + +#ifdef DEBUG + Vector4f qua(x(6), x(7), x(8), x(9)); + Vector3f e = quat.quat2eul(qua); + + LOG_DEBUG( + log, + "Init state vector: \n px: {:.2f} \n py: {:.2f} \n pz: {:.2f} \n vx: " + "{:.2f} \n " + "vy: {:.2f} \n vz: {:.2f} \n roll: {:.2f} " + "\n pitch: {:.2f} \n yaw: {:.2f}", + x(0), x(1), x(2), x(3), x(4), x(5), e(0), e(1), e(2)); + +#endif + + initialized = true; + + LoggerService::getInstance()->log(getTriadResult()); + + return initialized; +} + +template <typename IMU, typename Press, typename GPS> +bool NAS<IMU, Press, GPS>::selfTest() +{ + return true; +} + +template <typename IMU, typename Press, typename GPS> +NASData NAS<IMU, Press, GPS>::sampleImpl() +{ + if (!initialized) + { + return NASData{}; + } + + IMU imu_data = imu.getLastSample(); + GPS gps_data = gps.getLastSample(); + Press pressure_data = barometer.getLastSample(); + + // update ekf with new accel and gyro measures + if (imu_data.accel_timestamp != last_accel_timestamp && + imu_data.gyro_timestamp != last_gyro_timestamp) + { + last_accel_timestamp = imu_data.accel_timestamp; + last_gyro_timestamp = imu_data.gyro_timestamp; + + Vector3f accel_readings(imu_data.accel_x, imu_data.accel_y, + imu_data.accel_z); + filter.predict(accel_readings); + + Vector3f gyro_readings(imu_data.gyro_x, imu_data.gyro_y, + imu_data.gyro_z); + filter.predictMEKF(gyro_readings); + } + + // check if new pressure data is available + if (pressure_data.press_timestamp != last_press_timestamp) + { + last_press_timestamp = pressure_data.press_timestamp; + + filter.correctBaro(pressure_data.press); + } + + // check if new gps data is available and the gps has fix + if (gps_data.gps_timestamp != last_gps_timestamp && gps_data.fix == true) + { + last_gps_timestamp = gps_data.gps_timestamp; + + // float delta_lon = gps_data.longitude - ref_values.ref_longitude; + // float delta_lat = gps_data.latitude - ref_values.ref_latitude; + + // Vector4f gps_readings(delta_lon, delta_lat, gps_data.velocity_north, + // gps_data.velocity_east); + + Vector3f gps_readings(gps_data.latitude, gps_data.longitude, + gps_data.height); + Vector3f gps_ned = geodetic2NED(gps_readings); + + Vector4f pos_vel(gps_ned(0), gps_ned(1), gps_data.velocity_north, + gps_data.velocity_east); + filter.correctGPS(pos_vel, gps_data.num_satellites); + } + + // check if new magnetometer data is available + if (imu_data.mag_timestamp != last_mag_timestamp) + { + Vector3f mag_readings(imu_data.mag_x, imu_data.mag_y, imu_data.mag_z); + + if (mag_readings.norm() < EMF * JAMMING_FACTOR) + { + last_mag_timestamp = imu_data.mag_timestamp; + + mag_readings.normalize(); + filter.correctMEKF(mag_readings); + } + } + + // update states + x = filter.getState(); + + updateNASData(); + +#ifdef DEBUG + if (counter == 50) + { + // TRACE("[NAS] x(2) : %.2f - z_init : %.2f \n", x(2), z_init); + LOG_DEBUG(log, "z : {:.2f} - vz : {:.2f} - vMod : {:.2f}", nas_data.z, + nas_data.vz, nas_data.vMod); + + counter = 0; + + /*Vector4f qua(x(6), x(7), x(8), x(9)); + Vector3f e = quat.quat2eul(qua); + + LOG_DEBUG(log, + "State vector: \n px: {:.2f} \n py: {:.2f} \n pz: {:.2f} \n vx: + {:.2f} \n " "vy: {:.2f} \n vz: {:.2f} \n roll: {:.2f} \n pitch: {:.2f} + \n yaw: {:.2f} \n " "q1: {:.2f} \n q2: {:.2f} \n q3: {:.2f} \n q4 : + {:.2f} ", x(0), x(1), x(2), x(3), x(4), x(5), e(0), e(1), e(2), x(6), + x(7), x(8), x(9));*/ + } + else + { + counter++; + } +#endif + + return nas_data; +} + +template <typename IMU, typename Press, typename GPS> +NASTriadResult NAS<IMU, Press, GPS>::getTriadResult() +{ + Matrix<float, N, 1> state = states_init.getInitX(); + // Vector3f e = quat.quat2eul({state(6), state(7), state(8), state(9)}); + + NASTriadResult result; + result.x = state(0); + result.y = state(1); + result.z = -state(2); + result.roll = triad_result_eul(0); // e(0); + result.pitch = triad_result_eul(1); // e(1); + result.yaw = triad_result_eul(2); // e(2); + + return result; +} + +template <typename IMU, typename Press, typename GPS> +NASKalmanState NAS<IMU, Press, GPS>::getKalmanState() +{ + return NASKalmanState{nas_data.timestamp, x}; +} + +template <typename IMU, typename Press, typename GPS> +void NAS<IMU, Press, GPS>::setReferenceValues(const NASReferenceValues& ref_v) +{ + this->ref_values = ref_v; +} + +template <typename IMU, typename Press, typename GPS> +const NASReferenceValues& NAS<IMU, Press, GPS>::getReferenceValues() +{ + return ref_values; +} + +template <typename IMU, typename Press, typename GPS> +void NAS<IMU, Press, GPS>::resetReferenceValues() +{ + ref_values = NASReferenceValues{}; +} + +template <typename IMU, typename Press, typename GPS> +void NAS<IMU, Press, GPS>::setInitialOrientation(float roll, float pitch, + float yaw) +{ + Vector4f q = quat.eul2quat({yaw, pitch, roll}); + x(NL) = q(0); + x(NL + 1) = q(1); + x(NL + 2) = q(2); + x(NL + 3) = q(3); + LOG_INFO(log, "Initial orientation set to : ({:.2f}, {:.2f}, {:.2f})", roll, + pitch, yaw); + LOG_DEBUG(log, + "State vector: \n px: {:.2f} \n py: {:.2f} \n pz: {:.2f} \n vx: " + "{:.2f} \n " + "vy: {:.2f} \n vz: {:.2f} \n roll: {:.2f} \n pitch: {:.2f} \n " + "yaw: {:.2f} \n " + "q1: {:.2f} \n q2: {:.2f} \n q3: {:.2f} \n q4 : {:.2f}", + x(0), x(1), x(2), x(3), x(4), x(5), roll, pitch, yaw, x(6), x(7), + x(8), x(9)); +} + +template <typename IMU, typename Press, typename GPS> +void NAS<IMU, Press, GPS>::updateNASData() +{ + nas_data.timestamp = TimestampTimer::getTimestamp(); + + nas_data.x = x(0); + nas_data.y = x(1); + nas_data.z = + -x(2) - + ref_values + .ref_altitude; // Negative sign because we're working in the NED + // frame but we want a positive altitude as output. + nas_data.vx = x(3); + nas_data.vy = x(4); + nas_data.vz = -x(5); + nas_data.vMod = + sqrtf(nas_data.vx * nas_data.vx + nas_data.vy * nas_data.vy + + nas_data.vz * nas_data.vz); +} + +template <typename IMU, typename Press, typename GPS> +Vector3f NAS<IMU, Press, GPS>::geodetic2NED(const Vector3f& gps_data) +{ + const float a = 6378137; // [m] + const float a2 = pow(a, 2); // [m^2] + const float b2 = pow(6356752.3142, 2); // [m^2] + const float e2 = 1 - b2 / a2; + + float lat0 = ref_values.ref_latitude * DEGREES_TO_RADIANS; + float lon0 = ref_values.ref_longitude * DEGREES_TO_RADIANS; + float lat = gps_data(0) * DEGREES_TO_RADIANS; + float lon = gps_data(1) * DEGREES_TO_RADIANS; + float h = gps_data(2); + + float s1 = sin(lat0); + float c1 = cos(lat0); + float s2 = sin(lat); + float c2 = cos(lat); + float p1 = c1 * cos(lon0); + float p2 = c2 * cos(lon); + float q1 = c1 * sin(lon0); + float q2 = c2 * sin(lon); + float w1 = 1 / sqrt(1 - e2 * pow(s1, 2)); + float w2 = 1 / sqrt(1 - e2 * pow(s2, 2)); + float delta_x = + a * (p2 * w2 - p1 * w1) + (h * p2 - ref_values.ref_altitude * p1); + float delta_y = + a * (q2 * w2 - q1 * w1) + (h * q2 - ref_values.ref_altitude * q1); + float delta_z = (1 - e2) * a * (s2 * w2 - s1 * w1) + + (h * s2 - ref_values.ref_altitude * s1); + + // positions in ENU (east, north, up) frame + float p_east = -sin(lon0) * delta_x + cos(lon0) * delta_y; + float p_north = -sin(lat0) * cos(lon0) * delta_x - + sin(lat0) * sin(lon0) * delta_y + cos(lat0) * delta_z; + float p_up = cos(lat0) * cos(lon0) * delta_x + + cos(lat0) * sin(lon0) * delta_y + sin(lat0) * delta_z; + + // positions in NED frame + Vector3f p_ned(p_north, p_east, -p_up); + + return p_ned; +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.scxml b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.scxml new file mode 100644 index 0000000000000000000000000000000000000000..c0fe156785412a48bf23eb1b426805cb2eb30dc6 --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.scxml @@ -0,0 +1,18 @@ +<?xml version="1.0" encoding="UTF-8"?> +<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="idle" version="1.0"> + <state id="idle"> + <transition event="FLIGHT_EVENTS.EV_CALIBRATE" target="triad"/> + </state> + <state id="calibrating"> + <transition event="NAS.EV_CALIBRATE_NAS" target="triad"/> + <transition event="NAS.EV_NAS_READY" target="ready"/> + </state> + <state id="ready"> + <transition event="FLIGHT_EVENTS.EV_LIFTOFF" target="active"/> + <transition event="NAS.EV_CALIBRATE_NAS" target="triad"/> + </state> + <state id="active"> + <transition event="FLIGHT_EVENTS.EV_LANDED" target="end"/> + </state> + <state id="end"/> +</scxml> diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4ca3bdf0da8852f014ae6c9e0114a1ab9f1318fd --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp @@ -0,0 +1,124 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <NavigationAttitudeSystem/NASCalibrator.h> + +namespace DeathStackBoard +{ + +NASCalibrator::NASCalibrator(const uint32_t n_samples = 1000) + : n_samples(n_samples) +{ +} + +void NASCalibrator::addBaroSample(float p) { pressure_stats.add(p); } + +void NASCalibrator::addGPSSample(float lat, float lon) +{ + gps_lat_stats.add(lat); + gps_lon_stats.add(lon); +} + +void NASCalibrator::addAccelSample(float x, float y, float z) +{ + accel_x_stats.add(x); + accel_y_stats.add(y); + accel_z_stats.add(z); +} + +void NASCalibrator::addMagSample(float x, float y, float z) +{ + mag_x_stats.add(x); + mag_y_stats.add(y); + mag_z_stats.add(z); +} + +void NASCalibrator::setReferenceTemperature(float t) +{ + ref_values.ref_temperature = t; + ref_temperature_set = true; +} + +void NASCalibrator::setReferenceAltitude(float alt) +{ + ref_values.ref_altitude = alt; + ref_altitude_set = true; +} + +void NASCalibrator::setReferenceCoordinates(float lat, float lon) +{ + ref_values.ref_latitude = lat; + ref_values.ref_longitude = lon; + ref_coordinates_set = true; +} + +bool NASCalibrator::calibIsComplete() +{ + // Calibration is complete if enough samples were collected + return pressure_stats.getStats().nSamples >= n_samples && + // either enough gps samples or coordinates set via TC + (gps_lat_stats.getStats().nSamples >= n_samples || + ref_coordinates_set == true) && + ref_altitude_set == true && + accel_x_stats.getStats().nSamples >= n_samples && + mag_x_stats.getStats().nSamples >= n_samples && + ref_temperature_set == true; +} + +void NASCalibrator::reset() +{ + pressure_stats.reset(); + gps_lat_stats.reset(); + gps_lon_stats.reset(); + accel_x_stats.reset(); + accel_y_stats.reset(); + accel_z_stats.reset(); + mag_x_stats.reset(); + mag_y_stats.reset(); + mag_z_stats.reset(); +} + +NASReferenceValues NASCalibrator::getReferenceValues() +{ + // If calibration is not complete use default values + if (calibIsComplete()) + { + ref_values.ref_pressure = pressure_stats.getStats().mean; + + if (!ref_coordinates_set) + { + ref_values.ref_latitude = gps_lat_stats.getStats().mean; + ref_values.ref_longitude = gps_lon_stats.getStats().mean; + } + + ref_values.ref_accel_x = accel_x_stats.getStats().mean; + ref_values.ref_accel_y = accel_y_stats.getStats().mean; + ref_values.ref_accel_z = accel_z_stats.getStats().mean; + ref_values.ref_mag_x = mag_x_stats.getStats().mean; + ref_values.ref_mag_y = mag_y_stats.getStats().mean; + ref_values.ref_mag_z = mag_z_stats.getStats().mean; + } + + return ref_values; +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h new file mode 100644 index 0000000000000000000000000000000000000000..7a115db5a850bba090181daf612e876014e8ecbb --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h @@ -0,0 +1,107 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Common.h> +#include <Debug.h> +#include <NavigationAttitudeSystem/NASData.h> +#include <math/Stats.h> +#include <miosix.h> + +namespace DeathStackBoard +{ + +class NASCalibrator +{ +public: + NASCalibrator(const uint32_t n_samples); + + bool calibIsComplete(); + + /** + * @brief Adds a pressure sample to the stats + */ + void addBaroSample(float p); + + /** + * @brief Adds a GPS sample to the stats + */ + void addGPSSample(float lat, float lon); + + /** + * @brief Adds an acceleration sample to the stats + */ + void addAccelSample(float x, float y, float z); + + /** + * @brief Adds an acceleration sample to the stats + */ + void addMagSample(float x, float y, float z); + + /** + * @brief Set temperature of the launch location + */ + void setReferenceTemperature(float t); + + /** + * @brief Set latitude/longitude of the launchpad + */ + void setReferenceCoordinates(float lat, float lon); + + /** + * @brief Set latitude/longitude of the launchpad + */ + void setReferenceAltitude(float alt); + + /** + * @brief Reset stats + */ + void reset(); + + NASReferenceValues getReferenceValues(); + +private: + const uint32_t n_samples; + + NASReferenceValues ref_values{}; + + Stats pressure_stats; // Computes mean std dev etc for calibration + + Stats gps_lat_stats; + Stats gps_lon_stats; + + Stats accel_x_stats; + Stats accel_y_stats; + Stats accel_z_stats; + + Stats mag_x_stats; + Stats mag_y_stats; + Stats mag_z_stats; + + // Refernece flags + bool ref_coordinates_set = false; + bool ref_altitude_set = false; + bool ref_temperature_set = false; +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h new file mode 100644 index 0000000000000000000000000000000000000000..a0ad2176f6be281b712ef0e6e5142152c1af7720 --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h @@ -0,0 +1,474 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <LoggerService/LoggerService.h> +#include <NavigationAttitudeSystem/NAS.h> +#include <NavigationAttitudeSystem/NASCalibrator.h> +#include <NavigationAttitudeSystem/NASData.h> +#include <System/StackLogger.h> +#include <diagnostic/PrintLogger.h> +#include <events/Events.h> +#include <events/FSM.h> +#include <miosix.h> +#include <sensors/Sensor.h> + +using miosix::FastMutex; +using miosix::Lock; + +namespace DeathStackBoard +{ + +using namespace NASConfigs; + +/** + * @brief Navigatioin system state machine + */ +template <typename IMU, typename Press, typename GPS> +class NASController : public FSM<NASController<IMU, Press, GPS>> +{ + using NASCtrl = NASController<IMU, Press, GPS>; + using NASFsm = FSM<NASCtrl>; + + static_assert( + checkIfProduces<Sensor<IMU>, AccelerometerData>::value, + "Template argument must be a sensor that produces accelerometer data."); + static_assert( + checkIfProduces<Sensor<IMU>, GyroscopeData>::value, + "Template argument must be a sensor that produces gyroscope data."); + static_assert( + checkIfProduces<Sensor<IMU>, MagnetometerData>::value, + "Template argument must be a sensor that produces magnetometer data."); + static_assert( + checkIfProduces<Sensor<Press>, PressureData>::value, + "Template argument must be a sensor that produces pressure data."); + static_assert(checkIfProduces<Sensor<GPS>, GPSData>::value, + "Template argument must be a sensor that produces GPS data."); + +public: + NASController(Sensor<IMU>& imu, Sensor<Press>& baro, Sensor<GPS>& gps); + ~NASController(); + + void state_idle(const Event& ev); + void state_calibrating(const Event& ev); + void state_ready(const Event& ev); + void state_active(const Event& ev); + void state_end(const Event& ev); + + void setReferenceTemperature(float t); + void setInitialOrientation(float roll, float pitch, float yaw); + void setInitialCoordinates(float latitude, float longitude); + void setReferenceAltitude(float altitude); + + void update(); + + Sensor<NASData>& getNAS() { return nas; } + +private: + void finalizeCalibration(); + + /** + * @brief Update and log NAS FSM state + */ + void logStatus(NASState state); + void logData(); + + NASStatus status; + + FastMutex mutex; + NASCalibrator calibrator; + + Sensor<IMU>& imu; + Sensor<Press>& barometer; + Sensor<GPS>& gps; + + NAS<IMU, Press, GPS> nas; + + LoggerService& logger; + PrintLogger log = Logging::getLogger("deathstack.fsm.nas"); + + uint64_t last_gps_timestamp = 0; + uint64_t last_accel_timestamp = 0; + uint64_t last_mag_timestamp = 0; + uint64_t last_press_timestamp = 0; +}; + +template <typename IMU, typename Press, typename GPS> +NASController<IMU, Press, GPS>::NASController(Sensor<IMU>& imu, + Sensor<Press>& baro, + Sensor<GPS>& gps) + : NASFsm(&NASCtrl::state_idle), calibrator(CALIBRATION_N_SAMPLES), imu(imu), + barometer(baro), gps(gps), nas(imu, baro, gps), + logger(*(LoggerService::getInstance())) +{ + memset(&status, 0, sizeof(NASStatus)); + sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker->subscribe(this, TOPIC_NAS); + + status.state = NASState::IDLE; +} + +template <typename IMU, typename Press, typename GPS> +NASController<IMU, Press, GPS>::~NASController() +{ + sEventBroker->unsubscribe(this); +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::update() +{ + IMU imu_data = imu.getLastSample(); + Press press_data = barometer.getLastSample(); + GPS gps_data = gps.getLastSample(); + + NASState state = status.state; + + switch (state) + { + case NASState::IDLE: + { + break; + } + case NASState::CALIBRATING: + { + bool end_calib = false; + { + Lock<FastMutex> l(mutex); + + if (imu_data.accel_timestamp != last_accel_timestamp) + { + last_accel_timestamp = imu_data.accel_timestamp; + calibrator.addAccelSample( + imu_data.accel_x, imu_data.accel_y, imu_data.accel_z); + } + + if (imu_data.mag_timestamp != last_mag_timestamp) + { + last_mag_timestamp = imu_data.mag_timestamp; + calibrator.addMagSample(imu_data.mag_x, imu_data.mag_y, + imu_data.mag_z); + } + + // Add samples to the calibration + if (press_data.press_timestamp != last_press_timestamp) + { + last_press_timestamp = press_data.press_timestamp; + calibrator.addBaroSample(press_data.press); + } + + if (gps_data.fix == true && + gps_data.gps_timestamp != last_gps_timestamp) + { + last_gps_timestamp = gps_data.gps_timestamp; + calibrator.addGPSSample(gps_data.latitude, + gps_data.longitude); + } + + // Save the state of calibration to release mutex + end_calib = calibrator.calibIsComplete(); + } + + if (end_calib) + { + // If samples are enough + finalizeCalibration(); + } + + break; + } + case NASState::READY: + { + break; + } + case NASState::ACTIVE: + { + nas.sample(); + logData(); + break; + } + case NASState::END: + { + break; + } + default: + { + break; + } + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::finalizeCalibration() +{ + Lock<FastMutex> l(mutex); + + if (calibrator.calibIsComplete() && + nas.getReferenceValues() != calibrator.getReferenceValues()) + { + nas.setReferenceValues(calibrator.getReferenceValues()); + + // initialize NAS and execute TRIAD + nas.init(); + + // Log all the data after init, reference values, kalman state and also + // the triad result (with euler angles) + logger.log(calibrator.getReferenceValues()); + logger.log(nas.getTriadResult()); + logData(); + + LOG_INFO(log, "Finalized calibration and TRIAD"); + + sEventBroker->post({EV_NAS_READY}, TOPIC_NAS); + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::state_idle(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state idle"); + logStatus(NASState::IDLE); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state idle"); + break; + } + case EV_CALIBRATE_NAS: + { + this->transition(&NASCtrl::state_calibrating); + break; + } + default: + { + break; + } + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::state_calibrating(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state calibrating"); + logStatus(NASState::CALIBRATING); + + { + Lock<FastMutex> l(mutex); + calibrator.reset(); + nas.resetReferenceValues(); + } + + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state calibrating"); + break; + } + case EV_CALIBRATE_NAS: + { + this->transition(&NASCtrl::state_calibrating); + break; + } + case EV_NAS_READY: + { + this->transition(&NASCtrl::state_ready); + break; + } + default: + { + break; + } + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::state_ready(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state ready"); + logStatus(NASState::READY); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state ready"); + break; + } + case EV_LIFTOFF: + { + this->transition(&NASCtrl::state_active); + break; + } + case EV_CALIBRATE_NAS: + { + this->transition(&NASCtrl::state_calibrating); + break; + } + default: + { + break; + } + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::state_active(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state active"); + logStatus(NASState::ACTIVE); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state active"); + break; + } + case EV_LANDED: + { + this->transition(&NASCtrl::state_end); + break; + } + default: + { + break; + } + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::state_end(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + LOG_DEBUG(log, "Entering state end"); + logStatus(NASState::END); + break; + } + case EV_EXIT: + { + LOG_DEBUG(log, "Exiting state end"); + break; + } + + default: + { + break; + } + } +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::setReferenceTemperature(float t) +{ + if (status.state == NASState::CALIBRATING || + status.state == NASState::READY) + { + Lock<FastMutex> l(mutex); + calibrator.setReferenceTemperature(t); + logger.log(calibrator.getReferenceValues()); + } + + finalizeCalibration(); +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::setInitialOrientation(float roll, + float pitch, + float yaw) +{ + if (status.state == NASState::READY) + { + nas.setInitialOrientation(roll, pitch, yaw); + logData(); + } + + finalizeCalibration(); +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::setInitialCoordinates(float latitude, + float longitude) +{ + if (status.state == NASState::CALIBRATING || + status.state == NASState::READY) + { + Lock<FastMutex> l(mutex); + calibrator.setReferenceCoordinates(latitude, longitude); + logData(); + } + + finalizeCalibration(); +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::setReferenceAltitude(float altitude) +{ + if (status.state == NASState::CALIBRATING || + status.state == NASState::READY) + { + Lock<FastMutex> l(mutex); + calibrator.setReferenceAltitude(altitude); + logData(); + } + + finalizeCalibration(); +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::logStatus(NASState state) +{ + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + logger.log(status); + + StackLogger::getInstance()->updateStack(THID_NAS_FSM); +} + +template <typename IMU, typename Press, typename GPS> +void NASController<IMU, Press, GPS>::logData() +{ + NASKalmanState kalman_state = nas.getKalmanState(); + kalman_state.timestamp = TimestampTimer::getTimestamp(); + logger.log(kalman_state); + logger.log(nas.getLastSample()); +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h new file mode 100644 index 0000000000000000000000000000000000000000..105c3da296d6b6e70b68c43c6f2d8cf63c74459c --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h @@ -0,0 +1,223 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <configs/NASConfig.h> +#include <math/SkyQuaternion.h> +#include <stdint.h> + +#include <iostream> +#include <string> + +namespace DeathStackBoard +{ + +/** + * @brief Enum defining the possibile FSM states. + */ +enum class NASState : uint8_t +{ + IDLE = 0, + CALIBRATING, + READY, + ACTIVE, + END, +}; + +/** + * @brief Structure defining the overall NAS status. + */ +struct NASStatus +{ + uint64_t timestamp; + NASState state; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +/** + * @brief NavigationSystem output, used by the airbrakes algorithm. + */ +struct NASData +{ + uint64_t timestamp = 0; + float x = 0; + float y = 0; + float z = 0; + float vx = 0; + float vy = 0; + float vz = 0; + float vMod = 0; + + static std::string header() { return "timestamp,x,y,z,vx,vy,vz,vMod\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << x << "," << y << "," << z << "," << vx << "," + << vy << "," << vz << "," << vMod << "," + << "\n"; + } +}; + +/** + * @brief Struct to log the Kalman states. + */ +struct NASKalmanState +{ + uint64_t timestamp = 0; + float x0 = 0; + float x1 = 0; + float x2 = 0; + float x3 = 0; + float x4 = 0; + float x5 = 0; + float x6 = 0; + float x7 = 0; + float x8 = 0; + float x9 = 0; + float x10 = 0; + float x11 = 0; + float x12 = 0; + + NASKalmanState() {} + + NASKalmanState(uint64_t t, const Matrix<float, NASConfigs::N, 1>& state) + { + timestamp = t; + x0 = state(0); + x1 = state(1); + x2 = state(2); + x3 = state(3); + x4 = state(4); + x5 = state(5); + x6 = state(6); + x7 = state(7); + x8 = state(8); + x9 = state(9); + x10 = state(10); + x11 = state(11); + x12 = state(12); + } + + static std::string header() + { + return "timestamp,x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << x0 << "," << x1 << "," << x2 << "," << x3 + << "," << x4 << "," << x5 << "," << x6 << "," << x7 << "," << x8 + << "," << x9 << "," << x10 << "," << x11 << "," << x12 << "\n"; + } + + Vector3f toEul() const + { + SkyQuaternion q; + return q.quat2eul({x6, x7, x8, x9}); + } +}; + +/** + * @brief Struct to log reference values. + * + * Also used in NAS to store the values. + */ +struct NASReferenceValues +{ + float ref_pressure; + + float ref_temperature; + + float ref_altitude; + float ref_latitude; + float ref_longitude; + + float ref_accel_x; + float ref_accel_y; + float ref_accel_z; + + float ref_mag_x; + float ref_mag_y; + float ref_mag_z; + + static std::string header() + { + return "ref_pressure,ref_temperature,ref_altitude,ref_latitude,ref_" + "longitude,ref_" + "accel_x,ref_accel_y,ref_accel_z,ref_mag_x,ref_mag_y,ref_mag_" + "z\n"; + } + + void print(std::ostream& os) const + { + os << ref_pressure << "," << ref_temperature << "," << ref_altitude + << "," << ref_latitude << "," << ref_longitude << "," << ref_accel_x + << "," << ref_accel_y << "," << ref_accel_z << "," << ref_mag_x + << "," << ref_mag_y << "," << ref_mag_z << "\n"; + } + + bool operator==(const NASReferenceValues& other) const + { + return ref_pressure == other.ref_pressure && + ref_temperature == other.ref_temperature && + ref_altitude == other.ref_altitude && + ref_latitude == other.ref_latitude && + ref_longitude == other.ref_longitude && + ref_accel_x == other.ref_accel_x && + ref_accel_y == other.ref_accel_y && + ref_accel_z == other.ref_accel_z && + ref_mag_x == other.ref_mag_x && ref_mag_y == other.ref_mag_y && + ref_mag_z == other.ref_mag_z; + } + + bool operator!=(const NASReferenceValues& other) const + { + return !(*this == other); + } +}; + +struct NASTriadResult +{ + float x; + float y; + float z; + float roll; + float pitch; + float yaw; + + static std::string header() { return "x,y,z,roll,pitch,yaw\n"; } + + void print(std::ostream& os) const + { + os << x << "," << y << "," << z << "," << roll << "," << pitch << "," + << yaw << "\n"; + } +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/readme.md b/src/boards/DeathStack/NavigationAttitudeSystem/readme.md new file mode 100644 index 0000000000000000000000000000000000000000..7aa33f5f50058fa37a565db11268e421977f8364 --- /dev/null +++ b/src/boards/DeathStack/NavigationAttitudeSystem/readme.md @@ -0,0 +1,9 @@ +```mermaid +stateDiagram-v2 + [*] --> idle + idle --> triad : EV_CALIBRATE_NAS + triad --> ready : EV_NAS_READY + ready --> active : EV_LIFTOFF + ready --> triad : EV_CALIBRATE_NAS + active --> end : EV_LANDED +``` \ No newline at end of file diff --git a/src/boards/DeathStack/PinHandler/PinHandler.cpp b/src/boards/DeathStack/PinHandler/PinHandler.cpp index dbaa00624e8236db6f0ca00a1603c091241c9502..68fbafd5972ef56e7b034cdc9c35b1681bedc740 100644 --- a/src/boards/DeathStack/PinHandler/PinHandler.cpp +++ b/src/boards/DeathStack/PinHandler/PinHandler.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,19 +13,24 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include "PinHandler.h" +#include <LoggerService/LoggerService.h> +#include <PinHandler/PinHandler.h> +#include <diagnostic/PrintLogger.h> #include <events/EventBroker.h> +#include <events/Events.h> + #include <functional> -#include "DeathStack/events/Events.h" -#include "DeathStack/LoggerService/LoggerService.h" -#include "Debug.h" + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL.h> +#endif using std::bind; @@ -46,9 +50,9 @@ PinHandler::PinHandler() PinObserver::OnStateChangeCallback launch_statechange_cb = bind(&PinHandler::onLaunchPinStateChange, this, _1, _2, _3); - pin_obs.observePin(PORT_LAUNCH_PIN, NUM_LAUNCH_PIN, TRIGGER_LAUNCH_PIN, - launch_transition_cb, THRESHOLD_LAUNCH_PIN, - launch_statechange_cb); + pin_obs.observePin(launch_pin.getPort(), launch_pin.getNumber(), + TRIGGER_LAUNCH_PIN, launch_transition_cb, + THRESHOLD_LAUNCH_PIN, launch_statechange_cb); // Noseconse pin callbacks registration PinObserver::OnTransitionCallback nc_transition_cb = @@ -57,30 +61,35 @@ PinHandler::PinHandler() PinObserver::OnStateChangeCallback nc_statechange_cb = bind(&PinHandler::onNCPinStateChange, this, _1, _2, _3); - pin_obs.observePin(PORT_NC_DETACH_PIN, NUM_NC_DETACH_PIN, + pin_obs.observePin(nosecone_pin.getPort(), nosecone_pin.getNumber(), TRIGGER_NC_DETACH_PIN, nc_transition_cb, THRESHOLD_NC_DETACH_PIN, nc_statechange_cb); - // Motor pin callbacks registration - PinObserver::OnTransitionCallback motor_transition_cb = - bind(&PinHandler::onMotorPinTransition, this, _1, _2); + // Dpl servo pin callbacks registration + PinObserver::OnTransitionCallback dpl_transition_cb = + bind(&PinHandler::onDPLServoPinTransition, this, _1, _2); - PinObserver::OnStateChangeCallback motor_statechange_cb = - bind(&PinHandler::onMotorPinStateChange, this, _1, _2, _3); - + PinObserver::OnStateChangeCallback dpl_statechange_cb = + bind(&PinHandler::onDPLServoPinStateChange, this, _1, _2, _3); - pin_obs.observePin(PORT_MOTOR_PIN, NUM_MOTOR_PIN, TRIGGER_MOTOR_PIN, - motor_transition_cb, THRESHOLD_MOTOR_PIN, - motor_statechange_cb); + pin_obs.observePin(dpl_servo_pin.getPort(), dpl_servo_pin.getNumber(), + TRIGGER_DPL_SERVO_PIN, dpl_transition_cb, + THRESHOLD_DPL_SERVO_PIN, dpl_statechange_cb); } void PinHandler::onLaunchPinTransition(unsigned int p, unsigned char n) { UNUSED(p); UNUSED(n); + +#ifdef HARDWARE_IN_THE_LOOP + HIL::getInstance()->signalLiftoff(); +#endif sEventBroker->post(Event{EV_UMBILICAL_DETACHED}, TOPIC_FLIGHT_EVENTS); - status_pin_launch.last_detection_time = miosix::getTick(); + LOG_INFO(log, "Launch pin detached!"); + + status_pin_launch.last_detection_time = TimestampTimer::getTimestamp(); logger->log(status_pin_launch); } @@ -90,52 +99,75 @@ void PinHandler::onNCPinTransition(unsigned int p, unsigned char n) UNUSED(n); sEventBroker->post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS); - status_pin_nosecone.last_detection_time = miosix::getTick(); + LOG_INFO(log, "Nosecone detached!"); + + status_pin_nosecone.last_detection_time = TimestampTimer::getTimestamp(); logger->log(status_pin_nosecone); } -void PinHandler::onMotorPinTransition(unsigned int p, unsigned char n) +void PinHandler::onDPLServoPinTransition(unsigned int p, unsigned char n) { UNUSED(p); UNUSED(n); - - status_pin_motor.last_detection_time = miosix::getTick(); - logger->log(status_pin_motor); + + LOG_INFO(log, "Deployment servo actuated!"); + + // do not post any event, just log the timestamp + status_pin_dpl_servo.last_detection_time = TimestampTimer::getTimestamp(); + logger->log(status_pin_dpl_servo); } void PinHandler::onLaunchPinStateChange(unsigned int p, unsigned char n, - int state) + int state) { UNUSED(p); UNUSED(n); status_pin_launch.state = (uint8_t)state; - status_pin_launch.last_state_change = miosix::getTick(); + status_pin_launch.last_state_change = TimestampTimer::getTimestamp(); status_pin_launch.num_state_changes += 1; - logger->log(status_pin_launch); + + LOG_INFO(log, + "Launch pin state change at time {}: new " + "state = {}", + status_pin_launch.last_state_change, status_pin_launch.state); + + logger->log(status_pin_launch); } -void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, - int state) +void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state) { UNUSED(p); UNUSED(n); status_pin_nosecone.state = (uint8_t)state; - status_pin_nosecone.last_state_change = miosix::getTick(); + status_pin_nosecone.last_state_change = TimestampTimer::getTimestamp(); status_pin_nosecone.num_state_changes += 1; + + LOG_INFO(log, "Nosecone pin state change at time {}: new state = {}", + status_pin_nosecone.last_state_change, status_pin_nosecone.state); + logger->log(status_pin_nosecone); } -void PinHandler::onMotorPinStateChange(unsigned int p, unsigned char n, - int state) +void PinHandler::onDPLServoPinStateChange(unsigned int p, unsigned char n, + int state) { UNUSED(p); UNUSED(n); - status_pin_motor.state = (uint8_t)state; - status_pin_motor.last_state_change = miosix::getTick(); - status_pin_motor.num_state_changes += 1; - logger->log(status_pin_motor); + + status_pin_dpl_servo.state = (uint8_t)state; + status_pin_dpl_servo.last_state_change = TimestampTimer::getTimestamp(); + status_pin_dpl_servo.num_state_changes += 1; + + LOG_INFO(log, + "Deployment servo pin state change at time {}: " + "new " + "state = {}", + status_pin_dpl_servo.last_state_change, + status_pin_dpl_servo.state); + + logger->log(status_pin_dpl_servo); } } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/PinHandler/PinHandler.h b/src/boards/DeathStack/PinHandler/PinHandler.h index bcd0d2a93b4e50df025d0c48701160610b101dde..0d853757a4540941e97d038e55fc357e3d5e4ae8 100644 --- a/src/boards/DeathStack/PinHandler/PinHandler.h +++ b/src/boards/DeathStack/PinHandler/PinHandler.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -23,20 +22,23 @@ #pragma once +#include <PinHandler/PinHandlerData.h> +#include <configs/PinObserverConfig.h> +#include <diagnostic/PrintLogger.h> #include <utils/PinObserver.h> -#include "DeathStack/configs/PinObserverConfig.h" -#include "PinHandlerData.h" namespace DeathStackBoard { -//Forward dec +/** + * @brief Forward dec. + */ class LoggerService; - /** * @brief This class contains the handlers for both the launch pin (umbilical) * and the nosecone detachment pin. + * * It uses boardcore's PinObserver to bind these functions to the GPIO pins. * The handlers post an event on the EventBroker. */ @@ -46,22 +48,16 @@ public: PinHandler(); /** - * @brief Starts the pin observer + * @brief Starts the pin observer. * */ - bool start() - { - return pin_obs.start(); - } + bool start() { return pin_obs.start(); } /** - * @brief Stops the pin observer + * @brief Stops the pin observer. * */ - void stop() - { - pin_obs.stop(); - } + void stop() { pin_obs.stop(); } /** * @brief Function called by the pinobserver when a launch pin detachment is @@ -82,27 +78,27 @@ public: void onNCPinTransition(unsigned int p, unsigned char n); /** - * @brief Function called by the pinobserver when a motor pin detachment - * is detected. + * @brief Function called by the pinobserver when a deployment servo + * actuation is detected via the optical sensor. * * @param p * @param n */ - void onMotorPinTransition(unsigned int p, unsigned char n); + void onDPLServoPinTransition(unsigned int p, unsigned char n); void onLaunchPinStateChange(unsigned int p, unsigned char n, int state); void onNCPinStateChange(unsigned int p, unsigned char n, int state); - void onMotorPinStateChange(unsigned int p, unsigned char n, int state); - + void onDPLServoPinStateChange(unsigned int p, unsigned char n, int state); private: PinStatus status_pin_launch{ObservedPin::LAUNCH}; PinStatus status_pin_nosecone{ObservedPin::NOSECONE}; - PinStatus status_pin_motor{ObservedPin::MOTOR}; + PinStatus status_pin_dpl_servo{ObservedPin::DPL_SERVO}; PinObserver pin_obs; LoggerService* logger; + PrintLogger log = Logging::getLogger("deathstack.pinhandler"); }; } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/PinHandler/PinHandlerData.h b/src/boards/DeathStack/PinHandler/PinHandlerData.h index ccc78f135d43ba3f8c7e948749db35a0cdd2cc04..d3aa62143b3e69dee859a4b7de791294c2e93281 100644 --- a/src/boards/DeathStack/PinHandler/PinHandlerData.h +++ b/src/boards/DeathStack/PinHandler/PinHandlerData.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,14 +13,13 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - #pragma once #include <cstdint> @@ -32,28 +30,28 @@ namespace DeathStackBoard enum class ObservedPin : uint8_t { - LAUNCH = 0, - NOSECONE = 1, - MOTOR = 2 + LAUNCH = 0, + NOSECONE = 1, + DPL_SERVO = 2 }; /** - * @brief Struct represeting the status of an observed pin + * @brief Struct represeting the status of an observed pin. * */ struct PinStatus { ObservedPin pin; - long long last_state_change = 0; // Last time the pin changed state - uint8_t state = 0; // Current state of the pin + uint64_t last_state_change = 0; // Last time the pin changed state + uint8_t state = 0; // Current state of the pin unsigned int num_state_changes = 0; - long long last_detection_time = 0; // When a transition is detected + uint64_t last_detection_time = 0; // When a transition is detected - PinStatus() {}; + PinStatus(){}; PinStatus(ObservedPin pin) : pin(pin) {} - + static std::string header() { return "pin,last_state_change,state,num_state_changes,last_detection_" diff --git a/src/boards/DeathStack/SensorManager/SensorManager.cpp b/src/boards/DeathStack/SensorManager/SensorManager.cpp deleted file mode 100644 index 5c8acf9620f5ec3a563e879353d84d83affeb8c3..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/SensorManager.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <stdexcept> - -#include "SensorManager.h" - -#include "DeathStack/System/StackLogger.h" -#include "DeathStack/events/Events.h" -#include "DeathStack/events/Topics.h" -#include "Sensors/Test/TestSensor.h" -#include "events/EventBroker.h" - -#include <math/Stats.h> -#include <iostream> - -#include "SensorManagerData.h" -#include "Sensors/AD7994Wrapper.h" -#include "Sensors/ADCWrapper.h" -#include "Sensors/PiksiData.h" -#include "drivers/piksi/piksi.h" -#include "sensors/ADIS16405/ADIS16405.h" -#include "sensors/LM75B.h" -#include "sensors/MPU9250/MPU9250.h" -#include "sensors/MPU9250/MPU9250Data.h" -#include "sensors/MS580301BA07/MS580301BA07.h" - -#ifdef USE_MOCK_SENSORS -#include "Sensors/Test/MockGPS.h" -#include "Sensors/Test/MockPressureSensor.h" -#endif - -#include "Debug.h" - -#include "interfaces-impl/hwmapping.h" - -using miosix::FastMutex; -using miosix::Lock; - -using namespace miosix; - -namespace DeathStackBoard -{ - -SensorManager::SensorManager(ADAController* ada_controller) - : FSM(&SensorManager::stateIdle), scheduler(), - logger(*LoggerService::getInstance()), ada_controller(ada_controller) -{ - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_TC); - - memset(&sensor_status, 0, sizeof(sensor_status)); - - initSensors(); - initSamplers(); - - initScheduler(); -} - -SensorManager::~SensorManager() -{ - sEventBroker->unsubscribe(this); - scheduler.stop(); -} - -bool SensorManager::start() -{ - // Start the parent FSM - bool ok = FSM<SensorManager>::start(); - - // Start the scheduler - ok = ok && scheduler.start(); - - return ok; -} - -void SensorManager::initSensors() -{ - i2c1::init(); - spiMPU9250::init(); - spiMS5803::init(); - - // Instantiation - adc_ad7994 = new AD7994Wrapper(sensors::ad7994::addr, AD7994_V_REF); - temp_lm75b_analog = new LM75BType(sensors::lm75b_analog::addr); - temp_lm75b_imu = new LM75BType(sensors::lm75b_imu::addr); - pressure_ms5803 = new MS580301BA07Type(); - - imu_mpu9250 = - new MPU9250Type(MPU9250Type::ACC_FS_16G, MPU9250Type::GYRO_FS_2000); - - // imu_adis16405 = new ADIS16405Type(ADIS16405Type::GYRO_FS_300); - adc_internal = new ADCWrapper(); - - piksi = new Piksi("/dev/gps"); - -#ifdef USE_MOCK_SENSORS - mock_pressure_sensor = new MockPressureSensor(); - mock_gps = new MockGPS(); -#endif - - // Some sensors dont have init or self tests - sensor_status.piksi = 1; - - // Initialization - TRACE("Mpu init\n"); - sensor_status.mpu9250 = imu_mpu9250->init(); - - // sensor_status.adis = imu_adis16405->init(); - TRACE("LM75b IMU init\n"); - - sensor_status.lm75b_imu = temp_lm75b_imu->init(); - - TRACE("LM75b ANAL init\n"); - - sensor_status.lm75b_analog = temp_lm75b_analog->init(); - - // // TODO: lsm6ds3h - TRACE("MS5803 init\n"); - - sensor_status.ms5803 = pressure_ms5803->init(); - TRACE("AD7994 init\n"); - - sensor_status.ad7994 = adc_ad7994->init(); - - sensor_status.battery_sensor = adc_internal->getBatterySensorPtr()->init(); - sensor_status.current_sensor = adc_internal->getCurrentSensorPtr()->init(); - - status.sensor_status = sensor_status.toNumeric(); - - TRACE("[SM] Sensor init done\n"); -} - -void SensorManager::initSamplers() -{ - sampler_20hz_simple.AddSensor(adc_internal->getBatterySensorPtr()); - sampler_20hz_simple.AddSensor(adc_ad7994); - sampler_20hz_simple.AddSensor(adc_internal->getCurrentSensorPtr()); - - sampler_20hz_simple.AddSensor(temp_lm75b_imu); - sampler_20hz_simple.AddSensor(temp_lm75b_analog); - - sampler_50hz_simple.AddSensor(pressure_ms5803); - - sampler_250hz_simple.AddSensor(imu_mpu9250); - - // Piksi does not inherit from Sensor, so we sample it in a different way - - TRACE("[SM] Sampler init done\n"); -} - -void SensorManager::initScheduler() -{ - /* - * std::bind syntax: - * std::bind(&MyClass::someFunction, &myclass_instance, [someFunction args]) - */ - long long start_time = miosix::getTick() + 10; - - // 250 Hz sensor sampler - std::function<void()> simple_250hz_callback = - std::bind(&SensorManager::onSimple250HZCallback, this); - std::function<void()> simple_250hz_sampler = - std::bind(&SimpleSensorSampler::UpdateAndCallback, - &sampler_250hz_simple, simple_250hz_callback); - - scheduler.add(simple_250hz_sampler, 2, - static_cast<uint8_t>(SensorSamplerId::SIMPLE_250HZ), - start_time); - - // 100 Hz Calback ( MPU Magnetometer ) - std::function<void()> simple_100hz_callback = - std::bind(&SensorManager::onSimple100HZCallback, this); - - scheduler.add(simple_100hz_callback, 10, - static_cast<uint8_t>(SensorSamplerId::MPU_MAGN_100HZ), - start_time); - - // 50 Hz sensor sampler - std::function<void()> simple_50hz_callback = - std::bind(&SensorManager::onSimple50HZCallback, this); - std::function<void()> simple_50hz_sampler = - std::bind(&SimpleSensorSampler::UpdateAndCallback, &sampler_50hz_simple, - simple_50hz_callback); - - scheduler.add(simple_50hz_sampler, 20, - static_cast<uint8_t>(SensorSamplerId::SIMPLE_50HZ), - start_time); - - // 20 Hz sensor sampler - std::function<void()> simple_20hz_callback = - std::bind(&SensorManager::onSimple20HZCallback, this); - std::function<void()> simple_20hz_sampler = - std::bind(&SimpleSensorSampler::UpdateAndCallback, &sampler_20hz_simple, - simple_20hz_callback); - - scheduler.add(simple_20hz_sampler, 50, - static_cast<uint8_t>(SensorSamplerId::SIMPLE_20HZ), - start_time); - - // Lambda expression to collect data from GPS at 10 Hz - std::function<void()> gps_callback = - std::bind(&SensorManager::onGPSCallback, this); - - scheduler.add(gps_callback, 100, static_cast<uint8_t>(SensorSamplerId::GPS), - start_time); - - // Lambda expression callback to log scheduler stats, at 1 Hz - scheduler.add( - [&]() { - scheduler_stats = scheduler.getTaskStats(); - - for (TaskStatResult stat : scheduler_stats) - logger.log(stat); - - StackLogger::getInstance()->updateStack(THID_SENSOR_SAMPLER); - }, - 1000, static_cast<uint8_t>(SensorSamplerId::STATS), start_time); - - TRACE("[SM] Scheduler initialization complete\n"); -} - -void SensorManager::stateIdle(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - enable_sensor_logging = false; - - status.timestamp = miosix::getTick(); - status.state = SensorManagerState::IDLE; - logger.log(status); - - TRACE("[SM] Entering stateIdle\n"); - - StackLogger::getInstance()->updateStack(THID_SENSOR_MANAGER); - - break; - case EV_EXIT: - TRACE("[SM] Exiting stateIdle\n"); - - break; - - // Perform the transition in both cases - case EV_TC_START_SENSOR_LOGGING: - case EV_ARMED: - transition(&SensorManager::stateLogging); - break; - - default: - break; - } -} - -void SensorManager::stateLogging(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - enable_sensor_logging = true; - status.timestamp = miosix::getTick(); - status.state = SensorManagerState::LOGGING; - logger.log(status); - - TRACE("[SM] Entering stateLogging\n"); - - StackLogger::getInstance()->updateStack(THID_SENSOR_MANAGER); - - break; - case EV_EXIT: - - TRACE("[SM] Exiting stateLogging\n"); - - break; -#ifdef USE_MOCK_SENSORS - // Signal to the mock pressure sensor that we have liftoff in order - // to start simulating flight pressures - case EV_LIFTOFF: - mock_pressure_sensor->before_liftoff = false; - mock_gps->before_liftoff = false; - break; -#endif - // Go back to idle in both cases - case EV_TC_STOP_SENSOR_LOGGING: - case EV_LANDED: - transition(&SensorManager::stateIdle); - break; - - default: - break; - } -} - -void SensorManager::onSimple20HZCallback() -{ - AD7994WrapperData* ad7994_data = adc_ad7994->getDataPtr(); - LM75BData temp_data; - temp_data.timestamp = miosix::getTick(); - temp_data.temp_analog = temp_lm75b_analog->getTemp(); - temp_data.temp_imu = temp_lm75b_imu->getTemp(); - -#ifdef USE_MOCK_SENSORS - ad7994_data->nxp_baro_pressure = mock_pressure_sensor->getPressure(); -#endif - - if (enable_sensor_logging) - { - logger.log(*(adc_internal->getBatterySensorPtr()->getBatteryDataPtr())); - logger.log(*(adc_internal->getCurrentSensorPtr()->getCurrentDataPtr())); - logger.log(*(ad7994_data)); - - logger.log(temp_data); - } - - ada_controller->updateBaro(ad7994_data->nxp_baro_pressure); -} - -void SensorManager::onSimple50HZCallback() -{ - if (enable_sensor_logging) - { - // Since sampling both temps & pressure on the ms5803 takes two calls of - // onSimpleUpdate(), log only once every 2 - if (pressure_ms5803->getState() == - MS580301BA07Type::STATE_SAMPLED_PRESSURE) - { - logger.log(pressure_ms5803->getData()); - } - } -} - -void SensorManager::onSimple100HZCallback() -{ - imu_mpu9250->updateMagneto(); - - // Don't log here, magnetometer data will be logged in the 250 hz task with - // the accel & gyro data. -} - -void SensorManager::onSimple250HZCallback() -{ - MPU9250Data mpu9255_data{miosix::getTick(), *(imu_mpu9250->accelDataPtr()), - *(imu_mpu9250->gyroDataPtr()), - *(imu_mpu9250->compassDataPtr()), - *(imu_mpu9250->tempDataPtr())}; - - // Correct bias - mpu9255_data.accel = Vec3(mpu9255_data.accel.getX() + OFFSET_MPU_ACC_X, - mpu9255_data.accel.getY() + OFFSET_MPU_ACC_Y, - mpu9255_data.accel.getZ() + OFFSET_MPU_ACC_Z); - - if (enable_sensor_logging) - { - logger.log(mpu9255_data); - } - - ada_controller->updateAcc(mpu9255_data.accel.getZ()); -} - -void SensorManager::onGPSCallback() -{ - PiksiData data; - memset(&data, 0, sizeof(data)); - - // try - // { - // data.gps_data = piksi->getGpsData(); - - // long long fix_age = getTick() - data.gps_data.timestamp; - // // We have fix if the GPS sample is not too old and the number of - // // satellites is at least 4 - // data.fix = - // fix_age <= MAX_GPS_FIX_AGE && data.gps_data.numSatellites >= 4; - - // last_gps_timestamp = data.gps_data.timestamp; - // } - // catch (std::runtime_error rterr) - // { - // data.gps_data.timestamp = miosix::getTick(); - // data.fix = false; - // } - -#ifdef USE_MOCK_SENSORS - mock_gps->updateCoordinates(); - data.gps_data.timestamp = miosix::getTick(); - data.gps_data.latitude = mock_gps->lat; - data.gps_data.longitude = mock_gps->lon; - data.fix = mock_gps->fix; -#endif - - ada_controller->updateGPS(data.gps_data.latitude, data.gps_data.longitude, - data.fix); - - if (enable_sensor_logging) - { - logger.log(data); - } -} - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/SensorManager/SensorManager.h b/src/boards/DeathStack/SensorManager/SensorManager.h deleted file mode 100644 index 47efd0c86d1db0682081f5efe86fe44ea97c4d0a..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/SensorManager.h +++ /dev/null @@ -1,180 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <vector> -#include "Common.h" - -#include <math/Stats.h> -#include <scheduler/TaskScheduler.h> -#include <sensors/SensorSampling.h> - -#include "DeathStack/LoggerService/LoggerService.h" -#include "DeathStack/configs/SensorManagerConfig.h" -#include "events/FSM.h" - -#include "DeathStack/ADA/ADAController.h" -#include "SensorManagerData.h" - -#include <interfaces-impl/hwmapping.h> - -using miosix::PauseKernelLock; -using std::vector; - -// Forward declarations -template <typename ProtocolSPI> -class MPU9250; - -template <typename ProtocolSPI, typename RST> -class ADIS16405; - -template <typename ProtocolI2C> -class LM75B; - -template <typename ProtocolSPI> -class MS580301BA07; - -class Piksi; - -namespace DeathStackBoard -{ -class ADCWrapper; -class AD7994Wrapper; - -#ifdef USE_MOCK_SENSORS -class MockPressureSensor; -class MockGPS; -#endif - -// Type definitions -typedef MPU9250<spiMPU9250> MPU9250Type; -typedef ADIS16405<spiADIS16405, miosix::sensors::adis16405::rst> ADIS16405Type; -typedef MS580301BA07<spiMS5803> MS580301BA07Type; - -typedef LM75B<i2c1> LM75BType; -/** - * The SensorManager class manages all the sensors connected to the Homeone - * Board. - * - * Sensors are grouped by "type" (Simple or DMA) and "sample frequency" and - * grouped in various SensorSampler objects. These objects are then added to the - * scheduler that manages the timings for the sampling. - * After a SensorSampler has finished sampling its sensors, it will call a - * callback, where these samples can be processed and dispatched. - */ -class SensorManager : public FSM<SensorManager> -{ -public: - SensorManager(ADAController* ada); - ~SensorManager(); - - vector<TaskStatResult> getSchedulerStats() { return scheduler_stats; } - - SensorManagerStatus getStatus() { return status; } - - bool start() override; - -private: - /** - * Initialize all the sensors. - */ - void initSensors(); - - /** - * Initialize the samplers. - */ - void initSamplers(); - - /** - * @brief Sensor manager state machine entry state - * - */ - void stateIdle(const Event& ev); - - /** - * @brief Sensor manager state machine sampling state - * - */ - void stateLogging(const Event& ev); - - /** - * Adds all the SensorSamplers to the scheduler and begins sampling. - */ - void initScheduler(); - - /* - * Callbacks. These functions are called each time the corresponding - * SensorSampler has acquired new samples. - * These functions are called on the scheduler (sampler) thread, so avoid - * performing non-critical and intensive tasks. - */ - - /** - * Simple, 20 Hz SensorSampler Callback. - * Called each time all the sensors in the 20hz sampler have been sampled - */ - void onSimple20HZCallback(); // ADCs - void onSimple50HZCallback(); // MS5803 - void onSimple100HZCallback(); // Mpu magneto - void onSimple250HZCallback(); // Mpu accel & gyro - - void onGPSCallback(); - - TaskScheduler scheduler; - // Logger ref - LoggerService& logger; - - bool enable_sensor_logging = false; - - // Sensor samplers - SimpleSensorSampler sampler_20hz_simple; - SimpleSensorSampler sampler_50hz_simple; - SimpleSensorSampler sampler_250hz_simple; - - // Sensors - AD7994Wrapper* adc_ad7994; - MPU9250Type* imu_mpu9250; - ADIS16405Type* imu_adis16405; - LM75BType* temp_lm75b_imu; - LM75BType* temp_lm75b_analog; - ADCWrapper* adc_internal; - MS580301BA07Type* pressure_ms5803; -#ifdef USE_MOCK_SENSORS - MockPressureSensor* mock_pressure_sensor; - MockGPS* mock_gps; -#endif - - Piksi* piksi; - long long last_gps_timestamp = 0; - - // ADA - ADAController* ada_controller; - - // Stats & status - vector<TaskStatResult> scheduler_stats; - - SensorManagerStatus status; - SensorStatus sensor_status; -}; - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/SensorManager/SensorManagerData.h b/src/boards/DeathStack/SensorManager/SensorManagerData.h deleted file mode 100644 index ff16775d3154f89b1a6f466fd8c2aec23ec21fcf..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/SensorManagerData.h +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <math/Stats.h> -#include <cstdint> -#include <cstring> -#include <ostream> - -namespace DeathStackBoard -{ - -enum class SensorSamplerId : uint8_t -{ - STATS = 0, - GPS = 10, - SIMPLE_20HZ = 20, - SIMPLE_50HZ = 50, - MPU_MAGN_100HZ = 100, - SIMPLE_250HZ = 250 -}; - -enum class TempSensorId : uint8_t -{ - LM75B_ANALOG = 0, - LM75B_IMU = 1 -}; - -enum class SensorManagerState : uint8_t -{ - IDLE, - LOGGING -}; - -// Nominal value returned by SensorStatus.toNumeric() when every sensor was -// initialized successfully. -// 255 = 1 in every bit of the SensorStatus struct -static constexpr uint16_t NOMINAL_SENSOR_INIT_VALUE = 255; - -struct SensorStatus -{ - // Imus - uint16_t mpu9250 : 1; - - // Temperature sensors - uint16_t lm75b_imu : 1; - uint16_t lm75b_analog : 1; - - // GPS - uint16_t piksi : 1; - - // Internal ADC - uint16_t current_sensor : 1; - uint16_t battery_sensor : 1; - - // External ADC - uint16_t ad7994 : 1; - - // Digital pressure sensor - uint16_t ms5803 : 1; - - /** - * Converts data in the struct to a single uint16_t value - * @return uint16_t representing the struct - */ - uint16_t toNumeric() - { - uint16_t out; - memcpy(&out, this, sizeof(out)); - return out; - } - - static std::string header() - { - return "mpu9250,lm75b_imu,lm75b_analog,piksi,current_sensor,battery_" - "sensor,ad7994, ms5803\n"; - } - - void print(std::ostream& os) const - { - os << mpu9250 << "," << lm75b_imu << "," << lm75b_analog << "," << piksi - << "," << current_sensor << "," << battery_sensor << "," << ad7994 - << "," << ms5803 << "\n"; - } -}; - -struct LM75BData -{ - long long timestamp; - float temp_imu; - float temp_analog; - - static std::string header() { return "timestamp,temp_imu,temp_analog\n"; } - - void print(std::ostream& os) const - { - os << timestamp << "," << temp_imu << "," << temp_analog << "\n"; - } -}; - -struct SensorManagerStatus -{ - uint64_t timestamp; - SensorManagerState state; - - uint16_t sensor_status; - static std::string header() { return "timestamp,state,sensor_status\n"; } - - void print(std::ostream& os) const - { - os << timestamp << "," << (int)state << "," << sensor_status << "\n"; - } -}; - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/SensorManager/Sensors/AD7994Wrapper.h b/src/boards/DeathStack/SensorManager/Sensors/AD7994Wrapper.h deleted file mode 100644 index 13eafdbb11fd3972ac26c5361526ec1c2fdd676c..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/Sensors/AD7994Wrapper.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include "AD7994WrapperData.h" -#include "DeathStack/configs/SensorManagerConfig.h" -#include "drivers/adc/AD7994.h" -#include "drivers/adc/AD7994Data.h" - -namespace DeathStackBoard -{ - -using AD7994_t = AD7994<i2c1, ad7994_busy_pin, ad7994_nconvst>; - -class AD7994Wrapper : public ::Sensor -{ - -public: - AD7994Wrapper(uint8_t i2c_address, float v_ref) : adc(i2c_address), v_ref(v_ref) {} - - bool init() override - { - bool success = adc.init(); - if (success) - { - adc.enableChannel(NXP_BARO_CHANNEL); - adc.enableChannel(HONEYWELL_BARO_CHANNEL); - } - return success; - } - - bool onSimpleUpdate() override - { - bool result = adc.onSimpleUpdate(); - if (result) - { - AD7994Sample nxp_baro = adc.getLastSample(NXP_BARO_CHANNEL); - AD7994Sample hw_baro = adc.getLastSample(HONEYWELL_BARO_CHANNEL); - - data.timestamp = nxp_baro.timestamp; - - data.nxp_baro_volt = nxp_baro.value; - data.nxp_baro_flag = nxp_baro.alert_flag; - - data.honeywell_baro_volt = hw_baro.value; - data.honeywell_baro_flag = hw_baro.alert_flag; - - data.nxp_baro_pressure = nxpRaw2Pressure(data.nxp_baro_volt); - data.honeywell_baro_pressure = - hwRaw2Pressure(data.honeywell_baro_volt); - } - return result; - } - - bool selfTest() override { return adc.selfTest(); } - - AD7994WrapperData* getDataPtr() { return &data; } - -private: - float nxpRaw2Pressure(uint16_t raw_val) - { - return (raw_val * v_ref / (4096 * 5.0f) + 0.07739f) * 1000 / - 0.007826f; - } - float hwRaw2Pressure(uint16_t raw_val) - { - return (1.25f * raw_val * v_ref / (4096 * 5.0f) - 0.125f) * - 100000.0f; - } - - AD7994_t adc; - AD7994WrapperData data; - float v_ref; - - static constexpr AD7994_t::Channel NXP_BARO_CHANNEL = - static_cast<AD7994_t::Channel>(AD7994_NXP_BARO_CHANNEL); - - static constexpr AD7994_t::Channel HONEYWELL_BARO_CHANNEL = - static_cast<AD7994_t::Channel>(AD7994_HONEYWELL_BARO_CHANNEL); -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h b/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h deleted file mode 100644 index 45bdb7065aa91bc00db0e9885abf26435cf14cf9..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <drivers/adc/ADC.h> -#include "ADCWrapperData.h" -#include "DeathStack/configs/SensorManagerConfig.h" -#include "sensors/Sensor.h" - -namespace DeathStackBoard -{ -typedef ::SensorADC<INTERNAL_ADC_NUM> adc_t; -/** - * ADC wrapper to abstract reading of battery voltage & current sense on - * the R2A Hermes rocket. - * - * @tparam ADC_n - */ -class ADCWrapper -{ -public: - ADCWrapper() : battery_sensor(*this), current_sensor(*this) {} - /** - * Abstracts the current sense reading on the adc as if it was an - * independent sensor. - */ - class CurrentSensor : public ::Sensor - { - public: - CurrentSensor(ADCWrapper& parent) : parent(parent) {} - - bool init() override { return true; } - - bool selfTest() override { return true; } - - /** - * @brief Converts the channels associated with the current sense. - */ - bool onSimpleUpdate() override - { - current_data.timestamp = miosix::getTick(); - current_data.raw_value_1 = parent.adc.convertChannel(CS_1_CHANNEL); - - current_data.raw_value_2 = parent.adc.convertChannel(CS_2_CHANNEL); - - current_data.current_1 = adcToCurrent(current_data.raw_value_1); - current_data.current_2 = adcToCurrent(current_data.raw_value_2); - - return true; - } - - /** - * @brief Returns a pointer to the last conveted current sense value - */ - CurrentSenseData* getCurrentDataPtr() { return ¤t_data; } - - private: - float adcToCurrent(uint16_t adc_in) { return (adc_in - 107) / 23.8f; } - - const adc_t::Channel CS_1_CHANNEL = - static_cast<adc_t::Channel>(ADC_CURRENT_SENSE_1_CHANNEL); - - const adc_t::Channel CS_2_CHANNEL = - static_cast<adc_t::Channel>(ADC_CURRENT_SENSE_2_CHANNEL); - - ADCWrapper& parent; - CurrentSenseData current_data; - }; - - /** - * Abstracts the battery voltage reading on the adc as if it was an - * independent sensor. - */ - class BatterySensor : public ::Sensor - { - public: - BatterySensor(ADCWrapper& parent) : parent(parent) {} - - bool init() override { return true; } - - bool selfTest() override { return true; } - - /** - * @brief Converts the channel associated with the battery voltage - */ - bool onSimpleUpdate() override - { - uint16_t battery_volt = - parent.adc.convertChannel(BATTERY_VOLT_CHANNEL); - - battery_data.timestamp = miosix::getTick(); - battery_data.raw_value = battery_volt; - battery_data.volt = battery_volt * 0.005324f; - - return true; - } - /** - * @brief Returns the pointer to the result of the last conversion. - */ - BatteryVoltageData* getBatteryDataPtr() { return &battery_data; } - - private: - const adc_t::Channel BATTERY_VOLT_CHANNEL = - static_cast<adc_t::Channel>(ADC_BATTERY_VOLTAGE_CHANNEL); - ADCWrapper& parent; - - BatteryVoltageData battery_data; - }; - - BatterySensor* getBatterySensorPtr() { return &battery_sensor; } - - CurrentSensor* getCurrentSensorPtr() { return ¤t_sensor; } - -private: - BatterySensor battery_sensor; - CurrentSensor current_sensor; - adc_t adc; -}; - -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h b/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h deleted file mode 100644 index 6693a3da7203dc594899fdd260d459646b1d48b9..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <cstdint> -#include <ostream> - -struct BatteryVoltageData -{ - long long timestamp; - - uint16_t raw_value; - float volt; - - static std::string header() { return "timestamp,raw_value,volt\n"; } - - void print(std::ostream& os) const - { - os << timestamp << "," << (int)raw_value << "," << volt << "\n"; - } -}; - -struct CurrentSenseData -{ - long long timestamp; - uint16_t raw_value_1; - uint16_t raw_value_2; - - float current_1; - float current_2; - - static std::string header() - { - return "timestamp,raw_value_1,raw_value_2,current_1,current_2\n"; - } - - void print(std::ostream& os) const - { - os << timestamp << "," << raw_value_1 << "," << raw_value_2 << "," << current_1 - << "," << current_2 << "\n"; - } -}; \ No newline at end of file diff --git a/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h b/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h deleted file mode 100644 index 8ab13bf657b49e0dbbec28ea108b49fd89c30e57..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#ifndef SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H -#define SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H - -#include <cmath> -#include "Common.h" -#include "sensors/Sensor.h" - -using miosix::getTick; -using miosix::TICK_FREQ; - -class TestSensor : public Sensor -{ -public: - TestSensor() : mLastSample(0) {} - virtual ~TestSensor() {} - - bool init() { return true; } - bool onSimpleUpdate() - { - // printf("onSimpleUpdate\n"); - mLastSample = 10 * sin(PI * static_cast<float>(getTick()) / - static_cast<float>(TICK_FREQ)); - return true; - } - - bool selfTest() { return true; } - - float* testDataPtr() { return &mLastSample; } - -private: - float mLastSample; -}; - -#endif /* SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H */ diff --git a/src/boards/DeathStack/ServoInterface.h b/src/boards/DeathStack/ServoInterface.h new file mode 100644 index 0000000000000000000000000000000000000000..4cb7140170bf0725d6d2ec85f27fe7dcfb571ed5 --- /dev/null +++ b/src/boards/DeathStack/ServoInterface.h @@ -0,0 +1,159 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace DeathStackBoard +{ + +/** + * @brief Class for interfacing with 180° servo motors, works in degrees. + * + * This class provides all the methods for enabling/disabling servo + * communication as well as methods for testing it (like reset, setMaxPosition + * and selftest). + * The function set(float) is used to send data to the servo and calls, in + * sequence, the preprocessPosition() and the setPosition() functions. This + * function must not be overridden and all the logic of preprocessing and + * sending position to the actual servo must be implemented via those two + * methods. + * */ +class ServoInterface +{ +public: + /** + * @brief Class constructor. + * + * @param minPosition The minimum position possible for the Servo + * @param maxPosition The maximum position possible for the Servo + */ + ServoInterface(float minPosition, float maxPosition) + : MIN_POS(minPosition), MAX_POS(maxPosition), RESET_POS(minPosition) + { + } + + /** + * @brief Class constructor. + * + * @param minPosition The minimum position possible for the Servo + * @param maxPosition The maximum position possible for the Servo + * @param resetPosition The reset position for the Servo + */ + ServoInterface(float minPosition, float maxPosition, float resetPosition) + : MIN_POS(minPosition), MAX_POS(maxPosition), RESET_POS(resetPosition) + { + } + + virtual ~ServoInterface() {} + + /** + * @brief Enables the communication with the servo and sets it to its reset + * position. + */ + virtual void enable() = 0; + + /** + * @brief Disables the communication with the servo. + */ + virtual void disable() = 0; + + /** + * @brief Sends the given input to the Servo. + * + * Before sending data the input is preprocessed in order to have a physical + * consistent position to send to the servo. Do not override this method, + * but use @see{setPosition} and @see{preprocessPosition}. + * + * @param angle The input to be sent to the Servo + */ + void set(float angle, bool preprocess = false) + { + if (angle > MAX_POS) + { + angle = MAX_POS; + } + else if (angle < MIN_POS) + { + angle = MIN_POS; + } + + if (preprocess) + { + setPosition(preprocessPosition(angle)); + } + else + { + setPosition(angle); + } + } + + /** + * @brief Sends the Servo the highest input possible + */ + void setMaxPosition() { setPosition(MAX_POS); } + + /** + * @brief Sends the Servo the lowest input possible + */ + void setMinPosition() { setPosition(MIN_POS); } + + /** + * @brief Sets servo to its reset position + */ + void reset() { setPosition(RESET_POS); } + + /** + * @return current actuator position (in degrees) + */ + float getCurrentPosition() { return currentPosition; } + + virtual void selfTest() = 0; + + const float MIN_POS = 0; + const float MAX_POS = 0; + const float RESET_POS = 0; + +protected: + /** + * @brief Sends the data to the servo. + * + * @param angle Data to be sent to servo. + */ + virtual void setPosition(float angle) = 0; + + /** + * @brief Applies any transformation needed to the data before actually + * sending it to the servo. + * + * @param angle Non normalized input position. + * + * @returns Normalized input position. + */ + virtual float preprocessPosition(float angle) { return angle; }; + + /** + * @brief Actuator's current position. + */ + float currentPosition = 0; +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/System/StackLogger.h b/src/boards/DeathStack/System/StackLogger.h index a9f8a1b120755ee3983c5c8ebc200f386af284ca..9af6f2150648629ea94e0531ca1e7ed442745eb5 100644 --- a/src/boards/DeathStack/System/StackLogger.h +++ b/src/boards/DeathStack/System/StackLogger.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -29,16 +28,20 @@ namespace DeathStackBoard { -// define thread ids to be used by the StackLogger -enum HermesThreadIds : uint8_t +/** + * @brief Define thread ids to be used by the StackLogger. + */ +enum LynxThreadIds : uint8_t { THID_ENTRYPOINT = THID_FIRST_AVAILABLE_ID, - THID_SENSOR_MANAGER, - THID_SENSOR_SAMPLER, THID_DPL_FSM, THID_FMM_FSM, THID_TMTC_FSM, THID_STATS_FSM, - THID_ADA_FSM + THID_ADA_FSM, + THID_ABK_FSM, + THID_NAS_FSM, + THID_TASK_SCHEDULER }; + } \ No newline at end of file diff --git a/src/boards/DeathStack/System/SystemData.h b/src/boards/DeathStack/System/SystemData.h index be1d457c33c248d787501f000114cd476b7a56f2..f4253d251ab7fdc2eb187777d529c1eea0074de1 100644 --- a/src/boards/DeathStack/System/SystemData.h +++ b/src/boards/DeathStack/System/SystemData.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN diff --git a/src/boards/DeathStack/System/TaskID.h b/src/boards/DeathStack/System/TaskID.h new file mode 100644 index 0000000000000000000000000000000000000000..037f42766d2a18d189c88a702d473a49cbddaf59 --- /dev/null +++ b/src/boards/DeathStack/System/TaskID.h @@ -0,0 +1,46 @@ +/** + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace DeathStackBoard +{ + +/** + * @brief Define task ids to be used by the TaskScheduler. + */ +enum TaskIDs : uint8_t +{ + TASK_SCHEDULER_STATS_ID = 0, + TASK_SENSORS_6_MS_ID = 1, + TASK_SENSORS_15_MS_ID = 2, + TASK_SENSORS_20_MS_ID = 3, + TASK_SENSORS_24_MS_ID = 4, + TASK_SENSORS_40_MS_ID = 5, + TASK_SENSORS_1000_MS_ID = 6, + TASK_ADA_ID = 7, + TASK_ABK_ID = 8, + TASK_NAS_ID = 9 +}; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/TMTCManager/TCHandler.h b/src/boards/DeathStack/TMTCManager/TCHandler.h deleted file mode 100644 index cbbcc6972115a4a84519da01ddfe3404df20041d..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/TMTCManager/TCHandler.h +++ /dev/null @@ -1,187 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise De Faveri - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <Common.h> - -#include <DeathStack/LoggerService/LoggerService.h> -#include <DeathStack/configs/TMTCConfig.h> -#include "DeathStack/DeathStack.h" -#include <drivers/mavlink/MavlinkDriver.h> -#include "DeathStack/events/Events.h" -#include "DeathStack/events/Topics.h" -#include "TMBuilder.h" - -#define MAV_TC(X) MAVLINK_MSG_ID_##X##_TC - -namespace DeathStackBoard -{ - -namespace TCHandler -{ - -using Mav = MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; - -/** - * Map each noArg command to the corresponding event - */ -// clang-format off -static const std::map<uint8_t, uint8_t> noargCmdToEvt = -{ - { MAV_CMD_ARM, EV_TC_ARM }, - { MAV_CMD_DISARM, EV_TC_DISARM }, - { MAV_CMD_CALIBRATE_ADA, EV_TC_CALIBRATE_ADA}, - - { MAV_CMD_FORCE_INIT, EV_TC_FORCE_INIT}, - { MAV_CMD_FORCE_LAUNCH, EV_TC_LAUNCH}, - { MAV_CMD_NOSECONE_OPEN, EV_TC_NC_OPEN }, - { MAV_CMD_RESET_SERVO, EV_TC_RESET_SERVO }, - { MAV_CMD_WIGGLE_SERVO, EV_TC_WIGGLE_SERVO }, - - { MAV_CMD_START_LOGGING, EV_TC_START_SENSOR_LOGGING }, - { MAV_CMD_STOP_LOGGING, EV_TC_STOP_SENSOR_LOGGING }, - { MAV_CMD_CLOSE_LOG, EV_TC_CLOSE_LOG }, - - { MAV_CMD_TEST_MODE, EV_TC_TEST_MODE }, - { MAV_CMD_TEST_PRIMARY_CUTTER, EV_TC_TEST_CUTTER_PRIMARY }, - { MAV_CMD_TEST_BACKUP_CUTTER, EV_TC_TEST_CUTTER_BACKUP }, - { MAV_CMD_CUT_PRIMARY, EV_TC_CUT_PRIMARY }, - { MAV_CMD_CUT_BACKUP, EV_TC_CUT_BACKUP }, - { MAV_CMD_CUT_DROGUE, EV_TC_CUT_DROGUE }, - { MAV_CMD_BOARD_RESET, EV_TC_BOARD_RESET }, - - { MAV_CMD_END_MISSION, EV_TC_END_MISSION } -}; -// clang-format on - -/** - * Send an ACK to notify the sender that you received the given message. - */ -static void sendAck(Mav* channel, const mavlink_message_t& msg) -{ - mavlink_message_t ackMsg; - mavlink_msg_ack_tm_pack(TMTC_MAV_SYSID, TMTC_MAV_COMPID, &ackMsg, msg.msgid, - msg.seq); - - /* Send the message back to the sender */ - channel->enqueueMsg(ackMsg); - TRACE("[TMTC] Enqueued Ack\n"); -} - -/** - * Handle the Mavlink message, posting the corresponding event if needed. - */ -static void handleMavlinkMessage(Mav* channel, const mavlink_message_t& msg) -{ - TRACE("[TMTC] Handling command\n"); - - /* Log Status */ - MavlinkStatus status = channel->getStatus(); - LoggerService::getInstance()->log(status); - - /* Send acknowledge */ - sendAck(channel, msg); - - /* Finally handle TC */ - switch (msg.msgid) - { - case MAV_TC(NOARG): - { - TRACE("[TMTC] Received NOARG command\n"); - uint8_t commandId = mavlink_msg_noarg_tc_get_command_id(&msg); - auto it = - DeathStackBoard::TCHandler::noargCmdToEvt.find(commandId); - - if (it != noargCmdToEvt.end()) - sEventBroker->post(Event{it->second}, TOPIC_TC); - else - TRACE("[TMTC] Unknown NOARG command %d\n", commandId); - - break; - } - - case MAV_TC(TELEMETRY_REQUEST): - { - TRACE("[TMTC] Received TM request\n"); - uint8_t tmId = mavlink_msg_telemetry_request_tc_get_board_id(&msg); - mavlink_message_t response = TMBuilder::buildTelemetry(tmId); - channel->enqueueMsg(response); - - break; - } - - case MAV_TC(PING): - { - TRACE("[TMTC] Ping received\n"); - break; - } - case MAV_TC(UPLOAD_SETTING): - { - uint8_t id = mavlink_msg_upload_setting_tc_get_setting_id(&msg); - float setting = mavlink_msg_upload_setting_tc_get_setting(&msg); - - TRACE("[TMTC] Upload setting: %d, %f\n", (int)id, setting); - - switch (id) - { - case MAV_SET_DEPLOYMENT_ALTITUDE: - { - DeathStack::getInstance()->ada->setDeploymentAltitude( - setting); - break; - } - case MAV_SET_REFERENCE_ALTITUDE: - { - DeathStack::getInstance()->ada->setReferenceAltitude( - setting); - break; - } - case MAV_SET_REFERENCE_TEMP: - { - DeathStack::getInstance()->ada->setReferenceTemperature( - setting); - break; - } - } - break; - } - case MAV_TC(RAW_EVENT): - { - TRACE("[TMTC] Received RAW_EVENT command\n"); - /* Retrieve event from the message*/ - Event evt = {mavlink_msg_raw_event_tc_get_Event_id(&msg)}; - sEventBroker->post(evt, - mavlink_msg_raw_event_tc_get_Topic_id(&msg)); - break; - } - - default: - { - TRACE("[TMTC] Received message is not of a known type\n"); - break; - } - } -} - -} /* namespace TCHandler */ -} /* namespace DeathStackBoard */ diff --git a/src/boards/DeathStack/TMTCManager/TMBuilder.h b/src/boards/DeathStack/TMTCManager/TMBuilder.h deleted file mode 100644 index 4887b7aa3dac4bf7f7003f4e16c36c43ed4af5f8..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/TMTCManager/TMBuilder.h +++ /dev/null @@ -1,150 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise De Faveri - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <Common.h> -#include <DeathStack/LoggerService/TmRepository.h> -#include <DeathStack/configs/TMTCConfig.h> -namespace DeathStackBoard -{ -namespace TMBuilder -{ -static mavlink_nack_tm_t nack_tm; -/** - * Retrieve one of the telemetry structs - * @param req_tm required telemetry - * @param sys_id system id to pack it with - * @param comp_id component id to pack it with - * @return packed mavlink struct of that telemetry - */ -static mavlink_message_t getTM(uint8_t req_tm, uint8_t sys_id, uint8_t comp_id) -{ - mavlink_message_t m; - - switch (req_tm) - { - case MavTMList::MAV_SYS_TM_ID: - tm_repository.sys_tm.timestamp = miosix::getTick(); - mavlink_msg_sys_tm_encode(sys_id, comp_id, &m, - &(tm_repository.sys_tm)); - break; - case MavTMList::MAV_FMM_TM_ID: - tm_repository.fmm_tm.timestamp = miosix::getTick(); - mavlink_msg_fmm_tm_encode(sys_id, comp_id, &m, - &(tm_repository.fmm_tm)); - break; - case MavTMList::MAV_LOGGER_TM_ID: - tm_repository.logger_tm.timestamp = miosix::getTick(); - mavlink_msg_logger_tm_encode(sys_id, comp_id, &m, - &(tm_repository.logger_tm)); - break; - case MavTMList::MAV_TMTC_TM_ID: - tm_repository.tmtc_tm.timestamp = miosix::getTick(); - mavlink_msg_tmtc_tm_encode(sys_id, comp_id, &m, - &(tm_repository.tmtc_tm)); - break; - case MavTMList::MAV_SM_TM_ID: - tm_repository.sm_tm.timestamp = miosix::getTick(); - mavlink_msg_sm_tm_encode(sys_id, comp_id, &m, - &(tm_repository.sm_tm)); - break; - case MavTMList::MAV_DPL_TM_ID: - tm_repository.dpl_tm.timestamp = miosix::getTick(); - mavlink_msg_dpl_tm_encode(sys_id, comp_id, &m, - &(tm_repository.dpl_tm)); - break; - case MavTMList::MAV_IGN_TM_ID: - tm_repository.ign_tm.timestamp = miosix::getTick(); - mavlink_msg_ign_tm_encode(sys_id, comp_id, &m, - &(tm_repository.ign_tm)); - break; - case MavTMList::MAV_ADA_TM_ID: - tm_repository.ada_tm.timestamp = miosix::getTick(); - mavlink_msg_ada_tm_encode(sys_id, comp_id, &m, - &(tm_repository.ada_tm)); - break; - case MavTMList::MAV_CAN_TM_ID: - mavlink_msg_can_tm_encode(sys_id, comp_id, &m, - &(tm_repository.can_tm)); - break; - case MavTMList::MAV_ADC_TM_ID: - tm_repository.adc_tm.timestamp = miosix::getTick(); - mavlink_msg_adc_tm_encode(sys_id, comp_id, &m, - &(tm_repository.adc_tm)); - break; - case MavTMList::MAV_ADIS_TM_ID: - tm_repository.adis_tm.timestamp = miosix::getTick(); - mavlink_msg_adis_tm_encode(sys_id, comp_id, &m, - &(tm_repository.adis_tm)); - break; - case MavTMList::MAV_MPU_TM_ID: - tm_repository.mpu_tm.timestamp = miosix::getTick(); - mavlink_msg_mpu_tm_encode(sys_id, comp_id, &m, - &(tm_repository.mpu_tm)); - break; - case MavTMList::MAV_GPS_TM_ID: - tm_repository.gps_tm.timestamp = miosix::getTick(); - mavlink_msg_gps_tm_encode(sys_id, comp_id, &m, - &(tm_repository.gps_tm)); - break; - - // case MavTMList::MAV_HR_TM_ID: - // { - // mavlink_msg_hr_tm_encode(sys_id, comp_id, &m, - // &(tm_repository.hr_tm_packet)); - // break; - // } - // case MavTMList::MAV_LR_TM_ID: - // mavlink_msg_lr_tm_encode(sys_id, comp_id, &m, - // &(tm_repository.lr_tm_packet)); - // break; - case MavTMList::MAV_TEST_TM_ID: - tm_repository.test_tm.timestamp = miosix::getTick(); - mavlink_msg_test_tm_encode(sys_id, comp_id, &m, - &(tm_repository.test_tm)); - break; - default: - { - TRACE("[MAV] Unknown telemetry requested: %d\n", req_tm); - mavlink_msg_nack_tm_encode(sys_id, comp_id, &m, &nack_tm); - break; - } - } - - return m; -} - -/** - * Synchronously read the corresponding telemetry from the statusRepo - * (aka last logged struct). - * @param req_tm requested telemetry - * @return the telemetry as mavlink_message_t - */ -inline mavlink_message_t buildTelemetry(uint8_t req_tm) -{ - miosix::PauseKernelLock kLock; - return getTM(req_tm, TMTC_MAV_SYSID, TMTC_MAV_SYSID); -} - -} /* namespace TMBuilder */ -} /* namespace DeathStackBoard */ diff --git a/src/boards/DeathStack/TMTCManager/TMTCManager.cpp b/src/boards/DeathStack/TMTCManager/TMTCManager.cpp deleted file mode 100644 index 5a626afc1ab448103380bdfcf6f6f94566bfb321..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/TMTCManager/TMTCManager.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "TMTCManager.h" -#include <DeathStack/configs/TMTCConfig.h> -#include <DeathStack/events/Events.h> -#include <DeathStack/events/Topics.h> -#include <drivers/Xbee/Xbee.h> -#include "TCHandler.h" // Real message handling is here -#include "XbeeInterrupt.h" -#include "bitpacking/hermes/HermesPackets.h" -namespace DeathStackBoard -{ - -TMTCManager::TMTCManager() : FSM(&TMTCManager::stateGroundTM) -{ - busSPI2::init(); - enableXbeeInterrupt(); - - device = new Xbee_t(); - channel = new Mav(device, - &TCHandler::handleMavlinkMessage, // rcv function - TMTC_SLEEP_AFTER_SEND, MAV_OUT_BUFFER_MAX_AGE); - - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_TMTC); - - TRACE("[TMTC] Created TMTCManager\n"); -} - -TMTCManager::~TMTCManager() -{ - device->stop(); - sEventBroker->unsubscribe(this); - - channel->stop(); - delete device; - delete channel; -} - -bool TMTCManager::send(mavlink_message_t& msg) -{ - bool ok = channel->enqueueMsg(msg); - - MavlinkStatus status = channel->getStatus(); - logger.log(status); - - return ok; -} - -void TMTCManager::stateGroundTM(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - test_tm_event_id = sEventBroker->postDelayed<TEST_TM_TIMEOUT>( - Event{EV_SEND_TEST_TM}, TOPIC_TMTC); - - TRACE("[TMTC] Entering stateGroundTM\n"); - StackLogger::getInstance()->updateStack(THID_TMTC_FSM); - break; - - case EV_SEND_TEST_TM: - { - // Send both HR_TM and TEST_TM - test_tm_event_id = sEventBroker->postDelayed<TEST_TM_TIMEOUT>( - Event{EV_SEND_TEST_TM}, TOPIC_TMTC); - - // Pack the current data in hr_tm_packet.payload - packHRTelemetry(hr_tm_packet.payload, hr_tm_index); - - // Send HR telemetry once 4 packets are filled - if (hr_tm_index == 3) - { - mavlink_msg_hr_tm_encode(TMTC_MAV_SYSID, TMTC_MAV_SYSID, - &auto_telemetry_msg, &(hr_tm_packet)); - send(auto_telemetry_msg); - } - - // Two TEST_TM every one HR_TM - if (hr_tm_index % 2 == 1) - { - mavlink_message_t telem = - TMBuilder::buildTelemetry(MAV_TEST_TM_ID); - send(telem); - } - hr_tm_index = (hr_tm_index + 1) % 4; - - break; - } - case EV_ARMED: - case EV_LIFTOFF: - { - transition(&TMTCManager::stateFlightTM); - break; - } - - case EV_EXIT: - { - sEventBroker->removeDelayed(test_tm_event_id); - - TRACE("[TMTC] Exiting stateGroundTM\n"); - break; - } - default: - break; - } -} - -void TMTCManager::stateFlightTM(const Event& ev) -{ - switch (ev.sig) - { - case EV_ENTRY: - lr_event_id = sEventBroker->postDelayed<LR_TM_TIMEOUT>( - Event{EV_SEND_LR_TM}, TOPIC_TMTC); - hr_event_id = sEventBroker->postDelayed<HR_TM_TIMEOUT>( - Event{EV_SEND_HR_TM}, TOPIC_TMTC); - - TRACE("[TMTC] Entering stateFlightTM\n"); - StackLogger::getInstance()->updateStack(THID_TMTC_FSM); - break; - - case EV_SEND_HR_TM: - { - // Schedule the next HR telemetry - hr_event_id = sEventBroker->postDelayed<HR_TM_TIMEOUT>( - Event{EV_SEND_HR_TM}, TOPIC_TMTC); - - // Pack the current data in hr_tm_packet.payload - packHRTelemetry(hr_tm_packet.payload, hr_tm_index); - - // Send HR telemetry once 4 packets are filled - if (hr_tm_index == 3) - { - mavlink_msg_hr_tm_encode(TMTC_MAV_SYSID, TMTC_MAV_SYSID, - &auto_telemetry_msg, &(hr_tm_packet)); - send(auto_telemetry_msg); - } - - hr_tm_index = (hr_tm_index + 1) % 4; - break; - } - - case EV_SEND_LR_TM: - { - // Schedule the next HR telemetry - lr_event_id = sEventBroker->postDelayed<LR_TM_TIMEOUT>( - Event{EV_SEND_LR_TM}, TOPIC_TMTC); - - packLRTelemetry(lr_tm_packet.payload); - - mavlink_msg_lr_tm_encode(TMTC_MAV_SYSID, TMTC_MAV_SYSID, - &auto_telemetry_msg, &(lr_tm_packet)); - send(auto_telemetry_msg); - - break; - } - case EV_DISARMED: - { - transition(&TMTCManager::stateGroundTM); - break; - } - case EV_EXIT: - { - sEventBroker->removeDelayed(lr_event_id); - sEventBroker->removeDelayed(hr_event_id); - - TRACE("[TMTC] Exiting stateFlightTM\n"); - break; - } - default: - break; - } -} - -void TMTCManager::packHRTelemetry(uint8_t* packet, unsigned int index) -{ - HighRateTMPacker packer(packet); - - packer.packTimestamp(miosix::getTick(), index); - - packer.packPressureAda(tm_repository.hr_tm.pressure_ada, index); - packer.packPressureDigi(tm_repository.hr_tm.pressure_digi, index); - - packer.packMslAltitude(tm_repository.hr_tm.msl_altitude, index); - packer.packAglAltitude(tm_repository.hr_tm.agl_altitude, index); - - packer.packVertSpeed(tm_repository.hr_tm.vert_speed, index); - packer.packVertSpeed2(tm_repository.hr_tm.vert_speed_2, index); - - packer.packAccX(tm_repository.hr_tm.acc_x, index); - packer.packAccY(tm_repository.hr_tm.acc_y, index); - packer.packAccZ(tm_repository.hr_tm.acc_z, index); - - packer.packGyroX(tm_repository.hr_tm.gyro_x, index); - packer.packGyroY(tm_repository.hr_tm.gyro_y, index); - packer.packGyroZ(tm_repository.hr_tm.gyro_z, index); - - packer.packGpsLat(tm_repository.hr_tm.gps_lat, index); - packer.packGpsLon(tm_repository.hr_tm.gps_lon, index); - packer.packGpsAlt(tm_repository.hr_tm.gps_alt, index); - packer.packGpsFix(tm_repository.hr_tm.gps_fix, index); - - packer.packTemperature(tm_repository.hr_tm.temperature, index); - - packer.packFmmState(tm_repository.hr_tm.fmm_state, index); - packer.packDplState(tm_repository.hr_tm.dpl_state, index); - - packer.packPinLaunch(tm_repository.hr_tm.pin_launch, index); - packer.packPinNosecone(tm_repository.hr_tm.pin_nosecone, index); -} - -void TMTCManager::packLRTelemetry(uint8_t* packet) -{ - LowRateTMPacker packer(packet); - - packer.packLiftoffTs(tm_repository.lr_tm.liftoff_ts, 0); - packer.packLiftoffMaxAccTs(tm_repository.lr_tm.liftoff_max_acc_ts, 0); - packer.packLiftoffMaxAcc(tm_repository.lr_tm.liftoff_max_acc_ts, 0); - - packer.packMaxZspeedTs(tm_repository.lr_tm.max_zspeed_ts, 0); - packer.packMaxZspeed(tm_repository.lr_tm.max_zspeed, 0); - packer.packMaxSpeedAltitude(tm_repository.lr_tm.max_speed_altitude, 0); - - packer.packApogeeTs(tm_repository.lr_tm.apogee_ts, 0); - packer.packNxpMinPressure(tm_repository.lr_tm.nxp_min_pressure, 0); - packer.packHwMinPressure(tm_repository.lr_tm.hw_min_pressure, 0); - packer.packKalmanMinPressure(tm_repository.lr_tm.kalman_min_pressure, 0); - packer.packDigitalMinPressure(tm_repository.lr_tm.digital_min_pressure, 0); - - packer.packBaroMaxAltitutde(tm_repository.lr_tm.baro_max_altitutde, 0); - packer.packGpsMaxAltitude(tm_repository.lr_tm.gps_max_altitude, 0); - - packer.packApogeeLat(tm_repository.lr_tm.apogee_lat, 0); - packer.packApogeeLon(tm_repository.lr_tm.apogee_lon, 0); - - packer.packDrogueDplTs(tm_repository.lr_tm.drogue_dpl_ts, 0); - packer.packDrogueDplMaxAcc(tm_repository.lr_tm.drogue_dpl_max_acc, 0); - - packer.packMainDplTs(tm_repository.lr_tm.main_dpl_ts, 0); - packer.packMainDplAltitude(tm_repository.lr_tm.main_dpl_altitude, 0); - packer.packMainDplZspeed(tm_repository.lr_tm.main_dpl_zspeed, 0); - packer.packMainDplAcc(tm_repository.lr_tm.main_dpl_acc, 0); -} - -} /* namespace DeathStackBoard */ diff --git a/src/boards/DeathStack/TMTCManager/TMTCManager.h b/src/boards/DeathStack/TMTCManager/TMTCManager.h deleted file mode 100644 index da3e07f9d6ab7f8f2b1d460144cb4bd246e4e733..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/TMTCManager/TMTCManager.h +++ /dev/null @@ -1,93 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include "DeathStack/events/Events.h" -#include "DeathStack/configs/TMTCConfig.h" -#include "events/FSM.h" - -#include <drivers/mavlink/MavlinkDriver.h> -#include <DeathStack/LoggerService/LoggerService.h> -#include <interfaces-impl/hwmapping.h> - -namespace DeathStackBoard -{ -/** - * @brief This class handles the communication with the Ground Station. - * Uses Gamma868 transceiver with the Mavlink protocol. - */ -class TMTCManager : public FSM<TMTCManager> -{ - using Mav = MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; -public: - TMTCManager(); - ~TMTCManager(); - - /* Non-blocking send, logs status on the logger*/ - bool send(mavlink_message_t& msg); - - /* Status getter */ - MavlinkStatus getStatus() { return channel->getStatus(); } - - /* AO methods */ - bool start() override - { - device->start(); - bool ret = channel->start(); - ret = (ret && FSM::start()); - return ret; - } - - void stop() override - { - channel->stop(); - FSM::stop(); - } - -private: - Xbee_t* device; - Mav* channel; - - LoggerService& logger = *(LoggerService::getInstance()); - - /* State handlers */ - // void stateIdle(const Event& ev); - void stateGroundTM(const Event& ev); - void stateFlightTM(const Event& ev); - - inline void packHRTelemetry(uint8_t* packet, unsigned int index); - inline void packLRTelemetry(uint8_t* packet); - - uint16_t lr_event_id = 0; - uint16_t hr_event_id = 0; - uint16_t test_tm_event_id = 0; - - uint8_t hr_tm_index = 0; - - mavlink_hr_tm_t hr_tm_packet; - mavlink_lr_tm_t lr_tm_packet; - - mavlink_message_t auto_telemetry_msg; -}; - -} /* namespace DeathStackBoard */ diff --git a/src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp b/src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp deleted file mode 100644 index 6477c197e72f8c310ece35547fefe2b6fa8f05a1..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "XbeeInterrupt.h" -#include <miosix.h> -#include <drivers/Xbee/Xbee.h> - -using namespace miosix; - -void __attribute__((used)) EXTI10_IRQHandlerImpl() { Xbee::handleATTNInterrupt(); } - -namespace DeathStackBoard -{ -void enableXbeeInterrupt() -{ - { - FastInterruptDisableLock l; - RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; - } - // Refer to the datasheet for a detailed description on the procedure and - // interrupt registers - - // Clear the mask on the wanted line - EXTI->IMR |= EXTI_IMR_MR10; - - // Trigger the interrupt on a falling edge - EXTI->FTSR |= EXTI_FTSR_TR10; - - // Trigger the interrupt on a rising edge - // EXTI->RTSR |= EXTI_RTSR_TR0; - - EXTI->PR |= EXTI_PR_PR10; // Reset pending register - - // Enable interrupt on PF10 in SYSCFG - SYSCFG->EXTICR[2] = 0x5U << 8; - - // Enable the interrput in the interrupt controller - NVIC_EnableIRQ(EXTI15_10_IRQn); - NVIC_SetPriority(EXTI15_10_IRQn, 15); -} -} \ No newline at end of file diff --git a/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h b/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h new file mode 100644 index 0000000000000000000000000000000000000000..c2e6360ac468a4a3ea05b5075806a30472b1b4ce --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h @@ -0,0 +1,41 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +// Ignore warnings as these are auto-generated headers made with third party +// tools +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" +#include <mavlink_skyward_lib/mavlink_lib/lynx/mavlink.h> +#pragma GCC diagnostic pop + +#include <drivers/mavlink/MavlinkDriver.h> +#include <configs/MavlinkConfig.h> + +namespace DeathStackBoard +{ + +using MavDriver = MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8257c45f1ea7ec8ae26f5c8f9fd97b5b4d38dd97 --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp @@ -0,0 +1,241 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "TCHandler.h" + +#include <AirBrakes/AirBrakesController.h> +#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <DeathStack.h> +#include <configs/TMTCConfig.h> +#include <events/Events.h> + +using std::map; +namespace DeathStackBoard +{ + +const std::map<uint8_t, uint8_t> tcMap = { + {MAV_CMD_ARM, EV_TC_ARM}, + {MAV_CMD_DISARM, EV_TC_DISARM}, + + {MAV_CMD_FORCE_INIT, EV_TC_FORCE_INIT}, + {MAV_CMD_FORCE_LAUNCH, EV_TC_LAUNCH}, + + {MAV_CMD_NOSECONE_OPEN, EV_TC_NC_OPEN}, + {MAV_CMD_DPL_RESET_SERVO, EV_TC_DPL_RESET_SERVO}, + {MAV_CMD_DPL_WIGGLE_SERVO, EV_TC_DPL_WIGGLE_SERVO}, + {MAV_CMD_CUT_DROGUE, EV_TC_CUT_DROGUE}, + + {MAV_CMD_ARB_RESET_SERVO, EV_TC_ABK_RESET_SERVO}, + {MAV_CMD_ARB_WIGGLE_SERVO, EV_TC_ABK_WIGGLE_SERVO}, + {MAV_CMD_DISABLE_AEROBRAKES, EV_TC_ABK_DISABLE}, + {MAV_CMD_TEST_AEROBRAKES, EV_TC_TEST_ABK}, + + {MAV_CMD_CALIBRATE_ALGOS, EV_TC_CALIBRATE_ALGOS}, + {MAV_CMD_CALIBRATE_SENSORS, EV_TC_CALIBRATE_SENSORS}, + + {MAV_CMD_SERIAL_TM, EV_TC_SERIAL_TM}, + {MAV_CMD_RADIO_TM, EV_TC_RADIO_TM}, + + {MAV_CMD_START_LOGGING, EV_TC_START_LOG}, + {MAV_CMD_CLOSE_LOG, EV_TC_CLOSE_LOG}, + + {MAV_CMD_TEST_MODE, EV_TC_TEST_MODE}, + {MAV_CMD_BOARD_RESET, EV_TC_RESET_BOARD}, + {MAV_CMD_END_MISSION, EV_TC_END_MISSION}}; + +void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) +{ + // log status + DeathStack::getInstance()->radio->logStatus(); + + // acknowledge + sendAck(mav_driver, msg); + + // handle TC + switch (msg.msgid) + { + case MAVLINK_MSG_ID_NOARG_TC: // basic command + { + LOG_DEBUG(print_logger, "Received NOARG command"); + uint8_t commandId = mavlink_msg_noarg_tc_get_command_id(&msg); + + // search for the corresponding event and post it + auto it = tcMap.find(commandId); + if (it != tcMap.end()) + sEventBroker->post(Event{it->second}, TOPIC_TMTC); + else + LOG_WARN(print_logger, "Unknown NOARG command {:d}", commandId); + + switch (commandId) + { + case MAV_CMD_BOARD_RESET: + logger->stop(); + LOG_INFO(print_logger, "Received command BOARD_RESET"); + miosix::reboot(); + break; + case MAV_CMD_CLOSE_LOG: + logger->stop(); + sendTelemetry(mav_driver, MAV_LOGGER_TM_ID); + LOG_INFO(print_logger, "Received command CLOSE_LOG"); + break; + case MAV_CMD_START_LOGGING: + DeathStack::getInstance()->startLogger(); + sendTelemetry(mav_driver, MAV_LOGGER_TM_ID); + LOG_INFO(print_logger, "Received command START_LOG"); + break; + default: + break; + } + break; + } + + case MAVLINK_MSG_ID_TELEMETRY_REQUEST_TC: // tm request + { + uint8_t tmId = mavlink_msg_telemetry_request_tc_get_board_id(&msg); + LOG_DEBUG(print_logger, "Received TM request : id = {:d}", tmId); + + // send corresponding telemetry or NACK + sendTelemetry(mav_driver, tmId); + + break; + } + case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE: + { + float alt = + mavlink_msg_set_reference_altitude_get_ref_altitude(&msg); + LOG_DEBUG( + print_logger, + "Received SET_REFERENCE_ALTITUDE command. Ref altitude: {:f} m", + alt); + DeathStack::getInstance()->state_machines->setReferenceAltitude( + alt); + break; + } + case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: + { + float temp = + mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg); + LOG_DEBUG( + print_logger, + "Received SET_REFERENCE_TEMPERATURE command. Temp: {:f} degC", + temp); + DeathStack::getInstance()->state_machines->setReferenceTemperature( + temp); + break; + } + case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: + { + float alt = + mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg); + LOG_DEBUG( + print_logger, + "Received SET_DEPLOYMENT_ALTITUDE command. Dpl alt: {:f} m", + alt); + DeathStack::getInstance() + ->state_machines->ada_controller->setDeploymentAltitude(alt); + break; + } + case MAVLINK_MSG_ID_SET_INITIAL_ORIENTATION_TC: + { + float yaw = mavlink_msg_set_initial_orientation_tc_get_yaw(&msg); + float pitch = + mavlink_msg_set_initial_orientation_tc_get_pitch(&msg); + float roll = mavlink_msg_set_initial_orientation_tc_get_roll(&msg); + LOG_DEBUG(print_logger, + "Received SET_INITIAL_ORIENTATION command. roll: {:f}, " + "pitch: {:f}, yaw: {:f}", + roll, pitch, yaw); + DeathStack::getInstance()->state_machines->setInitialOrientation( + roll, pitch, yaw); + break; + } + + case MAVLINK_MSG_ID_SET_INITIAL_COORDINATES_TC: + { + float latitude = + mavlink_msg_set_initial_coordinates_tc_get_latitude(&msg); + float longitude = + mavlink_msg_set_initial_coordinates_tc_get_longitude(&msg); + LOG_DEBUG( + print_logger, + "Received SET_INITIAL_COORDINATES command. latitude: {:f}, " + "longitude: {:f}", + latitude, longitude); + DeathStack::getInstance()->state_machines->setInitialCoordinates( + latitude, longitude); + break; + } + case MAVLINK_MSG_ID_RAW_EVENT_TC: // post a raw event + { + LOG_DEBUG(print_logger, "Received RAW_EVENT command"); + + // post given event on given topic + sEventBroker->post({mavlink_msg_raw_event_tc_get_Event_id(&msg)}, + mavlink_msg_raw_event_tc_get_Topic_id(&msg)); + break; + } + + case MAVLINK_MSG_ID_PING_TC: + { + LOG_DEBUG(print_logger, "Ping received"); + break; + } + case MAVLINK_MSG_ID_SET_AEROBRAKE_ANGLE_TC: + { + float pos = mavlink_msg_set_aerobrake_angle_tc_get_angle(&msg); + DeathStack::getInstance() + ->state_machines->arb_controller->setAirBrakesPosition(pos); + LOG_DEBUG(print_logger, "Received set ab pos: {:.1f} deg", pos); + + break; + } + default: + { + LOG_DEBUG(print_logger, "Received message is not of a known type"); + break; + } + } +} +void sendAck(MavDriver* mav_driver, const mavlink_message_t& msg) +{ + mavlink_message_t ackMsg; + mavlink_msg_ack_tm_pack(TMTC_MAV_SYSID, TMTC_MAV_COMPID, &ackMsg, msg.msgid, + msg.seq); + + mav_driver->enqueueMsg(ackMsg); + LOG_INFO(print_logger, "Enqueued Ack ({:d})", msg.seq); +} + +bool sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id) +{ + // enqueue the TM packet taking it from the TM repo (pauses kernel to + // guarantee synchronicity) + bool ok = + mav_driver->enqueueMsg(TmRepository::getInstance()->packTM(tm_id)); + + // update status + DeathStack::getInstance()->radio->logStatus(); + + return ok; +} + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/IgnitionController/IgnitionStatus.h b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h similarity index 57% rename from src/boards/DeathStack/IgnitionController/IgnitionStatus.h rename to src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h index 48c7da392dae14363908da2a2a8d0db69d19cf08..7ac29dc46b5468ce61cdc503d24cd4dac98a68b3 100644 --- a/src/boards/DeathStack/IgnitionController/IgnitionStatus.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2018-2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta, Alvise de' Faveri Tron +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,37 +22,24 @@ #pragma once -#include <cstdint> -#include <CanInterfaces.h> +#include <LoggerService/LoggerService.h> +#include <TelemetriesTelecommands/Mavlink.h> +#include <diagnostic/PrintLogger.h> namespace DeathStackBoard { -enum IgnitionControllerState : uint8_t -{ - IGN_UNKNOWN, - IGN_IDLE, - IGN_ABORTED, - IGN_END -}; +void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg); -struct __attribute__((packed)) IgnCtrlStatus -{ - uint64_t timestamp; - uint8_t fsm_state; - uint8_t last_event; - uint16_t n_sent_messages; - uint16_t n_rcv_messages; - uint8_t launch_sent : 1; - uint8_t abort_sent : 1; - uint8_t abort_rcv : 1; - uint8_t padding : 5; -}; - -struct IgnBoardLoggableStatus -{ - uint64_t timestamp; - CanInterfaces::IgnitionBoardStatus board_status; -}; +void sendAck(MavDriver* mav_driver, const mavlink_message_t& msg); + +bool sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id); + +void logMavlinkStatus(MavDriver* mav_driver); + +static LoggerService* logger = LoggerService::getInstance(); + +static PrintLogger print_logger = + Logging::getLogger("deathstack.tmtc.tchandler"); -} +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTC.scxml b/src/boards/DeathStack/TelemetriesTelecommands/TMTC.scxml new file mode 100644 index 0000000000000000000000000000000000000000..2a1417f39c27f031810d03cf89cde75d73d0590b --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTC.scxml @@ -0,0 +1,41 @@ +<?xml version="1.0" encoding="UTF-8"?> +<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="groundTM" version="1.0"> + <state id="groundTM"> + <onentry>postD(TMTC.EV_SEND_HR_TM)</onentry> + <onexit>removeD(TMTC.EV_SEND_HR_TM)</onexit> + <transition event="TMTC.EV_SEND_HR_TM" target="groundTM"> + sendHRTM(); + sendTestTM(); + </transition> + <transition event="FLIGHT_EVENTS.EV_ARMED" target="flightTM"/> + <transition event="FLIGHT_EVENTS.EV_LIFTOFF" target="flightTM"/> + <transition event="TMTC.EV_TC_START_SENSOR_TM" target="sensorTM"/> + <transition event="TMTC.EV_TC_START_SERIAL_TM" target="serialDebugTM"/> + </state> + <state id="flightTM"> + <onentry>postD(TMTC.EV_SEND_HR_TM)</onentry> + <onentry>postD(TMTC.EV_SEND_LR_TM)</onentry> + <onexit>removeD(TMTC.EV_SEND_HR_TM)</onexit> + <onexit>removeD(TMTC.EV_SEND_LR_TM)</onexit> + <transition event="TMTC.EV_SEND_HR_TM" target="flightTM"> + sendHRTM(); + </transition> + <transition event="TMTC.EV_SEND_LR_TM" target="flightTM"> + sendLRTM(); + </transition> + <transition event="FLIGHT_EVENTS.EV_DISARMED" target="groundTM"/> + </state> + <state id="sensorTM"> + <onentry>postD(TMTC.EV_SEND_SENS_TM)</onentry> + <onexit>removeD(TMTC.EV_SEND_SENS_TM)</onexit> + <transition event="TMTC.EV_TC_STOP_SENSOR_TM" target="groundTM"> + </state> + <state id="serialDebugTM"> + <onentry>postD(TMTC.EV_SEND_HR_TM_OVER_SERIAL)</onentry> + <onexit>removeD(TMTC.EV_SEND_HR_TM_OVER_SERIAL)</onexit> + <transition event="TMTC.EV_SEND_HR_TM_OVER_SERIAL" target="serialDebugTM"> + sendSerialTM(); + </transition> + <transition event="TMTC.EV_TC_STOP_SERIAL_TM" target="groundTM"/> + </state> +</scxml> diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..77559358c8e981f28600424a204187581d5149b9 --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp @@ -0,0 +1,304 @@ +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <TelemetriesTelecommands/TMTCController.h> +#include <drivers/Xbee/Xbee.h> +#include <drivers/mavlink/MavlinkDriver.h> +#include <events/Topics.h> + +#include <cassert> + +namespace DeathStackBoard +{ + +TMTCController::TMTCController() + : FSM(&TMTCController::stateGroundTM, skywardStack(16 * 1024)) +{ + // init FSM + sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker->subscribe(this, TOPIC_TMTC); + + LOG_DEBUG(log, "Created TMTCController"); +} + +TMTCController::~TMTCController() { sEventBroker->unsubscribe(this); } + +bool TMTCController::send(const uint8_t tm_id) +{ + MavDriver* mav_driver = DeathStack::getInstance()->radio->mav_driver; + TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + // enqueue the TM packet taking it from the TM repo (pauses kernel to + // guarantee synchronicity) + bool ok = mav_driver->enqueueMsg(tm_repo->packTM(tm_id)); + // update status + DeathStack::getInstance()->radio->logStatus(); + return ok; +} + +void TMTCController::sendSerialTelemetry() +{ + // avoid spamming on serial of not in debug mode + // TODO : this has a problem, mavlink bytes are printed along with debug + // messages, which confuses everything +#ifdef DEBUG + TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + uint8_t buf[256]; + mavlink_message_t msg = tm_repo->packTM(MAV_HR_TM_ID); + uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + fwrite(buf, sizeof(uint8_t), len, stdout); + fflush(stdout); +#endif +} + +void TMTCController::stateGroundTM(const Event& ev) +{ + // TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + switch (ev.sig) + { + case EV_ENTRY: + { + // add periodic events + periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + Event{EV_SEND_HR_TM}, TOPIC_TMTC); + // periodicSensEvId = + // sEventBroker->postDelayed<GROUND_SENS_TM_TIMEOUT>( + // Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + + LOG_DEBUG(log, "Entering stateGroundTM"); + + // log stack usage + StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + break; + } + case EV_EXIT: + { + // remove periodic events + sEventBroker->removeDelayed(periodicHrEvId); + sEventBroker->removeDelayed(periodicSensEvId); + + LOG_DEBUG(log, "Exiting stateGroundTM"); + break; + } + case EV_SEND_HR_TM: + { + // repost periodic event + periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + Event{EV_SEND_HR_TM}, TOPIC_TMTC); + + send(MAV_HR_TM_ID); + + break; + } + case EV_SEND_SENS_TM: + { + // LOG_DEBUG(log, "Sending SENS_TM"); + // periodicSensEvId = + // sEventBroker->postDelayed<GROUND_SENS_TM_TIMEOUT>( + // Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + + // send tm + send(MAV_SENSORS_TM_ID); + break; + } + case EV_TC_START_SENSOR_TM: + { + transition(&TMTCController::stateSensorTM); + break; + } + case EV_TC_SERIAL_TM: + { + transition(&TMTCController::stateSerialDebugTM); + break; + } + case EV_ARMED: + case EV_LIFTOFF: + { + transition(&TMTCController::stateFlightTM); + break; + } + default: + break; + } +} + +void TMTCController::stateFlightTM(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + { + // add periodic events + periodicLrEvId = sEventBroker->postDelayed<LR_TM_TIMEOUT>( + Event{EV_SEND_LR_TM}, TOPIC_TMTC); + periodicHrEvId = sEventBroker->postDelayed<HR_TM_TIMEOUT>( + Event{EV_SEND_HR_TM}, TOPIC_TMTC); + + LOG_DEBUG(log, "Entering stateFlightTM"); + + // log stack usage + StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + break; + } + + case EV_EXIT: + { + // remove periodic events + sEventBroker->removeDelayed(periodicLrEvId); + sEventBroker->removeDelayed(periodicHrEvId); + + LOG_DEBUG(log, "Exiting stateFlightTM"); + break; + } + case EV_SEND_HR_TM: + { + // repost periodic event + periodicHrEvId = sEventBroker->postDelayed<HR_TM_TIMEOUT>( + Event{EV_SEND_HR_TM}, TOPIC_TMTC); + + // send tm once 4 packets are filled + send(MAV_HR_TM_ID); + // bool full = tm_repo->updateHR(); + // if (full) + // { + // send(MAV_HR_TM_ID); + // } + break; + } + case EV_SEND_LR_TM: + { + // repost periodic event + periodicLrEvId = sEventBroker->postDelayed<LR_TM_TIMEOUT>( + Event{EV_SEND_LR_TM}, TOPIC_TMTC); + + // send low rate tm + send(MAV_LR_TM_ID); + + break; + } + case EV_DISARMED: + { + transition(&TMTCController::stateGroundTM); + break; + } + + default: + break; + } +} + +void TMTCController::stateSensorTM(const Event& ev) +{ + // TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + switch (ev.sig) + { + case EV_ENTRY: + { + // add periodic events + periodicSensEvId = sEventBroker->postDelayed<SENS_TM_TIMEOUT>( + Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + + LOG_DEBUG(log, "Entering stateSensorTM"); + + // log stack usage + StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + break; + } + case EV_EXIT: + { + // remove periodic events + sEventBroker->removeDelayed(periodicSensEvId); + + LOG_DEBUG(log, "Exiting stateSensorTM"); + break; + } + case EV_SEND_SENS_TM: + { + // repost periodic event + periodicSensEvId = sEventBroker->postDelayed<SENS_TM_TIMEOUT>( + Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + + send(MAV_SENSORS_TM_ID); + + break; + } + case EV_TC_STOP_SENSOR_TM: + { + transition(&TMTCController::stateGroundTM); + break; + } + case EV_ARMED: + case EV_LIFTOFF: + { + transition(&TMTCController::stateFlightTM); + break; + } + default: + break; + } +} + +void TMTCController::stateSerialDebugTM(const Event& ev) +{ + // TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + switch (ev.sig) + { + case EV_ENTRY: + { + // add periodic events + periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); + + LOG_DEBUG(log, "Entering stateSerialDebugTM"); + + // log stack usage + StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + break; + } + case EV_EXIT: + { + // remove periodic events + sEventBroker->removeDelayed(periodicHrEvId); + + LOG_DEBUG(log, "Exiting stateSerialDebugTM"); + break; + } + case EV_SEND_HR_TM_OVER_SERIAL: + { + // repost periodic event + periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); + + sendSerialTelemetry(); + + break; + } + case EV_TC_RADIO_TM: + { + transition(&TMTCController::stateGroundTM); + break; + } + default: + break; + } +} + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h new file mode 100644 index 0000000000000000000000000000000000000000..28b6f93aeeeec73d81afdcd2b532c989372de80a --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h @@ -0,0 +1,90 @@ +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <DeathStack.h> +#include <LoggerService/LoggerService.h> +#include <configs/TMTCConfig.h> +#include <events/Events.h> +#include <events/FSM.h> + +namespace DeathStackBoard +{ + +/** + * @brief This class handles the communication with the Ground Station via the + * Mavlink protocol. + * + * It is responsible of: + * - sending periodic telemetries to GS + * - receiving and handling commands coming from GS + * - fetching the last logged values when sending telemetries + */ +class TMTCController : public FSM<TMTCController> +{ +public: + /** + * @brief Constructor + */ + TMTCController(); + + /** + * @brief Destructor + */ + ~TMTCController(); + +private: + /** + * @brief Non-blocking send. Adds the message to an out queue and logs the + * status. + * + * @param tm_id The id of the TM to be sent. Sends a NACK if the tm was not + * found. + * @return false if the message could not be enqueued for sending + */ + bool send(const uint8_t tm_id); + + /** + * @brief Outputs mavlink HR_TM messages on serial port. + */ + void sendSerialTelemetry(); + + // State handlers + void stateGroundTM(const Event& ev); + void stateSensorTM(const Event& ev); + void stateFlightTM(const Event& ev); + void stateSerialDebugTM(const Event& ev); + + LoggerService& logger = *(LoggerService::getInstance()); + + uint16_t periodicHrEvId = 0; + uint16_t periodicLrEvId = 0; + uint16_t periodicSensEvId = 0; + uint16_t periodicTestEvId = 0; + + uint8_t hrPktCounter = 0; + + PrintLogger log = Logging::getLogger("deathstack.fsm.tmtc"); +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9f3a842eb3f0fdb19db8be2f7d9b87a68110d8ba --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp @@ -0,0 +1,942 @@ +/** + * Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Alvise de' Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Constants.h> +#include <DeathStack.h> +#include <Debug.h> +#include <FlightStatsRecorder/FSRController.h> +#include <LoggerService/LoggerService.h> +#include <System/TaskID.h> +#include <TelemetriesTelecommands/TmRepository.h> +#include <configs/SensorsConfig.h> +#include <configs/TMTCConfig.h> + +namespace DeathStackBoard +{ + +TmRepository::TmRepository() { stats_rec.start(); } + +TmRepository::~TmRepository() {} + +/* TM getter */ + +mavlink_message_t TmRepository::packTM(uint8_t req_tm, uint8_t sys_id, + uint8_t comp_id) +{ + mavlink_message_t m; + mavlink_nack_tm_t nack_tm; + + miosix::PauseKernelLock kLock; + + switch (req_tm) + { + case MavTMList::MAV_SYS_TM_ID: + tm_repository.sys_tm.timestamp = miosix::getTick(); + mavlink_msg_sys_tm_encode(sys_id, comp_id, &m, + &(tm_repository.sys_tm)); + break; + case MavTMList::MAV_FMM_TM_ID: + tm_repository.fmm_tm.timestamp = miosix::getTick(); + mavlink_msg_fmm_tm_encode(sys_id, comp_id, &m, + &(tm_repository.fmm_tm)); + break; + case MavTMList::MAV_PIN_OBS_TM_ID: + tm_repository.pin_obs_tm.timestamp = miosix::getTick(); + mavlink_msg_pin_obs_tm_encode(sys_id, comp_id, &m, + &(tm_repository.pin_obs_tm)); + break; + case MavTMList::MAV_LOGGER_TM_ID: + tm_repository.logger_tm.timestamp = miosix::getTick(); + mavlink_msg_logger_tm_encode(sys_id, comp_id, &m, + &(tm_repository.logger_tm)); + break; + case MavTMList::MAV_TMTC_TM_ID: + tm_repository.tmtc_tm.timestamp = miosix::getTick(); + mavlink_msg_tmtc_tm_encode(sys_id, comp_id, &m, + &(tm_repository.tmtc_tm)); + break; + case MavTMList::MAV_TASK_STATS_TM_ID: + tm_repository.task_stats_tm.timestamp = miosix::getTick(); + mavlink_msg_task_stats_tm_encode(sys_id, comp_id, &m, + &(tm_repository.task_stats_tm)); + break; + case MavTMList::MAV_DPL_TM_ID: + tm_repository.dpl_tm.timestamp = miosix::getTick(); + mavlink_msg_dpl_tm_encode(sys_id, comp_id, &m, + &(tm_repository.dpl_tm)); + break; + case MavTMList::MAV_ADA_TM_ID: + tm_repository.ada_tm.timestamp = miosix::getTick(); + mavlink_msg_ada_tm_encode(sys_id, comp_id, &m, + &(tm_repository.ada_tm)); + break; + case MavTMList::MAV_ABK_TM_ID: + tm_repository.abk_tm.timestamp = miosix::getTick(); + mavlink_msg_abk_tm_encode(sys_id, comp_id, &m, + &(tm_repository.abk_tm)); + break; + case MavTMList::MAV_NAS_TM_ID: + tm_repository.nas_tm.timestamp = miosix::getTick(); + mavlink_msg_nas_tm_encode(sys_id, comp_id, &m, + &(tm_repository.nas_tm)); + break; + case MavTMList::MAV_ADC_TM_ID: + tm_repository.adc_tm.timestamp = miosix::getTick(); + mavlink_msg_adc_tm_encode(sys_id, comp_id, &m, + &(tm_repository.adc_tm)); + break; + case MavTMList::MAV_MS5803_TM_ID: + tm_repository.digital_baro_tm.timestamp = miosix::getTick(); + mavlink_msg_ms5803_tm_encode(sys_id, comp_id, &m, + &(tm_repository.digital_baro_tm)); + break; + case MavTMList::MAV_BMX160_TM_ID: + tm_repository.bmx_tm.timestamp = miosix::getTick(); + mavlink_msg_bmx160_tm_encode(sys_id, comp_id, &m, + &(tm_repository.bmx_tm)); + break; + case MavTMList::MAV_LIS3MDL_TM_ID: + tm_repository.lis3mdl_tm.timestamp = miosix::getTick(); + mavlink_msg_lis3mdl_tm_encode(sys_id, comp_id, &m, + &(tm_repository.lis3mdl_tm)); + break; + case MavTMList::MAV_GPS_TM_ID: + tm_repository.gps_tm.timestamp = miosix::getTick(); + mavlink_msg_gps_tm_encode(sys_id, comp_id, &m, + &(tm_repository.gps_tm)); + break; + case MavTMList::MAV_HR_TM_ID: + tm_repository.hr_tm.timestamp = miosix::getTick(); + mavlink_msg_hr_tm_encode(sys_id, comp_id, &m, + &(tm_repository.hr_tm)); + break; + case MavTMList::MAV_LR_TM_ID: + // tm_repository.tm_repository.lr_tm.timestamp = miosix::getTick(); + mavlink_msg_lr_tm_encode(sys_id, comp_id, &m, + &(tm_repository.lr_tm)); + break; + case MavTMList::MAV_TEST_TM_ID: + tm_repository.test_tm.timestamp = miosix::getTick(); + mavlink_msg_test_tm_encode(sys_id, comp_id, &m, + &(tm_repository.test_tm)); + break; + case MavTMList::MAV_WINDTUNNEL_TM_ID: + tm_repository.wind_tm.timestamp = miosix::getTick(); + mavlink_msg_windtunnel_tm_encode(sys_id, comp_id, &m, + &(tm_repository.wind_tm)); + break; + case MavTMList::MAV_SENSORS_TM_ID: + tm_repository.sensors_tm.timestamp = miosix::getTick(); + mavlink_msg_sensors_tm_encode(sys_id, comp_id, &m, + &(tm_repository.sensors_tm)); + break; + default: + { + LOG_DEBUG(log, "Unknown telemetry id: %d", req_tm); + nack_tm.recv_msgid = 0; + nack_tm.seq_ack = 0; + mavlink_msg_nack_tm_encode(sys_id, comp_id, &m, &nack_tm); + break; + } + } + + return m; +} + +/* Components TM upaters */ + +template <> +void TmRepository::update<AirBrakesControllerStatus>( + const AirBrakesControllerStatus& t) +{ + tm_repository.abk_tm.state = (uint8_t)t.state; + + tm_repository.hr_tm.ab_state = (uint8_t)t.state; +} + +template <> +void TmRepository::update<AirBrakesChosenTrajectory>( + const AirBrakesChosenTrajectory& t) +{ + tm_repository.abk_tm.trajectory = t.trajectory; +} + +template <> +void TmRepository::update<AirBrakesData>(const AirBrakesData& t) +{ + tm_repository.wind_tm.ab_angle = t.servo_position; + + tm_repository.abk_tm.servo_position = t.servo_position; + tm_repository.abk_tm.estimated_cd = t.estimated_cd; + tm_repository.abk_tm.pid_error = t.pid_error; + + tm_repository.hr_tm.ab_angle = t.servo_position; + tm_repository.hr_tm.ab_estimated_cd = t.estimated_cd; +} + +template <> +void TmRepository::update<AirBrakesAlgorithmData>( + const AirBrakesAlgorithmData& t) +{ + tm_repository.abk_tm.z = t.z; + tm_repository.abk_tm.vz = t.vz; + tm_repository.abk_tm.v_mod = t.vMod; +} + +/* +template <> +void TmRepository::update<CurrentSensorData>(const CurrentSensorData& t) +{ + if (t.channel_id == DeathStackBoard::SensorConfigs::ADC_CS_CUTTER_PRIMARY) + { + tm_repository.sensors_tm.c_sense_1 = t.current; + tm_repository.hr_tm.csense1 = t.current; + tm_repository.adc_tm.csense1 = t.current; + } + else if (t.channel_id == + DeathStackBoard::SensorConfigs::ADC_CS_CUTTER_BACKUP) + { + tm_repository.sensors_tm.c_sense_2 = t.current; + tm_repository.hr_tm.csense2 = t.current; + tm_repository.adc_tm.csense2 = t.current; + } + + stats_rec.update(t); +} +*/ + +template <> +void TmRepository::update<BatteryVoltageSensorData>( + const BatteryVoltageSensorData& t) +{ + if (t.channel_id == DeathStackBoard::SensorConfigs::ADC_BATTERY_VOLTAGE) + { + tm_repository.hr_tm.vbat = t.bat_voltage; + tm_repository.sensors_tm.vbat = t.bat_voltage; + tm_repository.adc_tm.bat_voltage = t.bat_voltage; + } +} + +template <> +void TmRepository::update<ADS1118Data>(const ADS1118Data& t) +{ + if (t.channel_id == SensorConfigs::ADC_CH_VREF) + { + // tm_repository.wind_tm.pressure_dpl = t.voltage; + tm_repository.sensors_tm.vsupply_5v = t.voltage; + tm_repository.hr_tm.vsupply_5v = t.voltage; + tm_repository.adc_tm.bat_voltage_5v = t.voltage; + } +} + +template <> +void TmRepository::update<MS5803Data>(const MS5803Data& t) +{ +#ifndef HARDWARE_IN_THE_LOOP + tm_repository.wind_tm.pressure_digital = t.press; + tm_repository.sensors_tm.ms5803_press = t.press; + + tm_repository.digital_baro_tm.pressure = t.press; + + tm_repository.hr_tm.pressure_digi = t.press; +#endif + + tm_repository.sensors_tm.ms5803_temp = t.temp; + tm_repository.hr_tm.temperature = t.temp; + tm_repository.digital_baro_tm.temperature = t.temp; + + stats_rec.update(t); +} + +template <> +void TmRepository::update<MPXHZ6130AData>(const MPXHZ6130AData& t) +{ + tm_repository.wind_tm.pressure_static = t.press; + tm_repository.sensors_tm.static_press = t.press; + tm_repository.adc_tm.static_pressure = t.press; + + stats_rec.update(t); +} + +template <> +void TmRepository::update<SSCDRRN015PDAData>(const SSCDRRN015PDAData& t) +{ + tm_repository.wind_tm.pressure_differential = t.press; + tm_repository.sensors_tm.pitot_press = t.press; + tm_repository.adc_tm.pitot_pressure = t.press; +} + +template <> +void TmRepository::update<AirSpeedPitot>(const AirSpeedPitot& t) +{ + tm_repository.hr_tm.airspeed_pitot = fabs(t.airspeed); + + stats_rec.update(t); +} + +template <> +void TmRepository::update<SSCDANN030PAAData>(const SSCDANN030PAAData& t) +{ + tm_repository.wind_tm.pressure_dpl = t.press; + tm_repository.sensors_tm.dpl_press = t.press; + tm_repository.hr_tm.pressure_dpl = t.press; + tm_repository.adc_tm.dpl_pressure = t.press; + + stats_rec.update(t); +} + +#ifndef HARDWARE_IN_THE_LOOP +template <> +void TmRepository::update<BMX160Data>(const BMX160Data& t) +{ + tm_repository.sensors_tm.bmx160_acc_x = t.accel_x; + tm_repository.sensors_tm.bmx160_acc_y = t.accel_y; + tm_repository.sensors_tm.bmx160_acc_z = t.accel_z; + tm_repository.sensors_tm.bmx160_gyro_x = t.gyro_x; + tm_repository.sensors_tm.bmx160_gyro_y = t.gyro_y; + tm_repository.sensors_tm.bmx160_gyro_z = t.gyro_z; + tm_repository.sensors_tm.bmx160_mag_x = t.mag_x; + tm_repository.sensors_tm.bmx160_mag_y = t.mag_y; + tm_repository.sensors_tm.bmx160_mag_z = t.mag_z; +} + +template <> +void TmRepository::update<BMX160WithCorrectionData>( + const BMX160WithCorrectionData& t) +{ + tm_repository.bmx_tm.acc_x = t.accel_x; + tm_repository.bmx_tm.acc_y = t.accel_y; + tm_repository.bmx_tm.acc_z = t.accel_z; + tm_repository.bmx_tm.gyro_x = t.gyro_x; + tm_repository.bmx_tm.gyro_y = t.gyro_y; + tm_repository.bmx_tm.gyro_z = t.gyro_z; + tm_repository.bmx_tm.mag_x = t.mag_x; + tm_repository.bmx_tm.mag_y = t.mag_y; + tm_repository.bmx_tm.mag_z = t.mag_z; + + tm_repository.hr_tm.acc_x = t.accel_x; + tm_repository.hr_tm.acc_y = t.accel_y; + tm_repository.hr_tm.acc_z = t.accel_z; + tm_repository.hr_tm.gyro_x = t.gyro_x; + tm_repository.hr_tm.gyro_y = t.gyro_y; + tm_repository.hr_tm.gyro_z = t.gyro_z; + tm_repository.hr_tm.mag_x = t.mag_x; + tm_repository.hr_tm.mag_y = t.mag_y; + tm_repository.hr_tm.mag_z = t.mag_z; + + stats_rec.update(t); +} +#endif + +template <> +void TmRepository::update<BMX160Temperature>(const BMX160Temperature& t) +{ + tm_repository.sensors_tm.bmx160_temp = t.temp; + tm_repository.bmx_tm.temp = t.temp; +} + +template <> +void TmRepository::update<LIS3MDLData>(const LIS3MDLData& t) +{ + tm_repository.sensors_tm.lis3mdl_mag_x = t.mag_x; + tm_repository.sensors_tm.lis3mdl_mag_y = t.mag_y; + tm_repository.sensors_tm.lis3mdl_mag_z = t.mag_z; + tm_repository.sensors_tm.lis3mdl_temp = t.temp; + + tm_repository.lis3mdl_tm.mag_x = t.mag_x; + tm_repository.lis3mdl_tm.mag_y = t.mag_y; + tm_repository.lis3mdl_tm.mag_z = t.mag_z; + tm_repository.lis3mdl_tm.temp = t.temp; +} + +#ifndef HARDWARE_IN_THE_LOOP +/** + * @brief GPS. + */ +template <> +void TmRepository::update<UbloxGPSData>(const UbloxGPSData& t) +{ + // GPS_TM + tm_repository.gps_tm.latitude = t.latitude; + tm_repository.gps_tm.longitude = t.longitude; + tm_repository.gps_tm.height = t.height; + tm_repository.gps_tm.vel_north = t.velocity_north; + tm_repository.gps_tm.vel_east = t.velocity_east; + tm_repository.gps_tm.vel_down = t.velocity_down; + tm_repository.gps_tm.speed = t.speed; + tm_repository.gps_tm.fix = (uint8_t)t.fix; + tm_repository.gps_tm.track = t.track; + tm_repository.gps_tm.n_satellites = t.num_satellites; + + // HR TM + tm_repository.hr_tm.gps_lat = t.latitude; + tm_repository.hr_tm.gps_lon = t.longitude; + tm_repository.hr_tm.gps_alt = t.height; + tm_repository.hr_tm.gps_fix = (uint8_t)t.fix; + + // TEST TM + tm_repository.test_tm.gps_nsats = t.num_satellites; + + stats_rec.update(t); +} +#endif + +template <> +void TmRepository::update<SensorsStatus>(const SensorsStatus& t) +{ + tm_repository.sys_tm.bmx160_status = t.bmx160; + tm_repository.sys_tm.ms5803_status = t.ms5803; + tm_repository.sys_tm.lis3mdl_status = t.lis3mdl; + tm_repository.sys_tm.gps_status = t.gps; + tm_repository.sys_tm.internal_adc_status = t.internal_adc; + tm_repository.sys_tm.ads1118_status = t.ads1118; +} + +template <> +void TmRepository::update<Xbee::ATCommandResponseFrameLog>( + const Xbee::ATCommandResponseFrameLog& t) +{ + if (strncmp(t.at_command, "DB", 2) == 0 && t.command_data_length == 1) + { + tm_repository.wind_tm.last_RSSI = -t.command_data[0]; + } +} + +template <> +void TmRepository::update<DeathStackStatus>(const DeathStackStatus& t) +{ + tm_repository.sys_tm.death_stack = t.death_stack; + tm_repository.sys_tm.logger = t.logger; + tm_repository.sys_tm.ev_broker = t.ev_broker; + tm_repository.sys_tm.pin_obs = t.pin_obs; + tm_repository.sys_tm.sensors = t.sensors; + tm_repository.sys_tm.radio = t.radio; + tm_repository.sys_tm.state_machines = t.state_machines; +} + +/** + * @brief Flight Mode Manager. + */ +template <> +void TmRepository::update<FMMStatus>(const FMMStatus& t) +{ + tm_repository.fmm_tm.state = static_cast<uint8_t>(t.state); + tm_repository.hr_tm.fmm_state = static_cast<uint8_t>(t.state); +} + +template <> +void TmRepository::update<NASStatus>(const NASStatus& t) +{ + tm_repository.nas_tm.state = static_cast<uint8_t>(t.state); + tm_repository.hr_tm.nas_state = static_cast<uint8_t>(t.state); +} + +template <> +void TmRepository::update<NASKalmanState>(const NASKalmanState& t) +{ + Vector3f orientation = t.toEul(); + + tm_repository.nas_tm.x0 = t.x0; + tm_repository.nas_tm.x1 = t.x1; + tm_repository.nas_tm.x2 = t.x2; + tm_repository.nas_tm.x3 = t.x3; + tm_repository.nas_tm.x4 = t.x4; + tm_repository.nas_tm.x5 = t.x5; + tm_repository.nas_tm.x6 = t.x6; + tm_repository.nas_tm.x7 = t.x7; + tm_repository.nas_tm.x8 = t.x8; + tm_repository.nas_tm.x9 = t.x9; + tm_repository.nas_tm.x10 = t.x10; + tm_repository.nas_tm.x11 = t.x11; + tm_repository.nas_tm.x12 = t.x12; + tm_repository.nas_tm.roll = orientation(0); + tm_repository.nas_tm.pitch = orientation(1); + tm_repository.nas_tm.yaw = orientation(2); + + tm_repository.hr_tm.nas_x = t.x0; + tm_repository.hr_tm.nas_y = t.x1; + // Altitude has negative sign because we're working in the NED + // frame but we want a positive altitude as output (same for vz) + tm_repository.hr_tm.nas_z = -t.x2; + tm_repository.hr_tm.nas_vx = t.x3; + tm_repository.hr_tm.nas_vy = t.x4; + tm_repository.hr_tm.nas_vz = -t.x5; + tm_repository.hr_tm.nas_roll = orientation(0); + tm_repository.hr_tm.nas_pitch = orientation(1); + tm_repository.hr_tm.nas_yaw = orientation(2); + tm_repository.hr_tm.nas_bias0 = t.x10; + tm_repository.hr_tm.nas_bias1 = t.x11; + tm_repository.hr_tm.nas_bias2 = t.x12; +} + +template <> +void TmRepository::update<NASReferenceValues>(const NASReferenceValues& t) +{ + tm_repository.nas_tm.ref_accel_x = t.ref_accel_x; + tm_repository.nas_tm.ref_accel_y = t.ref_accel_y; + tm_repository.nas_tm.ref_accel_z = t.ref_accel_z; + tm_repository.nas_tm.ref_mag_x = t.ref_mag_x; + tm_repository.nas_tm.ref_mag_y = t.ref_mag_y; + tm_repository.nas_tm.ref_mag_z = t.ref_mag_z; + + tm_repository.nas_tm.ref_latitude = t.ref_latitude; + tm_repository.nas_tm.ref_longitude = t.ref_longitude; + + tm_repository.nas_tm.ref_pressure = t.ref_pressure; + tm_repository.nas_tm.ref_temperature = t.ref_temperature; +} + +template <> +void TmRepository::update<NASTriadResult>(const NASTriadResult& t) +{ + tm_repository.nas_tm.triad_roll = t.roll; + tm_repository.nas_tm.triad_pitch = t.pitch; + tm_repository.nas_tm.triad_yaw = t.yaw; +} + +/** + * @brief Launch and nosecone detachment pins. + */ +template <> +void TmRepository::update<PinStatus>(const PinStatus& t) +{ + switch (t.pin) + { + + case ObservedPin::LAUNCH: + { + tm_repository.pin_obs_tm.pin_launch_last_change = + t.last_state_change / 1000; + tm_repository.pin_obs_tm.pin_launch_num_changes = + t.num_state_changes; + tm_repository.pin_obs_tm.pin_launch_state = t.state; + // HR TM + tm_repository.hr_tm.pin_launch = t.state; + break; + } + case ObservedPin::NOSECONE: + { + tm_repository.pin_obs_tm.pin_nosecone_last_change = + t.last_state_change / 1000; + tm_repository.pin_obs_tm.pin_nosecone_num_changes = + t.num_state_changes; + tm_repository.pin_obs_tm.pin_nosecone_state = t.state; + + // HR TM + tm_repository.hr_tm.pin_nosecone = t.state; + break; + } + case ObservedPin::DPL_SERVO: + { + tm_repository.pin_obs_tm.pin_dpl_servo_last_change = + t.last_state_change / 1000; + tm_repository.pin_obs_tm.pin_dpl_servo_num_changes = + t.num_state_changes; + tm_repository.pin_obs_tm.pin_dpl_servo_state = t.state; + + // HR TM + tm_repository.hr_tm.servo_sensor = t.state; + break; + } + default: + break; + } +} + +/** + * @brief Logger. + */ +template <> +void TmRepository::update<LogStats>(const LogStats& t) +{ + tm_repository.logger_tm.statLogNumber = t.logNumber; + tm_repository.logger_tm.statTooLargeSamples = t.statTooLargeSamples; + tm_repository.logger_tm.statDroppedSamples = t.statDroppedSamples; + tm_repository.logger_tm.statQueuedSamples = t.statQueuedSamples; + tm_repository.logger_tm.statBufferFilled = t.statBufferFilled; + tm_repository.logger_tm.statBufferWritten = t.statBufferWritten; + tm_repository.logger_tm.statWriteFailed = t.statWriteFailed; + tm_repository.logger_tm.statWriteError = t.statWriteError; + tm_repository.logger_tm.statWriteTime = t.statWriteTime; + tm_repository.logger_tm.statMaxWriteTime = t.statMaxWriteTime; + + tm_repository.wind_tm.log_num = t.logNumber; + tm_repository.wind_tm.log_status = t.opened ? t.statWriteError : -1000; + + tm_repository.hr_tm.logger_error = t.opened ? t.statWriteError : -1; +} + +/** + * @brief TMTCController (Mavlink). + */ +template <> +void TmRepository::update<MavlinkStatus>(const MavlinkStatus& t) +{ + // mavchannel stats + tm_repository.tmtc_tm.n_send_queue = t.n_send_queue; + tm_repository.tmtc_tm.max_send_queue = t.max_send_queue; + tm_repository.tmtc_tm.n_send_errors = t.n_send_errors; + tm_repository.tmtc_tm.msg_received = t.mav_stats.msg_received; + // mav stats + tm_repository.tmtc_tm.buffer_overrun = t.mav_stats.buffer_overrun; + tm_repository.tmtc_tm.parse_error = t.mav_stats.parse_error; + tm_repository.tmtc_tm.parse_state = t.mav_stats.parse_state; + tm_repository.tmtc_tm.packet_idx = t.mav_stats.packet_idx; + tm_repository.tmtc_tm.current_rx_seq = t.mav_stats.current_rx_seq; + tm_repository.tmtc_tm.current_tx_seq = t.mav_stats.current_tx_seq; + tm_repository.tmtc_tm.packet_rx_success_count = + t.mav_stats.packet_rx_success_count; + tm_repository.tmtc_tm.packet_rx_drop_count = + t.mav_stats.packet_rx_drop_count; +} + +/** + *@brief Sensor Manager. + */ +// template <> +// void TmRepository::update<SensorManagerStatus>(const SensorManagerStatus& +// t) +// { +// tm_repository.sm_tm.sensor_state = t.sensor_status; +// tm_repository.sm_tm.state = (uint8_t)t.state; +// } + +/** + *@brief Deployment Controller. + */ +template <> +void TmRepository::update<DeploymentStatus>(const DeploymentStatus& t) +{ + tm_repository.dpl_tm.fsm_state = (uint8_t)t.state; + tm_repository.dpl_tm.cutters_enabled = (uint8_t)t.cutters_enabled; + tm_repository.dpl_tm.servo_position = t.servo_position; + + // HR TM + tm_repository.hr_tm.dpl_state = (uint8_t)t.state; +} + +/** + * @brief ADA state machine. + */ +template <> +void TmRepository::update<ADAControllerStatus>(const ADAControllerStatus& t) +{ + tm_repository.ada_tm.state = (uint8_t)t.state; + tm_repository.ada_tm.apogee_reached = t.apogee_reached; + tm_repository.ada_tm.aerobrakes_disabled = t.disable_airbrakes; + tm_repository.ada_tm.dpl_altitude_reached = t.dpl_altitude_reached; + + tm_repository.hr_tm.ada_state = (uint8_t)t.state; +} + +/** + * @brief ADA target dpl pressure. + */ +template <> +void TmRepository::update<TargetDeploymentAltitude>( + const TargetDeploymentAltitude& t) +{ + tm_repository.ada_tm.target_dpl_altitude = t.deployment_altitude; +} + +/** + * @brief ADA kalman filter values. + */ +template <> +void TmRepository::update<ADAKalmanState>(const ADAKalmanState& t) +{ + tm_repository.ada_tm.kalman_x0 = t.x0; + tm_repository.ada_tm.kalman_x1 = t.x1; + tm_repository.ada_tm.kalman_x2 = t.x2; + + // tm_repository.ada_tm.kalman_acc_x0 = t.x0_acc; + // tm_repository.ada_tm.kalman_acc_x1 = t.x1_acc; + // tm_repository.ada_tm.kalman_acc_x2 = t.x2_acc; + + // HR_TM + tm_repository.hr_tm.pressure_ada = t.x0; + tm_repository.hr_tm.ada_vert_accel = t.x2; + + stats_rec.update(t); +} + +/** + * @brief ADA kalman altitude values. + */ +template <> +void TmRepository::update<ADAData>(const ADAData& t) +{ + tm_repository.ada_tm.msl_altitude = t.msl_altitude; + tm_repository.ada_tm.vert_speed = t.vert_speed; + + tm_repository.hr_tm.msl_altitude = t.msl_altitude; + tm_repository.hr_tm.ada_vert_speed = t.vert_speed; + + stats_rec.update(t); +} + +template <> +void TmRepository::update<ADAReferenceValues>(const ADAReferenceValues& t) +{ + tm_repository.ada_tm.msl_pressure = t.msl_pressure; + tm_repository.ada_tm.msl_temperature = t.msl_temperature; + + tm_repository.ada_tm.ref_altitude = t.ref_altitude; + tm_repository.ada_tm.ref_pressure = t.ref_pressure; + tm_repository.ada_tm.ref_temperature = + t.ref_temperature - 273.15; // send temperature in °C +} + +template <> +void TmRepository::update<TaskStatResult>(const TaskStatResult& t) +{ + switch (t.id) + { + case TASK_SENSORS_6_MS_ID: + tm_repository.task_stats_tm.task_sensors_6ms_max = + t.periodStats.maxValue; + tm_repository.task_stats_tm.task_sensors_6ms_min = + t.periodStats.minValue; + tm_repository.task_stats_tm.task_sensors_6ms_mean = + t.periodStats.mean; + tm_repository.task_stats_tm.task_sensors_6ms_stddev = + t.periodStats.stdev; + break; + case TASK_SENSORS_15_MS_ID: + tm_repository.task_stats_tm.task_sensors_15ms_max = + t.periodStats.maxValue; + tm_repository.task_stats_tm.task_sensors_15ms_min = + t.periodStats.minValue; + tm_repository.task_stats_tm.task_sensors_15ms_mean = + t.periodStats.mean; + tm_repository.task_stats_tm.task_sensors_15ms_stddev = + t.periodStats.stdev; + break; + case TASK_SENSORS_20_MS_ID: + tm_repository.task_stats_tm.task_sensors_20ms_max = + t.periodStats.maxValue; + tm_repository.task_stats_tm.task_sensors_20ms_min = + t.periodStats.minValue; + tm_repository.task_stats_tm.task_sensors_20ms_mean = + t.periodStats.mean; + tm_repository.task_stats_tm.task_sensors_20ms_stddev = + t.periodStats.stdev; + break; + case TASK_SENSORS_24_MS_ID: + tm_repository.task_stats_tm.task_sensors_24ms_max = + t.periodStats.maxValue; + tm_repository.task_stats_tm.task_sensors_24ms_min = + t.periodStats.minValue; + tm_repository.task_stats_tm.task_sensors_24ms_mean = + t.periodStats.mean; + tm_repository.task_stats_tm.task_sensors_24ms_stddev = + t.periodStats.stdev; + break; + case TASK_SENSORS_40_MS_ID: + tm_repository.task_stats_tm.task_sensors_40ms_max = + t.periodStats.maxValue; + tm_repository.task_stats_tm.task_sensors_40ms_min = + t.periodStats.minValue; + tm_repository.task_stats_tm.task_sensors_40ms_mean = + t.periodStats.mean; + tm_repository.task_stats_tm.task_sensors_40ms_stddev = + t.periodStats.stdev; + break; + case TASK_SENSORS_1000_MS_ID: + tm_repository.task_stats_tm.task_sensors_1000ms_max = + t.periodStats.maxValue; + tm_repository.task_stats_tm.task_sensors_1000ms_min = + t.periodStats.minValue; + tm_repository.task_stats_tm.task_sensors_1000ms_mean = + t.periodStats.mean; + tm_repository.task_stats_tm.task_sensors_1000ms_stddev = + t.periodStats.stdev; + break; + case TASK_ADA_ID: + tm_repository.task_stats_tm.task_ada_max = t.periodStats.maxValue; + tm_repository.task_stats_tm.task_ada_min = t.periodStats.minValue; + tm_repository.task_stats_tm.task_ada_mean = t.periodStats.mean; + tm_repository.task_stats_tm.task_ada_stddev = t.periodStats.stdev; + break; + case TASK_ABK_ID: + tm_repository.task_stats_tm.task_abk_max = t.periodStats.maxValue; + tm_repository.task_stats_tm.task_abk_min = t.periodStats.minValue; + tm_repository.task_stats_tm.task_abk_mean = t.periodStats.mean; + tm_repository.task_stats_tm.task_abk_stddev = t.periodStats.stdev; + break; + case TASK_NAS_ID: + tm_repository.task_stats_tm.task_nas_max = t.periodStats.maxValue; + tm_repository.task_stats_tm.task_nas_min = t.periodStats.minValue; + tm_repository.task_stats_tm.task_nas_mean = t.periodStats.mean; + tm_repository.task_stats_tm.task_nas_stddev = t.periodStats.stdev; + break; + // case TASK_SCHEDULER_STATS_ID: + // tm_repository.task_stats_tm.task_250hz_max = + // t.periodStats.maxValue; + // tm_repository.task_stats_tm.task_250hz_min = + // t.periodStats.minValue; + // tm_repository.task_stats_tm.task_250hz_mean = + // t.periodStats.mean; + // tm_repository.task_stats_tm.task_250hz_stddev = + // t.periodStats.stdev; break; + + default: + break; + } +} + +template <> +void TmRepository::update<SystemData>(const SystemData& t) +{ + tm_repository.lr_tm.cpu_load = t.cpu_usage; + tm_repository.lr_tm.free_heap = t.free_heap; +} + +template <> +void TmRepository::update<LiftOffStats>(const LiftOffStats& t) +{ + tm_repository.lr_tm.liftoff_ts = t.T_liftoff; + + tm_repository.lr_tm.liftoff_max_acc_ts = t.T_max_acc; + tm_repository.lr_tm.liftoff_max_acc = t.acc_max; + + tm_repository.lr_tm.max_z_speed_ts = t.T_max_speed; + tm_repository.lr_tm.max_z_speed = t.vert_speed_max; + tm_repository.lr_tm.max_airspeed_pitot = t.airspeed_pitot_max; + tm_repository.lr_tm.max_speed_altitude = t.altitude_max_speed; +} + +template <> +void TmRepository::update<ApogeeStats>(const ApogeeStats& t) +{ + tm_repository.lr_tm.apogee_ts = t.T_apogee; + + tm_repository.lr_tm.static_min_pressure = t.static_min_pressure; + tm_repository.lr_tm.digital_min_pressure = t.digital_min_pressure; + tm_repository.lr_tm.ada_min_pressure = t.ada_min_pressure; + tm_repository.lr_tm.digital_min_pressure = t.digital_min_pressure; + + tm_repository.lr_tm.baro_max_altitutde = t.baro_max_altitude; + tm_repository.lr_tm.gps_max_altitude = t.gps_max_altitude; + + tm_repository.lr_tm.apogee_lat = t.lat_apogee; + tm_repository.lr_tm.apogee_lon = t.lon_apogee; +} + +template <> +void TmRepository::update<DrogueDPLStats>(const DrogueDPLStats& t) +{ + tm_repository.lr_tm.drogue_dpl_ts = t.T_dpl; + tm_repository.lr_tm.drogue_dpl_max_acc = t.max_dpl_acc; + tm_repository.lr_tm.dpl_vane_max_pressure = t.max_dpl_vane_pressure; +} + +template <> +void TmRepository::update<MainDPLStats>(const MainDPLStats& t) +{ + tm_repository.lr_tm.main_dpl_altitude_ts = t.T_dpl; + tm_repository.lr_tm.main_dpl_altitude = t.altitude_dpl; + tm_repository.lr_tm.main_dpl_acc = t.max_dpl_acc; + tm_repository.lr_tm.main_dpl_zspeed = t.vert_speed_dpl; +} + +/* +template <> +void TmRepository::update<CutterTestStats>(const CutterTestStats& t) +{ + tm_repository.dpl_tm.primary_cutter_test_current = t.cutter_1_avg; + tm_repository.dpl_tm.backup_cutter_test_current = t.cutter_2_avg; +} +*/ + +#ifdef HARDWARE_IN_THE_LOOP +template <> +void TmRepository::update<HILImuData>(const HILImuData& t) +{ + tm_repository.sensors_tm.bmx160_acc_x = t.accel_x; + tm_repository.sensors_tm.bmx160_acc_y = t.accel_y; + tm_repository.sensors_tm.bmx160_acc_z = t.accel_z; + + tm_repository.sensors_tm.bmx160_gyro_x = t.gyro_x; + tm_repository.sensors_tm.bmx160_gyro_y = t.gyro_y; + tm_repository.sensors_tm.bmx160_gyro_z = t.gyro_z; + + tm_repository.sensors_tm.bmx160_mag_x = t.mag_x; + tm_repository.sensors_tm.bmx160_mag_y = t.mag_y; + tm_repository.sensors_tm.bmx160_mag_z = t.mag_z; + + tm_repository.hr_tm.acc_x = t.accel_x; + tm_repository.hr_tm.acc_y = t.accel_y; + tm_repository.hr_tm.acc_z = t.accel_z; + + tm_repository.hr_tm.gyro_x = t.gyro_x; + tm_repository.hr_tm.gyro_y = t.gyro_y; + tm_repository.hr_tm.gyro_z = t.gyro_z; + + tm_repository.hr_tm.mag_x = t.mag_x; + tm_repository.hr_tm.mag_y = t.mag_y; + tm_repository.hr_tm.mag_z = t.mag_z; + + stats_rec.update(t); +} + +template <> +void TmRepository::update<HILBaroData>(const HILBaroData& t) +{ + tm_repository.wind_tm.pressure_digital = t.press; + tm_repository.sensors_tm.ms5803_press = t.press; + + tm_repository.hr_tm.pressure_digi = t.press; + + stats_rec.update(t); +} + +template <> +void TmRepository::update<HILGpsData>(const HILGpsData& t) +{ + // GPS_TM + tm_repository.gps_tm.latitude = t.latitude; + tm_repository.gps_tm.longitude = t.longitude; + tm_repository.gps_tm.height = t.height; + tm_repository.gps_tm.vel_north = t.velocity_north; + tm_repository.gps_tm.vel_east = t.velocity_east; + tm_repository.gps_tm.vel_down = t.velocity_down; + tm_repository.gps_tm.speed = t.speed; + tm_repository.gps_tm.fix = (uint8_t)t.fix; + tm_repository.gps_tm.track = t.track; + tm_repository.gps_tm.n_satellites = t.num_satellites; + + // HR TM + tm_repository.hr_tm.gps_lat = t.latitude; + tm_repository.hr_tm.gps_lon = t.longitude; + tm_repository.hr_tm.gps_alt = t.height; + tm_repository.hr_tm.gps_fix = (uint8_t)t.fix; + + // TEST TM + tm_repository.test_tm.gps_nsats = t.num_satellites; + + stats_rec.update(t); +} +#endif + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h new file mode 100644 index 0000000000000000000000000000000000000000..ca29482889dfd645563a41f09b252596af106b41 --- /dev/null +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h @@ -0,0 +1,355 @@ +/* Copyright (c) 2019-2020 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <AirBrakes/AirBrakesData.h> +#include <ApogeeDetectionAlgorithm/ADAData.h> +#include <DeathStackStatus.h> +#include <Deployment/DeploymentData.h> +#include <FlightModeManager/FMMStatus.h> +#include <FlightStatsRecorder/FSRController.h> +#include <FlightStatsRecorder/FSRData.h> +#include <Main/SensorsData.h> +#include <NavigationAttitudeSystem/NASData.h> +#include <PinHandler/PinHandlerData.h> +#include <Singleton.h> +#include <System/SystemData.h> +#include <TelemetriesTelecommands/Mavlink.h> +#include <diagnostic/PrintLogger.h> +#include <drivers/Xbee/APIFramesLog.h> +#include <drivers/adc/ADS1118/ADS1118Data.h> +#include <drivers/gps/ublox/UbloxGPSData.h> +#include <scheduler/TaskSchedulerData.h> +#include <sensors/BMX160/BMX160WithCorrectionData.h> +#include <sensors/LIS3MDL/LIS3MDLData.h> +#include <sensors/MS5803/MS5803Data.h> +#include <sensors/analog/battery/BatteryVoltageSensorData.h> +#include <sensors/analog/current/CurrentSensorData.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAAData.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDAData.h> + +#include <configs/TMTCConfig.h> + +#ifdef HARDWARE_IN_THE_LOOP +#include <hardware_in_the_loop/HIL_sensors/HILSensors.h> +#endif + +namespace DeathStackBoard +{ + +/** + * @brief This class contains an instance of each mavlink packet that can be + * sent by the OBSW and related getter/setter functions. + * + * WARNING: These packets are updated by the LoggerService. If the + * LoggerService is not active, the values inside packets WILL NOT BE UPDATED. + */ +class TmRepository : public Singleton<TmRepository> +{ + friend class Singleton<TmRepository>; + +public: + /** + * @brief Default update function. + * + * Later in this file you will find a template-specialized version of this + * function for each loggable struct that has to be sent via telemetry. + * + * @tparam T type of the logged struct + * @param t reference to the logged struct + */ + template <typename T> + inline void update(const T& t) + { + UNUSED(t); + return; + } + + /** + * @brief Retrieve a telemetry message in packed form. + * + * @param req_tm required telemetry + * @param sys_id system id to pack it with + * @param comp_id component id to pack it with + * @return packed mavlink struct of that telemetry or a NACK_TM if + * the telemetry id was not found. + */ + mavlink_message_t packTM(uint8_t req_tm, uint8_t sys_id = TMTC_MAV_SYSID, + uint8_t comp_id = TMTC_MAV_COMPID); + + void sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id); + +private: + TmRepository(); + ~TmRepository(); + + /** + * @brief Struct containing all TMs in the form of mavlink messages. + */ + struct TmRepository_t + { + mavlink_sys_tm_t sys_tm; + mavlink_pin_obs_tm_t pin_obs_tm; + mavlink_logger_tm_t logger_tm; + mavlink_fmm_tm_t fmm_tm; + mavlink_tmtc_tm_t tmtc_tm; + mavlink_task_stats_tm_t task_stats_tm; + mavlink_dpl_tm_t dpl_tm; + mavlink_ada_tm_t ada_tm; + mavlink_abk_tm_t abk_tm; + mavlink_nas_tm_t nas_tm; + + mavlink_ms5803_tm_t digital_baro_tm; + mavlink_bmx160_tm_t bmx_tm; + mavlink_lis3mdl_tm_t lis3mdl_tm; + mavlink_adc_tm_t adc_tm; + mavlink_gps_tm_t gps_tm; + + mavlink_hr_tm_t hr_tm; + mavlink_lr_tm_t lr_tm; + mavlink_windtunnel_tm_t wind_tm; + mavlink_sensors_tm_t sensors_tm; + mavlink_test_tm_t test_tm; + } tm_repository; + + uint8_t curHrIndex = 0; + + FlightStatsRecorder stats_rec; + + PrintLogger log = Logging::getLogger("deathstack.tmrepo"); +}; + +template <> +void TmRepository::update<AirBrakesControllerStatus>( + const AirBrakesControllerStatus& t); + +template <> +void TmRepository::update<AirBrakesChosenTrajectory>( + const AirBrakesChosenTrajectory& t); + +template <> +void TmRepository::update<AirBrakesData>(const AirBrakesData& t); + +template <> +void TmRepository::update<AirBrakesAlgorithmData>( + const AirBrakesAlgorithmData& t); + +template <> +void TmRepository::update<ADS1118Data>(const ADS1118Data& t); + +template <> +void TmRepository::update<MS5803Data>(const MS5803Data& t); + +template <> +void TmRepository::update<MPXHZ6130AData>(const MPXHZ6130AData& t); + +template <> +void TmRepository::update<SSCDRRN015PDAData>(const SSCDRRN015PDAData& t); + +template <> +void TmRepository::update<AirSpeedPitot>(const AirSpeedPitot& t); + +template <> +void TmRepository::update<SSCDANN030PAAData>(const SSCDANN030PAAData& t); + +#ifndef HARDWARE_IN_THE_LOOP +template <> +void TmRepository::update<BMX160Data>(const BMX160Data& t); + +template <> +void TmRepository::update<BMX160WithCorrectionData>( + const BMX160WithCorrectionData& t); +#endif + +template <> +void TmRepository::update<BMX160Temperature>(const BMX160Temperature& t); + +template <> +void TmRepository::update<LIS3MDLData>(const LIS3MDLData& t); + +#ifndef HARDWARE_IN_THE_LOOP +template <> +void TmRepository::update<UbloxGPSData>(const UbloxGPSData& t); +#endif + +template <> +void TmRepository::update<SensorsStatus>(const SensorsStatus& t); + +/** + * @brief Battery status, sampled by internal ADC. + */ +template <> +void TmRepository::update<BatteryVoltageSensorData>( + const BatteryVoltageSensorData& t); + +/** + * @brief Cutters current sense, sampled by internal ADC. + */ +// template <> +// void TmRepository::update<CurrentSensorData>(const CurrentSensorData& t); + +template <> +void TmRepository::update<Xbee::ATCommandResponseFrameLog>( + const Xbee::ATCommandResponseFrameLog& t); + +/** + * @brief Logger. + */ +template <> +void TmRepository::update<LogStats>(const LogStats& t); + +/** + * @brief Initialization status of the board. + */ +template <> +void TmRepository::update<DeathStackStatus>(const DeathStackStatus& t); + +/** + * @brief Flight Mode Manager. + */ +template <> +void TmRepository::update<FMMStatus>(const FMMStatus& t); + +/** + * @brief Navigation System. + */ +template <> +void TmRepository::update<NASStatus>(const NASStatus& t); + +template <> +void TmRepository::update<NASKalmanState>(const NASKalmanState& t); + +template <> +void TmRepository::update<NASReferenceValues>(const NASReferenceValues& t); + +template <> +void TmRepository::update<NASTriadResult>(const NASTriadResult& t); + +/** + * @brief Launch and Nosecone detachment pins and DPL servo optical sensor. + */ +template <> +void TmRepository::update<PinStatus>(const PinStatus& t); + +/** + * @brief TMTCController (Mavlink). + */ +template <> +void TmRepository::update<MavlinkStatus>(const MavlinkStatus& t); + +/** + * @brief Sensors. + */ +// template <> +// void TmRepository::update<SensorStatus>(const SensorStatus& t); + +/** + * @brief Deployment Controller. + */ +template <> +void TmRepository::update<DeploymentStatus>(const DeploymentStatus& t); + +/** + * @brief ADA state machine. + */ +template <> +void TmRepository::update<ADAControllerStatus>(const ADAControllerStatus& t); + +/** + * @brief ADA target dpl pressure. + */ +template <> +void TmRepository::update<TargetDeploymentAltitude>( + const TargetDeploymentAltitude& t); + +/** + * @brief ADA kalman filter values. + */ +template <> +void TmRepository::update<ADAKalmanState>(const ADAKalmanState& t); + +// /* ADA kalman altitude values */ +template <> +void TmRepository::update<ADAData>(const ADAData& t); + +/** + * @brief ADA calibration reference values set by TC. + */ +template <> +void TmRepository::update<ADAReferenceValues>(const ADAReferenceValues& t); + +/** + * @brief System statistics. + */ +template <> +void TmRepository::update<SystemData>(const SystemData& t); + +/** + * @brief Sensor Manager scheduler. + */ +template <> +void TmRepository::update<TaskStatResult>(const TaskStatResult& t); + +/** + * @brief FlightStatsRecorder liftoff stats. + */ +template <> +void TmRepository::update<LiftOffStats>(const LiftOffStats& t); + +/** + * @brief FlightStatsRecorder apogee stats. + */ +template <> +void TmRepository::update<ApogeeStats>(const ApogeeStats& t); + +/** + * @brief FlightStatsRecorder drogue deployment stats. + */ +template <> +void TmRepository::update<DrogueDPLStats>(const DrogueDPLStats& t); + +/** + * @brief FlightStatsRecorder main deployment stats. + */ +template <> +void TmRepository::update<MainDPLStats>(const MainDPLStats& t); + +// /** +// * @brief FlightStatsRecorder hbridge test stats. +// */ +// template <> +// void TmRepository::update<CutterTestStats>(const CutterTestStats& t); + +#ifdef HARDWARE_IN_THE_LOOP +template <> +void TmRepository::update<HILImuData>(const HILImuData& t); + +template <> +void TmRepository::update<HILBaroData>(const HILBaroData& t); + +template <> +void TmRepository::update<HILGpsData>(const HILGpsData& t); +#endif + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/ADAConfig.h b/src/boards/DeathStack/configs/ADAConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..c569e60c596a8aee1fce662c355bd4434b7348fe --- /dev/null +++ b/src/boards/DeathStack/configs/ADAConfig.h @@ -0,0 +1,149 @@ +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <kalman/KalmanEigen.h> + +#include <Eigen/Dense> + +namespace DeathStackBoard +{ + +namespace ADAConfigs +{ + +static const unsigned int ADA_UPDATE_PERIOD = 50; // ms -> 20 hz + +static const unsigned int ADA_PRIORITY = 2; // high + +// Number of consecutive samples with negative speed after which AD is triggered +constexpr unsigned int APOGEE_N_SAMPLES = 5; +// Number of consecutive samples with negative speed after which ABK are +// disabled +constexpr unsigned int ABK_DISABLE_N_SAMPLES = 5; +// Number of consecutive samples after which the main Deployment is triggered +constexpr unsigned int DEPLOYMENT_N_SAMPLES = 5; + +// When the vertical speed is smaller than this value, apogee is detected. +// 0: Exact apogee +// > 0: Apogee detected ahead of time (while still going up) +constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5; +// When the vertical speed is smaller than this value, airbrakes are disabled. +constexpr float ABK_DISABLE_VERTICAL_SPEED_TARGET = 7.5; + +// State timeouts +#ifdef EUROC +static const unsigned int TIMEOUT_ADA_SHADOW_MODE = 8.5 * 1000; // ms +#else +static const unsigned int TIMEOUT_ADA_SHADOW_MODE = 6.5 * 1000; // ms +#endif + +static const unsigned int TIMEOUT_ADA_P_STABILIZATION = 5 * 1000; // ms + +// Number of samples used to calibrate the kalman initial state +static const unsigned int CALIBRATION_BARO_N_SAMPLES = 200; // 1200; + +// Default reference values settings +static const float DEFAULT_REFERENCE_TEMPERATURE = 288.15f; +#ifdef EUROC +static const float DEFAULT_REFERENCE_ALTITUDE = 109.0f; +static const float DEFAULT_REFERENCE_PRESSURE = 100022.4f; +#else +static const float DEFAULT_REFERENCE_ALTITUDE = 1420.0f; +static const float DEFAULT_REFERENCE_PRESSURE = 85389.4f; +#endif + +// Deployment altitude AGL +static const float DEFAULT_DEPLOYMENT_ALTITUDE = 450; + +// Do cut the drogue above this altitude +//static const float MAX_DEPLOYMENT_ALTITUDE_MSL = 2000; + +// ------ Kalman parameters ------ +static const float SAMPLING_PERIOD = ADA_UPDATE_PERIOD / 1000.0f; // in seconds + +// Initialize the Kalman filter with a negative (pressure) acceleration in order +// to make it more responsive during the propulsive phase +static const float KALMAN_INITIAL_ACCELERATION = -500; + +// kalman dimensions +static const uint8_t KALMAN_STATES_NUM = 3; +static const uint8_t KALMAN_OUTPUTS_NUM = 1; + +using MatrixNN = Matrix<float, KALMAN_STATES_NUM, KALMAN_STATES_NUM>; +using MatrixPN = Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_STATES_NUM>; +using MatrixNP = Matrix<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>; +using MatrixPP = Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_OUTPUTS_NUM>; +using CVectorN = Matrix<float, KALMAN_STATES_NUM, 1>; +using CVectorP = Matrix<float, KALMAN_OUTPUTS_NUM, 1>; + +// clang-format off +static inline MatrixNN f_init() +{ + MatrixNN f; + f << 1.0f, SAMPLING_PERIOD, 0.5f * SAMPLING_PERIOD * SAMPLING_PERIOD, + 0.0f, 1.0f, SAMPLING_PERIOD, + 0.0f, 0.0f, 1.0f; + + return f; +} + +static inline MatrixNN p_init() +{ + MatrixNN p; + p << 0.1f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f; + + return p; +} + +static inline MatrixNN q_init() +{ + MatrixNN q; + q << 1.0f, 0.0f, 0.0f, + 0.0f, 10.0f, 0.0f, + 0.0f, 0.0f, 100.0f; + + return q; +} +// clang-format on + +// kalman matrices +static const MatrixNN F_INIT = f_init(); + +// Output matrix +static const MatrixPN H_INIT{1.0f, 0.0f, 0.0f}; + +// Initial error covariance matrix +static const MatrixNN P_INIT = p_init(); + +// Model variance matrix +static const MatrixNN Q_INIT = q_init(); + +// Measurement variance +static const MatrixPP R_INIT{800.0f}; + +} // namespace ADAConfigs + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/ADA_config.h b/src/boards/DeathStack/configs/ADA_config.h deleted file mode 100644 index dd08ab73c0ac784df47383f8f47cd7890e0ebf21..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/configs/ADA_config.h +++ /dev/null @@ -1,94 +0,0 @@ -/* Copyright (c) 2018,2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once -#include "skyward-boardcore/libs/simple-template-matrix/matrix.h" - -namespace DeathStackBoard -{ -// Number of consecutive samples with negative speed after which AD is triggered -constexpr unsigned int APOGEE_N_SAMPLES = 5; - -// Number of consecutive samples after which Deployment is triggered -constexpr unsigned int DEPLOYMENT_N_SAMPLES = 5; - -// When the vertical speed is smaller than this value, apogee is detected. -// 0: Exact apogee -// > 0: Apogee detected ahead of time (while still going up) -constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5; - -// State timeouts -static const unsigned int TIMEOUT_ADA_SHADOW_MODE = 6.5 * 1000; // ms -static const unsigned int TIMEOUT_ADA_P_STABILIZATION = 5 * 1000; // ms - -// Number of samples used to calibrate the kalman initial state -static const unsigned int CALIBRATION_BARO_N_SAMPLES = 1200; -static const unsigned int ACCELERATION_AVERAGE_N_SAMPLES = 25; - -// Default reference values settings -// Standard atmosphere values @ Roccaraso -static const float DEFAULT_REFERENCE_TEMPERATURE = 278.920f; -static const float DEFAULT_REFERENCE_ALTITUDE = 1420.0f; -static const float DEFAULT_REFERENCE_PRESSURE = 85389.4f; - -static const float DEFAULT_MSL_TEMPERATURE = 288.15f; -static const float DEFAULT_MSL_PRESSURE = 101325.0f; - -// Deployment altitude AGL -// Set it under the ground level: don't deploy the Rogallo wing if we somehow -// forget to set the deployment altitude via telecommand -static const float DEFAULT_DEPLOYMENT_ALTITUDE = -100; - -// Do cut the drogue above this altitude -static const float MAX_DEPLOYMENT_ALTITUDE_MSL = 1700; - -// ------ Kalman parameters ------ -static const float SAMPLING_PERIOD = 1 / 20.0f; // In seconds - -// State matrix -// Note that sampling frequency is supposed to be constant and known at -// compile time. If this is not the case the matrix has to be updated at -// each iteration -static const MatrixBase<float, 3, 3> A_INIT( - {1.0f, SAMPLING_PERIOD, 0.5f * SAMPLING_PERIOD* SAMPLING_PERIOD, 0.0f, 1.0f, - SAMPLING_PERIOD, 0.0f, 0.0f, 1.0f}); - -// Output matrix -static const MatrixBase<float, 1, 3> C_INIT{1, 0, 0}; -static const MatrixBase<float, 2, 3> C_INIT_ACC{1, 0, 0, 0, 0, 1}; - -// Initial error covariance matrix -static const MatrixBase<float, 3, 3> P_INIT{0.1, 0, 0, 0, 0, 0, 0, 0, 0}; -static const MatrixBase<float, 3, 3> P_INIT_ACC{0.1, 0, 0, 0, 0, 0, 0, 0, 100}; - -// Model variance matrix -static const MatrixBase<float, 3, 3> V1_INIT{1, 0, 0, 0, 10, 0, 0, 0, 100}; -static const MatrixBase<float, 3, 3> V1_INIT_ACC{0.1, 0, 0, 0, 80, 1, 0, 1, 10}; - -// Measurement variance -static const MatrixBase<float, 1, 1> V2_INIT{800}; -static const MatrixBase<float, 2, 2> V2_INIT_ACC{1000,0,0,100}; - -// Initialize the Kalman filter with a negative (pressure) acceleration in order -// to make it more respondive during the propulsive phase -static const float KALMAN_INITIAL_ACCELERATION = -500; -} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/AirBrakesConfig.h b/src/boards/DeathStack/configs/AirBrakesConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..43a5873564b97c162b2d90072e024a48b52996d6 --- /dev/null +++ b/src/boards/DeathStack/configs/AirBrakesConfig.h @@ -0,0 +1,146 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Constants.h> +#include <drivers/HardwareTimer.h> +#include <drivers/pwm/pwm.h> + +namespace DeathStackBoard +{ + +namespace AirBrakesConfigs +{ + +static const PWM::Timer AB_SERVO_TIMER{ + TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; + +static constexpr PWMChannel AB_SERVO_PWM_CH = PWMChannel::CH2; + +// Rocket's parameters +#ifdef EUROC +static constexpr float M = 27.0; /**< rocket's mass */ +#else +static constexpr float M = 19.2; /**< rocket's mass */ +#endif + +static constexpr float D = 0.15; /**< rocket's diameter */ +static constexpr float S0 = (PI * D * D) / 4.0; +static constexpr float RHO = 1.225; +static constexpr float Hn = 10400.0; +static constexpr float Co = 340.3; +static constexpr float ALPHA = -3.871e-3; +static constexpr float A = -1.04034; +static constexpr float B = 0.30548; +static constexpr float A_DELTAS = -9.43386 / 1000; +static constexpr float B_DELTAS = 19.86779 / 1000; +static constexpr float S_MIN = 0.0; +static constexpr float S_MAX = 0.01; +static constexpr float S_STEP = 0.0005; + +// PID configs +static constexpr float Kp = 40; +static constexpr float Ki = 5; + +// Airbrakes servo configs +static constexpr float AB_SERVO_MAX_POS = 50.0; // deg +static constexpr float AB_SERVO_MIN_POS = 0.0; // deg +// airbrakes servo rate : datasheet says 60/0.13, increased for robustness +static constexpr float AB_SERVO_MAX_RATE = 15.0 / 0.1; // deg/s +static constexpr float AB_SERVO_MIN_RATE = -15.0 / 0.1; // deg/s +static constexpr float AB_SERVO_WIGGLE_AMPLITUDE = 10.0; // deg + +// Control algorithm configs +// static constexpr int LOOKS = 150; +static constexpr int START_INDEX_OFFSET = -1; +static constexpr float FILTER_COEFF = 0.9; + +#ifdef HARDWARE_IN_THE_LOOP +static constexpr float ABK_UPDATE_PERIOD = 0.1 * 1000; // ms -> 10 Hz +#else +static constexpr float ABK_UPDATE_PERIOD = 0.05 * 1000; // ms -> 20 Hz +#endif +static constexpr float ABK_UPDATE_PERIOD_SECONDS = ABK_UPDATE_PERIOD / 1000; + +#ifdef EUROC +static constexpr int SHADOW_MODE_DURATION = 7.5 * 1000; +#else +static constexpr int SHADOW_MODE_DURATION = 3.5 * 1000; +#endif + +#ifdef EUROC +struct Coefficients +{ + float n000 = 0.4912038462193552; + float n100 = -1.2829337399401595; + float n200 = 5.4496525366603175; + float n300 = -14.373489732912505; + float n400 = 23.041565019164537; + float n500 = -20.665098183955564; + float n600 = 7.781756180733372; + float n010 = 8.578612850167778; + float n020 = 139.0917211542655; + float n110 = 0.7488030083983109; + float n120 = -143.1155855707458; + float n210 = 85.43971374816863; + float n220 = 1012.1982268667119; + float n310 = -318.72827049284604; + float n320 = -3465.8287321207386; + float n410 = 465.76527224155507; + float n420 = 5018.865244618964; + float n510 = -216.65938340162148; + float n520 = -2334.271751434503; + float n001 = 2.7225033263675867e-06; +}; +#else +struct Coefficients +{ + float n000 = 0.47885644032626357; + float n100 = -1.3034714816627486; + float n200 = 5.855250857341603; + float n300 = -15.804467871608331; + float n400 = 25.101568926578267; + float n500 = -21.807834163413037; + float n600 = 7.9441228817993546; + float n010 = 8.332725338197339; + float n020 = 150.92179004169594; + float n110 = 4.450607930313118; + float n120 = -261.9317740122675; + float n210 = 62.374798498326285; + float n220 = 1402.273850221572; + float n310 = -261.26595308714985; + float n320 = -4020.2954140409474; + float n410 = 402.46390018341435; + float n420 = 5332.223039294196; + float n510 = -192.02443481902043; + float n520 = -2381.855640881901; + float n001 = 3.0688005499541758e-06; +}; +#endif + +const Coefficients coeffs; + +} // namespace AirBrakesConfigs + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/CutterConfig.h b/src/boards/DeathStack/configs/CutterConfig.h index c5dcd13352070f6637dd73a4ab9f53299000adde..964a593001898c06effbf83655c87ffe94f6cfd2 100644 --- a/src/boards/DeathStack/configs/CutterConfig.h +++ b/src/boards/DeathStack/configs/CutterConfig.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -23,45 +22,26 @@ #pragma once -#include "drivers/HardwareTimer.h" -#include "drivers/pwm/pwm.h" -#include "interfaces-impl/hwmapping.h" +#include <drivers/HardwareTimer.h> +#include <drivers/pwm/pwm.h> +#include <interfaces-impl/hwmapping.h> namespace DeathStackBoard { -// clang-format off -// Struct required by the PWM driver to know the specifics of the timer to use -static const PWM::Timer CUTTER_TIM{ - TIM9, - &(RCC->APB2ENR), - RCC_APB2ENR_TIM9EN, - TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2) - }; - -// clang-format on - -static constexpr int CUT_DURATION = 5 * 1000; -static constexpr int CUT_TEST_DURATION = 3 * 1000; +namespace CutterConfig +{ +// Input signal +typedef miosix::actuators::nosecone::th_cut_input CuttersInput; -// PRIMARY --> THCUT1 on theboard -static const PWMChannel CUTTER_CHANNEL_PRIMARY = PWMChannel::CH2; -typedef miosix::actuators::thCut1::ena PrimaryCutterEna; +// PRIMARY --> THCUT1 on the board +typedef miosix::actuators::nosecone::thCut1::ena PrimaryCutterEna; // BACKUP --> THCUT2 on theboard -static const PWMChannel CUTTER_CHANNEL_BACKUP = PWMChannel::CH2; -typedef miosix::actuators::thCut2::ena BackupCutterEna; - -// PWM Frequency & duty-cycle -static const unsigned int CUTTER_PWM_FREQUENCY = 15000; // Hz -// Duty cycle to be used during flight to cut the chord -static constexpr float CUTTER_PWM_DUTY_CYCLE = 0.30f; +typedef miosix::actuators::nosecone::thCut2::ena BackupCutterEna; -// Duty cycle to be used during integration, to perform a a non-destructive -// continuity check -static constexpr float CUTTER_TEST_PWM_DUTY_CYCLE = 0.05f; +static constexpr int CUT_DURATION = 50; -// Period of time where the IN must be kept low before bringing ENA/INH low -static const int CUTTER_DISABLE_DELAY_MS = 50; +} // namespace CutterConfig -} // namespace DeathStackBoard +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/DeploymentConfig.h b/src/boards/DeathStack/configs/DeploymentConfig.h index 9f1e0a57c57ac43b3a83b9c9c304dfbd5246889d..9a7ad6df154e28e02186afceeb2fc41b80ca0002 100644 --- a/src/boards/DeathStack/configs/DeploymentConfig.h +++ b/src/boards/DeathStack/configs/DeploymentConfig.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -25,10 +24,6 @@ #include <drivers/HardwareTimer.h> #include <drivers/pwm/pwm.h> -#include <interfaces-impl/hwmapping.h> -#include <miosix.h> -#include "DeathStack/DeploymentController/Motor/MotorData.h" -#include "config.h" namespace DeathStackBoard { @@ -36,22 +31,20 @@ namespace DeathStackBoard namespace DeploymentConfigs { -static constexpr uint8_t MAX_EJECTION_ATTEMPTS = 1; - -static constexpr int NC_OPEN_TIMEOUT = 5000; - -static const PWM::Timer SERVO_TIMER{ +static const PWM::Timer DPL_SERVO_TIMER{ TIM4, &(RCC->APB1ENR), RCC_APB1ENR_TIM4EN, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)}; -static constexpr PWMChannel SERVO_CHANNEL = PWMChannel::CH1; +static constexpr PWMChannel DPL_SERVO_PWM_CH = PWMChannel::CH1; -// Servo rest position -static constexpr float SERVO_RESET_POS = 0.77f; -static constexpr float SERVO_WIGGLE_AMPLITUDE = 0.02f; +static constexpr int NC_OPEN_TIMEOUT = 5000; -// Servo position when ejecting the nosecone -static constexpr float SERVO_EJECT_POS = 0.35f; +// Angles in degrees +static constexpr float DPL_SERVO_MIN_POS = 155; +static constexpr float DPL_SERVO_MAX_POS = 178; +static constexpr float DPL_SERVO_RESET_POS = 178; +static constexpr float DPL_SERVO_EJECT_POS = 157; +static constexpr float DPL_SERVO_WIGGLE_AMPLITUDE = 2; } // namespace DeploymentConfigs diff --git a/src/boards/DeathStack/configs/FMMConfig.h b/src/boards/DeathStack/configs/FMMConfig.h index b898745d7c8aa65f2b43da13f7b521a3cbd71f51..ad0c44b4f9b3993ebe9cd84ac510eb587d268470 100644 --- a/src/boards/DeathStack/configs/FMMConfig.h +++ b/src/boards/DeathStack/configs/FMMConfig.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2015-2021 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -28,7 +28,9 @@ namespace DeathStackBoard // State timeouts // Automatically end the mission after a very long time, in order to safely // close the logs if radio comms have been lost -static constexpr unsigned int TIMEOUT_FMM_END_MISSION = 30 * 60 * 1000; +static constexpr unsigned int TIMEOUT_END_MISSION = 30 * 60 * 1000; + +static constexpr unsigned int FMM_PRIORITY = 2; } // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/FlightStatsConfig.h b/src/boards/DeathStack/configs/FlightStatsConfig.h index 06746ce4dbc29ca5960fbf735c29ccfb6c9c6091..eb7fe88fc016a12ea6f3e4f1af232ff0e9972d46 100644 --- a/src/boards/DeathStack/configs/FlightStatsConfig.h +++ b/src/boards/DeathStack/configs/FlightStatsConfig.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -23,19 +22,17 @@ #pragma once -#include "config.h" -#include "CutterConfig.h" +#include <configs/CutterConfig.h> namespace DeathStackBoard { + namespace FlightStatsConfig { - -static constexpr long long TIMEOUT_CUTTER_TEST_STATS = CUT_TEST_DURATION; -static constexpr long long TIMEOUT_LIFTOFF_STATS = 6000; -static constexpr long long TIMEOUT_APOGEE_STATS = 1500; -static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 15000; -static constexpr long long TIMEOUT_MAIN_DPL_STATS = 15000; +static constexpr long long TIMEOUT_LIFTOFF_STATS = 7500; // [ms] +static constexpr long long TIMEOUT_APOGEE_STATS = 1500; // [ms] +static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 10000; // [ms] +static constexpr long long TIMEOUT_MAIN_DPL_STATS = 10000; // [ms] } // namespace FlightStatsConfig } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/HILConfig.h b/src/boards/DeathStack/configs/HILConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..c1916e617faba386124767f1690a1519e912dc6d --- /dev/null +++ b/src/boards/DeathStack/configs/HILConfig.h @@ -0,0 +1,128 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" + +// serial port number +static constexpr int HIL_SERIAL_PORT_NUM = 3; + +// baudrate of connection +static constexpr int HIL_BAUDRATE = 115200; + +// Period of simulation in milliseconds +static constexpr int HIL_SIMULATION_PERIOD = 100; // 10 hz + +// sample frequency of sensor (samples/second) +static constexpr int HIL_IMU_PERIOD = 10; // 100 hz +static constexpr int HIL_ACCEL_PERIOD = 10; // 100 hz +static constexpr int HIL_GYRO_PERIOD = 10; // 100 hz +static constexpr int HIL_MAGN_PERIOD = 10; // 100 hz +static constexpr int HIL_BARO_PERIOD = 50; // 20 hz +static constexpr int HIL_GPS_PERIOD = 100; // 10 hz; + +// // update frequency of the Navigation System +// const int KALMAN_FREQ = 100; +// // update frequency of airbrakes control algorithm +// const int CONTROL_FREQ = 10; +// // update frequency of airbrakes control algorithm +// const int ADA_FREQ = 20; + +// ADA configs +// [TODO?] Prendi valori da config ADA +// const float deploymentAltitude = 450; +// const float referenceAltitude = 109; // launchpad Altitude for Pont De +// Sor? const float referenceTemperature = 15; + +// Number of samples per sensor at each simulator iteration +static constexpr int N_DATA_IMU = (HIL_SIMULATION_PERIOD / HIL_IMU_PERIOD); +static constexpr int N_DATA_ACCEL = (HIL_SIMULATION_PERIOD / HIL_ACCEL_PERIOD); +static constexpr int N_DATA_GYRO = (HIL_SIMULATION_PERIOD) / HIL_GYRO_PERIOD; +static constexpr int N_DATA_MAGN = (HIL_SIMULATION_PERIOD / HIL_MAGN_PERIOD); +static constexpr int N_DATA_BARO = (HIL_SIMULATION_PERIOD / HIL_BARO_PERIOD); +static constexpr int N_DATA_GPS = (HIL_SIMULATION_PERIOD / HIL_GPS_PERIOD); +// const int N_DATA_KALMAN = (HIL_KALMAN_FREQ * HIL_SIMULATION_PERIOD); + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received. + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + * + * NOTE: all the fields contained in the struct are floats because it is the + * data type sent over serial by MATLAB + */ +struct SimulatorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + float fix; + float num_satellites; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; diff --git a/src/boards/DeathStack/configs/IgnitionConfig.h b/src/boards/DeathStack/configs/IgnitionConfig.h index 003cee78bbc13e5a8d12187399e095bba332bf45..6bc4ca1889389923137a790f5a318d6da6915b20 100644 --- a/src/boards/DeathStack/configs/IgnitionConfig.h +++ b/src/boards/DeathStack/configs/IgnitionConfig.h @@ -1,5 +1,5 @@ /* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,12 +22,11 @@ #pragma once - namespace DeathStackBoard { - -// How many milliseconds between a status request to the CAN bus and the -// next one. + +// How many milliseconds between a status request to the CAN bus and the +// next one. static const unsigned int INTERVAL_IGN_GET_STATUS = 1000; // Milliseconds after which, if the ignition board never responded, an @@ -35,7 +34,6 @@ static const unsigned int INTERVAL_IGN_GET_STATUS = 1000; // NOTE: since the ignition and abort operations take ~10sec and the // ignition board doesn't respond during that time, this timeout // should be considerably longer than max(ignitionTime, abortTime). -static const unsigned int TIMEOUT_IGN_OFFLINE = 3*1000; - -} // DeathStackBoard +static const unsigned int TIMEOUT_IGN_OFFLINE = 3 * 1000; +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/DeploymentController/ThermalCutter/CutterData.h b/src/boards/DeathStack/configs/MavlinkConfig.h similarity index 72% rename from src/boards/DeathStack/DeploymentController/ThermalCutter/CutterData.h rename to src/boards/DeathStack/configs/MavlinkConfig.h index 39f6d04990cc912dead538e894931a53fa8c66ee..946497910bc89c738e4b512c5b1e714576e99780 100644 --- a/src/boards/DeathStack/DeploymentController/ThermalCutter/CutterData.h +++ b/src/boards/DeathStack/configs/MavlinkConfig.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,32 +13,20 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - #pragma once -#include <ostream> -#include <string> - namespace DeathStackBoard { - -enum class CutterState : uint8_t -{ - IDLE, - CUTTING_PRIMARY, - CUTTING_BACKUP, - TESTING_PRIMARY, - TESTING_BACKUP -}; -struct CutterStatus -{ - CutterState state = CutterState::IDLE; -}; +/* Mavlink Driver queue settings */ +static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10; +static constexpr unsigned int MAV_PKT_SIZE = 255; +static constexpr long long MAV_OUT_BUFFER_MAX_AGE = 200; + } // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/MotorConfig.h b/src/boards/DeathStack/configs/MotorConfig.h index 8b28641062382be399cfcea75cc07b056786ce6d..882a7286e6b5b46b83bcf3a6d05584b688d10113 100644 --- a/src/boards/DeathStack/configs/MotorConfig.h +++ b/src/boards/DeathStack/configs/MotorConfig.h @@ -1,5 +1,5 @@ /* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron + * Author: Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -21,9 +21,10 @@ */ #pragma once -namespace DeathStackBoard { +namespace DeathStackBoard +{ -/* Period of time where the IN must be kept low before bringing ENA/INH low */ +// Period of time where the IN must be kept low before bringing ENA/INH low static const int MOTOR_DISABLE_DELAY_MS = 50; -} \ No newline at end of file +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/NASConfig.h b/src/boards/DeathStack/configs/NASConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..b64b13837312042c81d0691d933344e0c505cc8a --- /dev/null +++ b/src/boards/DeathStack/configs/NASConfig.h @@ -0,0 +1,155 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "Constants.h" +#include "Eigen/Dense" + +using namespace Eigen; + +namespace DeathStackBoard +{ + +namespace NASConfigs +{ + +static const unsigned int NAS_UPDATE_PERIOD = 20; // ms -> 50 hz + +static const float SAMPLE_RATE = 1000.0F / NAS_UPDATE_PERIOD; // [Hz] + +static const float T = 1.0F / SAMPLE_RATE; // [s] + +static const uint32_t CALIBRATION_N_SAMPLES = 200; + +static const float SIGMA_BETA = + (float)1e-2; // [Rad/s^2] + // Estimated gyroscope bias variance + +static const float SIGMA_MAG = 0.7089F; // [uT^2] + // Estimated magnetometer variance + +static const float SIGMA_W = 0.000027927F; // [Rad^2] + // Estimated gyroscope variance + +static const float SIGMA_GPS = 2.0F; // [deg^2] + // Estimated GPS variance + +static const float SIGMA_BAR = 4.3665F; // [kPa^2] + // Estimated barometer variance + +static const float SIGMA_POS = + 0.0192F; // [m^2] + // Estimated variance of the position noise + +static const float SIGMA_VEL = + 0.096F; // [(m/s)^2] + // Estimated variance of the velocity noise + +static const float P_POS = 0.01F; // Position prediction covariance + +static const float P_VEL = 0.01F; // Velocity prediction covariance + +static const float P_ATT = 0.01F; // Attitude prediction covariance + +static const float P_BIAS = 0.01F; // Bias prediction covariance + +/*----------------------------------------------------------------------------*/ + +// Equirectangular projection for gps: https://bit.ly/2RaMbD5 + +// static const float RAD = +// 6371.0F * powf(10, 3); // [m] +// // Earth radius, used for the GPS correction + +// static const float PHI1 = +// 0.0F; // [rad] +// // Latitude where the scale of the projection is true + +// static const float CLAT = cosf(PHI1); + +static const float SATS_NUM = 6.0F; // Number of available satellites + +static const unsigned int JAMMING_FACTOR = 3; + +#ifdef EUROC +// static const float LAT0 = +// 39.201778F; // [deg] +// // Latitude of the launch location (pont de sor) +// static const float LON0 = +// -8.138368F; // [deg] +// // Longitude of the launch location (pont de sor) + +static const float EMF = 45.0F; // [uT] micro Tesla + // Earth magnetic field, used to + // check if there's magnetic jamming + +static const Vector3f NED_MAG( + 0.5969F, -0.0139F, + 0.8022F); // Normalized magnetic field vector at Ponte de Sor + // Measurement units are not important since it's normalized +#else +// static const float LAT0 = +// 41.8098571; // [deg] +// // Latitude of the launch location (roccaraso) +// static const float LON0 = +// 14.0545127; // [deg] +// // Longitude of the launch location (roccaraso) + +static const float EMF = 46.77F; // [uT] micro Tesla + // Earth magnetic field, used to + // check if there's magnetic jamming + +static const Vector3f NED_MAG( + 0.5248, 0.0356, 0.8505); // Normalized magnetic field vector at Roccaraso +#endif + +// normalized magentic field at Milano +// static const Vector3f NED_MAG(0.4742, 0.025, 0.8801); + +// DIMENSIONS OF MATRICES AND VECTORS + +static const uint16_t N = 13; // State vector elements, N x 1 +static const uint16_t NP = N - 1; // P matrix, N-1 x N-1. + // Reduced order thanks to the MEKF +static const uint16_t NATT = 7; // Number of attitude related elements. + // Quaternion components and biases: + // [q1, q2, q3, q4, bx, by, bz] +static const uint16_t NL = + N - NATT; // Number of linear elements in the state + // vector. Position and velocity: + // [p_north, p_east, p_down, v_n, v_e, v_d] + +static const uint16_t NBAR = 1; // States of the barometer + // [pressure] +static const uint16_t NGPS = 4; // States of the gps + // [lon, lat, v_north, v_east] +static const uint16_t NMAG = 3; // States of the magnetometer + // [mx, my, mz] +static const uint16_t NMEKF = 6; // Dimension used in the MEKF. + // 4 quaternion components + 3 biases - 1 + // The MEKF structure allows us to perform + // the dimensionality reduction of 1 + +} // namespace NASConfigs + +} // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/PinObserverConfig.h b/src/boards/DeathStack/configs/PinObserverConfig.h index 365c921c048f9a01b070ed4efc8b2bdbe0b00ac0..29854a8bac3d34821af752f91aa1790ea26851bc 100644 --- a/src/boards/DeathStack/configs/PinObserverConfig.h +++ b/src/boards/DeathStack/configs/PinObserverConfig.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -25,7 +24,7 @@ #include <interfaces-impl/hwmapping.h> #include <miosix.h> -#include "utils/PinObserver.h" +#include <utils/PinObserver.h> namespace DeathStackBoard { @@ -33,32 +32,27 @@ namespace DeathStackBoard static const unsigned int PIN_POLL_INTERVAL = 10; // ms // Launch pin config -static const unsigned int PORT_LAUNCH_PIN = GPIOC_BASE; -static const unsigned char NUM_LAUNCH_PIN = 6; - +static const GpioPin launch_pin(miosix::inputs::lp_dtch::getPin()); static const PinObserver::Transition TRIGGER_LAUNCH_PIN = PinObserver::Transition::FALLING_EDGE; - // How many consecutive times the launch pin should be detected as detached // before triggering a launch event. static const unsigned int THRESHOLD_LAUNCH_PIN = 10; -static const unsigned int PORT_NC_DETACH_PIN = GPIOB_BASE; -static const unsigned char NUM_NC_DETACH_PIN = 7; +// Nosecone detach pin config +static const GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin()); static const PinObserver::Transition TRIGGER_NC_DETACH_PIN = PinObserver::Transition::FALLING_EDGE; - -static const unsigned int PORT_MOTOR_PIN = GPIOE_BASE; -static const unsigned char NUM_MOTOR_PIN = 3; - -static const unsigned int THRESHOLD_MOTOR_PIN = 1; - - -static const PinObserver::Transition TRIGGER_MOTOR_PIN = - PinObserver::Transition::FALLING_EDGE; - // How many consecutive times the nosecone pin should be detected as detached // before triggering a nosecone detach event. static const unsigned int THRESHOLD_NC_DETACH_PIN = 10; +// Dpl servo actuation pin config +static const GpioPin dpl_servo_pin(miosix::inputs::expulsion_in::getPin()); +static const PinObserver::Transition TRIGGER_DPL_SERVO_PIN = + PinObserver::Transition::RISING_EDGE; +// How many consecutive times the deployment servo pin should be detected as +// detached before triggering a nosecone detach event. +static const unsigned int THRESHOLD_DPL_SERVO_PIN = 10; + } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/configs/SensorManagerConfig.h b/src/boards/DeathStack/configs/SensorManagerConfig.h deleted file mode 100644 index 9c458bfe00cff89e8e0d28fb8df1884823d474b8..0000000000000000000000000000000000000000 --- a/src/boards/DeathStack/configs/SensorManagerConfig.h +++ /dev/null @@ -1,86 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <Common.h> -#include <drivers/BusTemplate.h> -#include <drivers/stm32f2_f4_i2c.h> -#include <interfaces-impl/hwmapping.h> -#include <drivers/SoftwareI2CAdapter.h> - -using miosix::Gpio; - -namespace DeathStackBoard -{ - -// I2C 1 -typedef SoftwareI2CAdapter<miosix::interfaces::i2c::sda, miosix::interfaces::i2c::scl> i2c1; - -// SPI1 -typedef BusSPI<1, miosix::interfaces::spi1::mosi, - miosix::interfaces::spi1::miso, miosix::interfaces::spi1::sck> - busSPI1; - -// Spi protocol defs -typedef ProtocolSPI<busSPI1, miosix::sensors::mpu9250::cs> spiMPU9250; -typedef ProtocolSPI<busSPI1, miosix::sensors::adis16405::cs> spiADIS16405; -typedef ProtocolSPI<busSPI1, miosix::sensors::ms5803::cs> spiMS5803; - -typedef miosix::sensors::ad7994::ab ad7994_busy_pin; -typedef miosix::sensors::ad7994::nconvst ad7994_nconvst; - -static constexpr uint8_t AD7994_NXP_BARO_CHANNEL = 1; -static constexpr uint8_t AD7994_HONEYWELL_BARO_CHANNEL = 3; - - -static constexpr uint8_t INTERNAL_ADC_NUM = 3; -static constexpr uint8_t ADC_CURRENT_SENSE_1_CHANNEL = 6; -static constexpr uint8_t ADC_CURRENT_SENSE_2_CHANNEL = 4; -static constexpr uint8_t ADC_BATTERY_VOLTAGE_CHANNEL = 5; - -//Time after which a gps location is no longer valid in milliseconds -static constexpr long long MAX_GPS_FIX_AGE = 300; - -#ifdef DEATH_STACK_2 -static constexpr float AD7994_V_REF = 4.29f; - -static constexpr float OFFSET_MPU_ACC_X = 0.0f; -static constexpr float OFFSET_MPU_ACC_Y = 0.0f; -static constexpr float OFFSET_MPU_ACC_Z = 0.0f; - -#else -static constexpr float AD7994_V_REF = 4.21f; - -static constexpr float OFFSET_MPU_ACC_X = 0.0f; -static constexpr float OFFSET_MPU_ACC_Y = 0.0f; -static constexpr float OFFSET_MPU_ACC_Z = 0.0f; - -#ifndef DEATH_STACK_1 -#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST -#warning "You have not specified which stack you are using! (-DDEATH_STACK_1 OR -DDEATH_STACK_2)" -#endif -#endif -#endif - -} // namespace DeathStackBoard - diff --git a/src/boards/DeathStack/configs/SensorsConfig.h b/src/boards/DeathStack/configs/SensorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..e0772d316e802c2379cceb3bb59f1434ae244fc7 --- /dev/null +++ b/src/boards/DeathStack/configs/SensorsConfig.h @@ -0,0 +1,139 @@ +/* Copyright (c) 2015-2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/adc/ADS1118/ADS1118.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <interfaces-impl/hwmapping.h> +#include <sensors/BMX160/BMX160Config.h> +#include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/calibration/Calibration.h> + +using miosix::Gpio; + +namespace DeathStackBoard +{ + +namespace SensorConfigs +{ +static constexpr float INTERNAL_ADC_VREF = 3.3; +static constexpr InternalADC::Channel ADC_BATTERY_VOLTAGE = + InternalADC::Channel::CH5; +// static constexpr InternalADC::Channel ADC_CS_CUTTER_PRIMARY = +// InternalADC::Channel::CH6; +// static constexpr InternalADC::Channel ADC_CS_CUTTER_BACKUP = +// InternalADC::Channel::CH4; + +static constexpr float BATTERY_VOLTAGE_COEFF = 5.98; +static constexpr float BATTERY_MIN_SAFE_VOLTAGE = 10.5; // [V] + +// static constexpr float CS_CURR_DKILIS = 19500.0; // Typ: 19.5 +// static constexpr float CS_CURR_RIS = 510; +// static constexpr float CS_CURR_IISOFF = .000170; // Typ: 170uA + +static constexpr ADS1118::ADS1118Mux ADC_CH_STATIC_PORT = ADS1118::MUX_AIN0_GND; +static constexpr ADS1118::ADS1118Mux ADC_CH_PITOT_PORT = ADS1118::MUX_AIN1_GND; +static constexpr ADS1118::ADS1118Mux ADC_CH_DPL_PORT = ADS1118::MUX_AIN2_GND; +static constexpr ADS1118::ADS1118Mux ADC_CH_VREF = ADS1118::MUX_AIN3_GND; + +static constexpr ADS1118::ADS1118DataRate ADC_DR_STATIC_PORT = ADS1118::DR_860; +static constexpr ADS1118::ADS1118DataRate ADC_DR_PITOT_PORT = ADS1118::DR_860; +static constexpr ADS1118::ADS1118DataRate ADC_DR_DPL_PORT = ADS1118::DR_860; +static constexpr ADS1118::ADS1118DataRate ADC_DR_VREF = ADS1118::DR_860; + +static constexpr ADS1118::ADS1118Pga ADC_PGA_STATIC_PORT = ADS1118::FSR_6_144; +static constexpr ADS1118::ADS1118Pga ADC_PGA_PITOT_PORT = ADS1118::FSR_6_144; +static constexpr ADS1118::ADS1118Pga ADC_PGA_DPL_PORT = ADS1118::FSR_6_144; +static constexpr ADS1118::ADS1118Pga ADC_PGA_VREF = ADS1118::FSR_6_144; + +// Sampling periods in milliseconds +static constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = + 1000; // only for battery voltage +static constexpr unsigned int SAMPLE_PERIOD_ADC_ADS1118 = 6; + +static constexpr unsigned int SAMPLE_PERIOD_PRESS_DIGITAL = 15; +static constexpr unsigned int TEMP_DIVIDER_PRESS_DIGITAL = 5; + +static constexpr unsigned int SAMPLE_PERIOD_PRESS_PITOT = + SAMPLE_PERIOD_ADC_ADS1118 * 4; +static constexpr unsigned int SAMPLE_PERIOD_PRESS_DPL = + SAMPLE_PERIOD_ADC_ADS1118 * 4; +static constexpr unsigned int SAMPLE_PERIOD_PRESS_STATIC = + SAMPLE_PERIOD_ADC_ADS1118 * 4; + +static constexpr BMX160Config::AccelerometerRange IMU_BMX_ACC_FULLSCALE_ENUM = + BMX160Config::AccelerometerRange::G_16; +static constexpr BMX160Config::GyroscopeRange IMU_BMX_GYRO_FULLSCALE_ENUM = + BMX160Config::GyroscopeRange::DEG_500; + +static constexpr unsigned int IMU_BMX_ACC_GYRO_ODR = 1600; +static constexpr BMX160Config::OutputDataRate IMU_BMX_ACC_GYRO_ODR_ENUM = + BMX160Config::OutputDataRate::HZ_1600; +static constexpr unsigned int IMU_BMX_MAG_ODR = 100; +static constexpr BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM = + BMX160Config::OutputDataRate::HZ_100; + +static constexpr unsigned int IMU_BMX_FIFO_HEADER_SIZE = 1; +static constexpr unsigned int IMU_BMX_ACC_DATA_SIZE = 6; +static constexpr unsigned int IMU_BMX_GYRO_DATA_SIZE = 6; +static constexpr unsigned int IMU_BMX_MAG_DATA_SIZE = 8; + +static constexpr unsigned int IMU_BMX_FIFO_WATERMARK = 80; + +// How many bytes go into the fifo each second +static constexpr unsigned int IMU_BMX_FIFO_FILL_RATE = + IMU_BMX_ACC_GYRO_ODR * (IMU_BMX_FIFO_HEADER_SIZE + IMU_BMX_ACC_DATA_SIZE + + IMU_BMX_GYRO_DATA_SIZE) + + IMU_BMX_MAG_ODR * (IMU_BMX_MAG_DATA_SIZE + IMU_BMX_FIFO_HEADER_SIZE); + +// How long does it take for the bmx fifo to fill up +static constexpr unsigned int IMU_BMX_FIFO_FILL_TIME = + 1024 * 1000 / IMU_BMX_FIFO_FILL_RATE; + +// Sample before the fifo is full, but slightly after the watermark level +// (watermark + 30) ---> high slack due to scheduler imprecision, +// avoid clearing the fifo before the interrupt +static constexpr unsigned int SAMPLE_PERIOD_IMU_BMX = + IMU_BMX_FIFO_FILL_TIME * (IMU_BMX_FIFO_WATERMARK + 30) * 4 / 1024; + +static constexpr unsigned int IMU_BMX_TEMP_DIVIDER = 1; + +// IMU axis rotation +static const AxisOrthoOrientation BMX160_AXIS_ROTATION = { + Direction::NEGATIVE_Z, Direction::POSITIVE_Y}; + +static constexpr char BMX160_CORRECTION_PARAMETERS_FILE[30] = + "/sd/bmx160_params.csv"; + +static constexpr unsigned int SAMPLE_PERIOD_MAG_LIS = 15; +static constexpr LIS3MDL::ODR MAG_LIS_ODR_ENUM = LIS3MDL::ODR_80_HZ; +static constexpr LIS3MDL::FullScale MAG_LIS_FULLSCALE = LIS3MDL::FS_4_GAUSS; + +static constexpr unsigned int GPS_SAMPLE_RATE = 25; +static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; +static constexpr unsigned int GPS_BAUD_RATE = 460800; + +static constexpr float REFERENCE_VOLTAGE = 4.8; +} // namespace SensorConfigs + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/TMTCConfig.h b/src/boards/DeathStack/configs/TMTCConfig.h index a3690c0c838e8d9c1022f66fa6989e90ce81e4c2..b11b0065c85d015140c50bae33a22c97874e13be 100644 --- a/src/boards/DeathStack/configs/TMTCConfig.h +++ b/src/boards/DeathStack/configs/TMTCConfig.h @@ -1,5 +1,5 @@ /* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron + * Author: Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -21,35 +21,35 @@ */ #pragma once -#include <drivers/BusTemplate.h> +#include <drivers/Xbee/Xbee.h> +#include <drivers/spi/SPIDriver.h> #include <interfaces-impl/hwmapping.h> -#include <skyward-boardcore/libs/mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h> -#include "drivers/Xbee/Xbee.h" +#include <mavlink_skyward_lib/mavlink_lib/lynx/mavlink.h> namespace DeathStackBoard { -/* Mavlink Driver queue settings */ -static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10; -static constexpr unsigned int MAV_PKT_SIZE = 255; -static constexpr long long MAV_OUT_BUFFER_MAX_AGE = 200; -/* Min guaranteed sleep time after each packet sent(milliseconds) */ -static const uint16_t TMTC_SLEEP_AFTER_SEND = 0; +/* Min guaranteed sleep time after each packet sent (milliseconds) */ +static const uint16_t SLEEP_AFTER_SEND = 0; -/* Device */ -typedef BusSPI<2, miosix::interfaces::spi2::mosi, - miosix::interfaces::spi2::miso, miosix::interfaces::spi2::sck> - busSPI2; - -typedef Xbee::Xbee<busSPI2, miosix::xbee::cs, miosix::xbee::attn, - miosix::xbee::reset> - Xbee_t; +/* Xbee */ +typedef miosix::xbee::cs XbeeCS; +typedef miosix::xbee::attn XbeeATTN; +typedef miosix::xbee::reset XbeeRST; +/* Xbee data rate (80 or 10 kbps) */ +static const bool XBEE_80KBPS_DATA_RATE = true; +static const bool XBEE_TIMEOUT = 5000; // milliseconds /* Periodic telemetries periods */ -static const unsigned int LR_TM_TIMEOUT = 1000; -static const unsigned int HR_TM_TIMEOUT = 63; +static const unsigned int LR_TM_TIMEOUT = 1000; +static const unsigned int TUNNEL_TM_TIMEOUT = 100; + +static const unsigned int HR_TM_TIMEOUT = 63; +static const unsigned int HR_TM_GROUND_TIMEOUT = 250; -static const unsigned int TEST_TM_TIMEOUT = 250; +static const unsigned int TEST_TM_TIMEOUT = 250; +static const unsigned int GROUND_SENS_TM_TIMEOUT = 1000; +static const unsigned int SENS_TM_TIMEOUT = 100; /* Mavlink messages sysID and compID */ static const unsigned int TMTC_MAV_SYSID = 1; diff --git a/src/boards/DeathStack/configs/config.h b/src/boards/DeathStack/configs/config.h index 078db8293f31cfc96b4dc1882877bdc066c06144..f995ced2a40c9cd547fb56afb6b381e6fd15c888 100644 --- a/src/boards/DeathStack/configs/config.h +++ b/src/boards/DeathStack/configs/config.h @@ -1,20 +1,20 @@ /** * Copyright (c) 2019 Skyward Experimental Rocketry * Authors: Luca Erbetta - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN diff --git a/src/boards/DeathStack/events/EventData.h b/src/boards/DeathStack/events/EventData.h index b221fb73d5ab5f6c0f1e29c646bed051a0d5fde8..5dc3a3ec02d14030c6dd6f32ab6b705bbfc0adcc 100644 --- a/src/boards/DeathStack/events/EventData.h +++ b/src/boards/DeathStack/events/EventData.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN diff --git a/src/boards/DeathStack/events/EventInjector.h b/src/boards/DeathStack/events/EventInjector.h index b15f7de7f35fe1e61d19f2adf685c2d266242faf..c105df210b630c825bddc67be69c5ec6c18f468a 100644 --- a/src/boards/DeathStack/events/EventInjector.h +++ b/src/boards/DeathStack/events/EventInjector.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,12 +21,13 @@ */ #include <ActiveObject.h> +#include <DeathStack.h> #include <events/EventBroker.h> +#include <events/Events.h> + #include <iostream> #include <sstream> #include <string> -#include "DeathStack/DeathStack.h" -#include "DeathStack/events/Events.h" using std::string; using std::stringstream; @@ -51,36 +51,36 @@ protected: switch (ev) { - case EV_TC_SET_DPL_ALTITUDE: - { - float in; - cout << "Payload:\n"; - getline(cin, temp); - stringstream(temp) >> in; + // case EV_TC_SET_DPL_ALTITUDE: + // { + // float in; + // cout << "Payload:\n"; + // getline(cin, temp); + // stringstream(temp) >> in; - DeathStack::getInstance()->ada->setDeploymentAltitude(in); - break; - } - case EV_TC_SET_REFERENCE_ALTITUDE: - { - float in; - cout << "Payload:\n"; - getline(cin, temp); - stringstream(temp) >> in; + // DeathStack::getInstance()->ada->setDeploymentAltitude(in); + // break; + // } + // case EV_TC_SET_REFERENCE_ALTITUDE: + // { + // float in; + // cout << "Payload:\n"; + // getline(cin, temp); + // stringstream(temp) >> in; - DeathStack::getInstance()->ada->setReferenceAltitude(in); - break; - } - case EV_TC_SET_REFERENCE_TEMP: - { - float in; - cout << "Payload:\n"; - getline(cin, temp); - stringstream(temp) >> in; + // DeathStack::getInstance()->ada->setReferenceAltitude(in); + // break; + // } + // case EV_TC_SET_REFERENCE_TEMP: + // { + // float in; + // cout << "Payload:\n"; + // getline(cin, temp); + // stringstream(temp) >> in; - DeathStack::getInstance()->ada->setReferenceTemperature(in); - break; - } + // DeathStack::getInstance()->ada->setReferenceTemperature(in); + // break; + // } default: { sEventBroker->post({(uint8_t)ev}, topic); diff --git a/src/boards/DeathStack/events/EventStrings.cpp b/src/boards/DeathStack/events/EventStrings.cpp index 495e1ead073c1631b3cb3f74c7ab4cc955c57694..ff34c22add5e6eab4bb2d6b41f6006f06820ac7f 100644 --- a/src/boards/DeathStack/events/EventStrings.cpp +++ b/src/boards/DeathStack/events/EventStrings.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -26,104 +26,96 @@ ****************************************************************************** */ -// Generated from: https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0 -// Autogen date: 2019-11-06 23:09:09.820014 - - -#include "Events.h" -#include "Topics.h" +// Autogen date: 2021-09-08 23:46:23.104837 #include <map> using std::map; -namespace DeathStackBoard -{ +#include "Events.h" +#include "Topics.h" string getEventString(uint8_t event) { - static const map<uint8_t, string> event_string_map { - { EV_ADA_APOGEE_DETECTED, "EV_ADA_APOGEE_DETECTED" }, - { EV_ADA_DPL_ALT_DETECTED, "EV_ADA_DPL_ALT_DETECTED" }, - { EV_ADA_READY, "EV_ADA_READY" }, - { EV_APOGEE, "EV_APOGEE" }, - { EV_ARMED, "EV_ARMED" }, - { EV_BUTTON_DOWN, "EV_BUTTON_DOWN" }, - { EV_BUTTON_LONG_PRESS, "EV_BUTTON_LONG_PRESS" }, - { EV_BUTTON_SHORT_PRESS, "EV_BUTTON_SHORT_PRESS" }, - { EV_BUTTON_UP, "EV_BUTTON_UP" }, - { EV_BUTTON_VERY_LONG_PRESS, "EV_BUTTON_VERY_LONG_PRESS" }, - { EV_CALIBRATE_ADA, "EV_CALIBRATE_ADA" }, - { EV_CUT_BACKUP, "EV_CUT_BACKUP" }, - { EV_CUT_DROGUE, "EV_CUT_DROGUE" }, - { EV_CUT_PRIMARY, "EV_CUT_PRIMARY" }, - { EV_DISARMED, "EV_DISARMED" }, - { EV_DPL_ALTITUDE, "EV_DPL_ALTITUDE" }, - { EV_FLIGHTSTATS_TIMEOUT, "EV_FLIGHTSTATS_TIMEOUT" }, - { EV_INIT_ERROR, "EV_INIT_ERROR" }, - { EV_INIT_OK, "EV_INIT_OK" }, - { EV_LANDED, "EV_LANDED" }, - { EV_LIFTOFF, "EV_LIFTOFF" }, - { EV_NC_DETACHED, "EV_NC_DETACHED" }, - { EV_NC_OPEN, "EV_NC_OPEN" }, - { EV_RESET_SERVO, "EV_RESET_SERVO" }, - { EV_SEND_HR_TM, "EV_SEND_HR_TM" }, - { EV_SEND_LR_TM, "EV_SEND_LR_TM" }, - { EV_SEND_TEST_TM, "EV_SEND_TEST_TM" }, - { EV_TC_ARM, "EV_TC_ARM" }, - { EV_TC_BOARD_RESET, "EV_TC_BOARD_RESET" }, - { EV_TC_CALIBRATE_ADA, "EV_TC_CALIBRATE_ADA" }, - { EV_TC_CLOSE_LOG, "EV_TC_CLOSE_LOG" }, - { EV_TC_CUT_BACKUP, "EV_TC_CUT_BACKUP" }, - { EV_TC_CUT_DROGUE, "EV_TC_CUT_DROGUE" }, - { EV_TC_CUT_PRIMARY, "EV_TC_CUT_PRIMARY" }, - { EV_TC_DISARM, "EV_TC_DISARM" }, - { EV_TC_END_MISSION, "EV_TC_END_MISSION" }, - { EV_TC_FORCE_INIT, "EV_TC_FORCE_INIT" }, - { EV_TC_LAUNCH, "EV_TC_LAUNCH" }, - { EV_TC_NC_CLOSE, "EV_TC_NC_CLOSE" }, - { EV_TC_NC_OPEN, "EV_TC_NC_OPEN" }, - { EV_TC_RESET_SERVO, "EV_TC_RESET_SERVO" }, - { EV_TC_SET_DPL_ALTITUDE, "EV_TC_SET_DPL_ALTITUDE" }, - { EV_TC_SET_REFERENCE_ALTITUDE, "EV_TC_SET_REFERENCE_ALTITUDE" }, - { EV_TC_SET_REFERENCE_TEMP, "EV_TC_SET_REFERENCE_TEMP" }, - { EV_TC_START_ROGALLO_CONTROL, "EV_TC_START_ROGALLO_CONTROL" }, - { EV_TC_START_SENSOR_LOGGING, "EV_TC_START_SENSOR_LOGGING" }, - { EV_TC_STOP_SENSOR_LOGGING, "EV_TC_STOP_SENSOR_LOGGING" }, - { EV_TC_TEST_CUTTER_BACKUP, "EV_TC_TEST_CUTTER_BACKUP" }, - { EV_TC_TEST_CUTTER_PRIMARY, "EV_TC_TEST_CUTTER_PRIMARY" }, - { EV_TC_TEST_MODE, "EV_TC_TEST_MODE" }, - { EV_TC_WIGGLE_SERVO, "EV_TC_WIGGLE_SERVO" }, - { EV_TEST_CUTTER_BACKUP, "EV_TEST_CUTTER_BACKUP" }, - { EV_TEST_CUTTER_PRIMARY, "EV_TEST_CUTTER_PRIMARY" }, - { EV_TEST_MODE, "EV_TEST_MODE" }, - { EV_TIMEOUT_CUTTING, "EV_TIMEOUT_CUTTING" }, - { EV_TIMEOUT_END_MISSION, "EV_TIMEOUT_END_MISSION" }, - { EV_TIMEOUT_NC_OPEN, "EV_TIMEOUT_NC_OPEN" }, - { EV_TIMEOUT_PRESS_STABILIZATION, "EV_TIMEOUT_PRESS_STABILIZATION" }, - { EV_TIMEOUT_SERVO_RESET, "EV_TIMEOUT_SERVO_RESET" }, - { EV_TIMEOUT_SHADOW_MODE, "EV_TIMEOUT_SHADOW_MODE" }, - { EV_UMBILICAL_DETACHED, "EV_UMBILICAL_DETACHED" }, - { EV_WIGGLE_SERVO, "EV_WIGGLE_SERVO" } + static const map<uint8_t, string> event_string_map{ + {EV_ADA_APOGEE_DETECTED, "EV_ADA_APOGEE_DETECTED"}, + {EV_ADA_DISABLE_ABK, "EV_ADA_DISABLE_ABK"}, + {EV_ADA_DPL_ALT_DETECTED, "EV_ADA_DPL_ALT_DETECTED"}, + {EV_ADA_READY, "EV_ADA_READY"}, + {EV_APOGEE, "EV_APOGEE"}, + {EV_ARMED, "EV_ARMED"}, + {EV_CALIBRATE, "EV_CALIBRATE"}, + {EV_CALIBRATE_ADA, "EV_CALIBRATE_ADA"}, + {EV_CALIBRATE_NAS, "EV_CALIBRATE_NAS"}, + {EV_CALIBRATION_OK, "EV_CALIBRATION_OK"}, + {EV_CUTTING_TIMEOUT, "EV_CUTTING_TIMEOUT"}, + {EV_CUT_DROGUE, "EV_CUT_DROGUE"}, + {EV_DISABLE_ABK, "EV_DISABLE_ABK"}, + {EV_DISARMED, "EV_DISARMED"}, + {EV_DPL_ALTITUDE, "EV_DPL_ALTITUDE"}, + {EV_INIT_ERROR, "EV_INIT_ERROR"}, + {EV_INIT_OK, "EV_INIT_OK"}, + {EV_LANDED, "EV_LANDED"}, + {EV_LIFTOFF, "EV_LIFTOFF"}, + {EV_NAS_READY, "EV_NAS_READY"}, + {EV_NC_DETACHED, "EV_NC_DETACHED"}, + {EV_NC_OPEN, "EV_NC_OPEN"}, + {EV_NC_OPEN_TIMEOUT, "EV_NC_OPEN_TIMEOUT"}, + {EV_RESET_SERVO, "EV_RESET_SERVO"}, + {EV_SEND_HR_TM, "EV_SEND_HR_TM"}, + {EV_SEND_HR_TM_OVER_SERIAL, "EV_SEND_HR_TM_OVER_SERIAL"}, + {EV_SEND_LR_TM, "EV_SEND_LR_TM"}, + {EV_SEND_SENS_TM, "EV_SEND_SENS_TM"}, + {EV_SEND_TEST_TM, "EV_SEND_TEST_TM"}, + {EV_SENSORS_READY, "EV_SENSORS_READY"}, + {EV_SHADOW_MODE_TIMEOUT, "EV_SHADOW_MODE_TIMEOUT"}, + {EV_STATS_TIMEOUT, "EV_STATS_TIMEOUT"}, + {EV_TC_ABK_DISABLE, "EV_TC_ABK_DISABLE"}, + {EV_TC_ABK_RESET_SERVO, "EV_TC_ABK_RESET_SERVO"}, + {EV_TC_ABK_WIGGLE_SERVO, "EV_TC_ABK_WIGGLE_SERVO"}, + {EV_TC_ARM, "EV_TC_ARM"}, + {EV_TC_CALIBRATE_ALGOS, "EV_TC_CALIBRATE_ALGOS"}, + {EV_TC_CALIBRATE_SENSORS, "EV_TC_CALIBRATE_SENSORS"}, + {EV_TC_CLOSE_LOG, "EV_TC_CLOSE_LOG"}, + {EV_TC_CUT_DROGUE, "EV_TC_CUT_DROGUE"}, + {EV_TC_DISARM, "EV_TC_DISARM"}, + {EV_TC_DPL_RESET_SERVO, "EV_TC_DPL_RESET_SERVO"}, + {EV_TC_DPL_WIGGLE_SERVO, "EV_TC_DPL_WIGGLE_SERVO"}, + {EV_TC_END_MISSION, "EV_TC_END_MISSION"}, + {EV_TC_FORCE_INIT, "EV_TC_FORCE_INIT"}, + {EV_TC_LAUNCH, "EV_TC_LAUNCH"}, + {EV_TC_NC_OPEN, "EV_TC_NC_OPEN"}, + {EV_TC_RADIO_TM, "EV_TC_RADIO_TM"}, + {EV_TC_RESET_BOARD, "EV_TC_RESET_BOARD"}, + {EV_TC_SERIAL_TM, "EV_TC_SERIAL_TM"}, + {EV_TC_START_SENSOR_TM, "EV_TC_START_SENSOR_TM"}, + {EV_TC_START_LOG, "EV_TC_START_LOG"}, + {EV_TC_STOP_SENSOR_TM, "EV_TC_STOP_SENSOR_TM"}, + {EV_TC_TEST_ABK, "EV_TC_TEST_ABK"}, + {EV_TC_TEST_MODE, "EV_TC_TEST_MODE"}, + {EV_TEST_ABK, "EV_TEST_ABK"}, + {EV_TEST_TIMEOUT, "EV_TEST_TIMEOUT"}, + {EV_TIMEOUT_END_MISSION, "EV_TIMEOUT_END_MISSION"}, + {EV_TIMEOUT_PRESS_STABILIZATION, "EV_TIMEOUT_PRESS_STABILIZATION"}, + {EV_TIMEOUT_SHADOW_MODE, "EV_TIMEOUT_SHADOW_MODE"}, + {EV_UMBILICAL_DETACHED, "EV_UMBILICAL_DETACHED"}, + {EV_WIGGLE_SERVO, "EV_WIGGLE_SERVO"}, }; - auto it = event_string_map.find(event); + auto it = event_string_map.find(event); return it == event_string_map.end() ? "EV_UNKNOWN" : it->second; } string getTopicString(uint8_t topic) { - static const map<uint8_t, string> topic_string_map{ - { TOPIC_ADA, "TOPIC_ADA" }, - { TOPIC_CAN, "TOPIC_CAN" }, - { TOPIC_DEPLOYMENT, "TOPIC_DEPLOYMENT" }, - { TOPIC_FLIGHT_EVENTS, "TOPIC_FLIGHT_EVENTS" }, - { TOPIC_FMM, "TOPIC_FMM" }, - { TOPIC_IGNITION, "TOPIC_IGNITION" }, - { TOPIC_STATS, "TOPIC_STATS" }, - { TOPIC_TC, "TOPIC_TC" }, - { TOPIC_TMTC, "TOPIC_TMTC" } - }; - auto it = topic_string_map.find(topic); - return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second; -} - + static const map<uint8_t, string> topic_string_map{ + {TOPIC_ABK, "TOPIC_ABK"}, + {TOPIC_ADA, "TOPIC_ADA"}, + {TOPIC_DPL, "TOPIC_DPL"}, + {TOPIC_FLIGHT_EVENTS, "TOPIC_FLIGHT_EVENTS"}, + {TOPIC_FMM, "TOPIC_FMM"}, + {TOPIC_NAS, "TOPIC_NAS"}, + {TOPIC_STATS, "TOPIC_STATS"}, + {TOPIC_TMTC, "TOPIC_TMTC"}, + }; + auto it = topic_string_map.find(topic); + return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second; } \ No newline at end of file diff --git a/src/boards/DeathStack/events/Events.h b/src/boards/DeathStack/events/Events.h index 644c10376b3507c5b1df7faee1be93d37bb8dcc2..1191b4c146aacdca114613d5c502ea8ac4b3aa48 100644 --- a/src/boards/DeathStack/events/Events.h +++ b/src/boards/DeathStack/events/Events.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -26,8 +26,7 @@ ****************************************************************************** */ -// Generated from: https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0 -// Autogen date: 2019-11-06 23:09:09.820014 +// Autogen date: 2021-09-08 23:46:23.104837 #pragma once @@ -35,92 +34,151 @@ #include <string> #include <vector> +#include "Topics.h" #include "events/Event.h" #include "events/EventBroker.h" -#include "Topics.h" using std::string; -namespace DeathStackBoard -{ /** - * Definition of all events in the Homeone Board software - * Refer to section 5.1.1 of the Software Design Document. + * Definition of all events in the Board software. + * Refer to the Software Design Document. */ enum Events : uint8_t { EV_ADA_APOGEE_DETECTED = EV_FIRST_SIGNAL, + EV_ADA_DISABLE_ABK, EV_ADA_DPL_ALT_DETECTED, EV_ADA_READY, EV_APOGEE, EV_ARMED, - EV_BUTTON_DOWN, - EV_BUTTON_LONG_PRESS, - EV_BUTTON_SHORT_PRESS, - EV_BUTTON_UP, - EV_BUTTON_VERY_LONG_PRESS, + EV_CALIBRATE, EV_CALIBRATE_ADA, - EV_CUT_BACKUP, + EV_CALIBRATE_NAS, + EV_CALIBRATION_OK, + EV_CUTTING_TIMEOUT, EV_CUT_DROGUE, - EV_CUT_PRIMARY, + EV_DISABLE_ABK, EV_DISARMED, EV_DPL_ALTITUDE, - EV_FLIGHTSTATS_TIMEOUT, EV_INIT_ERROR, EV_INIT_OK, EV_LANDED, EV_LIFTOFF, + EV_NAS_READY, EV_NC_DETACHED, EV_NC_OPEN, + EV_NC_OPEN_TIMEOUT, EV_RESET_SERVO, EV_SEND_HR_TM, + EV_SEND_HR_TM_OVER_SERIAL, EV_SEND_LR_TM, + EV_SEND_SENS_TM, EV_SEND_TEST_TM, + EV_SENSORS_READY, + EV_SHADOW_MODE_TIMEOUT, + EV_STATS_TIMEOUT, + EV_TC_ABK_DISABLE, + EV_TC_ABK_RESET_SERVO, + EV_TC_ABK_WIGGLE_SERVO, EV_TC_ARM, - EV_TC_BOARD_RESET, - EV_TC_CALIBRATE_ADA, + EV_TC_CALIBRATE_ALGOS, + EV_TC_CALIBRATE_SENSORS, EV_TC_CLOSE_LOG, - EV_TC_CUT_BACKUP, EV_TC_CUT_DROGUE, - EV_TC_CUT_PRIMARY, EV_TC_DISARM, + EV_TC_DPL_RESET_SERVO, + EV_TC_DPL_WIGGLE_SERVO, EV_TC_END_MISSION, EV_TC_FORCE_INIT, EV_TC_LAUNCH, - EV_TC_NC_CLOSE, EV_TC_NC_OPEN, - EV_TC_RESET_SERVO, - EV_TC_SET_DPL_ALTITUDE, - EV_TC_SET_REFERENCE_ALTITUDE, - EV_TC_SET_REFERENCE_TEMP, - EV_TC_START_ROGALLO_CONTROL, - EV_TC_START_SENSOR_LOGGING, - EV_TC_STOP_SENSOR_LOGGING, - EV_TC_TEST_CUTTER_BACKUP, - EV_TC_TEST_CUTTER_PRIMARY, + EV_TC_RADIO_TM, + EV_TC_RESET_BOARD, + EV_TC_SERIAL_TM, + EV_TC_START_SENSOR_TM, + EV_TC_START_LOG, + EV_TC_STOP_SENSOR_TM, + EV_TC_TEST_ABK, EV_TC_TEST_MODE, - EV_TC_WIGGLE_SERVO, - EV_TEST_CUTTER_BACKUP, - EV_TEST_CUTTER_PRIMARY, - EV_TEST_MODE, - EV_TIMEOUT_CUTTING, + EV_TEST_ABK, + EV_TEST_TIMEOUT, EV_TIMEOUT_END_MISSION, - EV_TIMEOUT_NC_OPEN, EV_TIMEOUT_PRESS_STABILIZATION, - EV_TIMEOUT_SERVO_RESET, EV_TIMEOUT_SHADOW_MODE, EV_UMBILICAL_DETACHED, - EV_WIGGLE_SERVO + EV_WIGGLE_SERVO, }; -const std::vector<uint8_t> EVENT_LIST {EV_ADA_APOGEE_DETECTED, EV_ADA_DPL_ALT_DETECTED, EV_ADA_READY, EV_APOGEE, EV_ARMED, EV_BUTTON_DOWN, EV_BUTTON_LONG_PRESS, EV_BUTTON_SHORT_PRESS, EV_BUTTON_UP, EV_BUTTON_VERY_LONG_PRESS, EV_CALIBRATE_ADA, EV_CUT_BACKUP, EV_CUT_DROGUE, EV_CUT_PRIMARY, EV_DISARMED, EV_DPL_ALTITUDE, EV_FLIGHTSTATS_TIMEOUT, EV_INIT_ERROR, EV_INIT_OK, EV_LANDED, EV_LIFTOFF, EV_NC_DETACHED, EV_NC_OPEN, EV_RESET_SERVO, EV_SEND_HR_TM, EV_SEND_LR_TM, EV_SEND_TEST_TM, EV_TC_ARM, EV_TC_BOARD_RESET, EV_TC_CALIBRATE_ADA, EV_TC_CLOSE_LOG, EV_TC_CUT_BACKUP, EV_TC_CUT_DROGUE, EV_TC_CUT_PRIMARY, EV_TC_DISARM, EV_TC_END_MISSION, EV_TC_FORCE_INIT, EV_TC_LAUNCH, EV_TC_NC_CLOSE, EV_TC_NC_OPEN, EV_TC_RESET_SERVO, EV_TC_SET_DPL_ALTITUDE, EV_TC_SET_REFERENCE_ALTITUDE, EV_TC_SET_REFERENCE_TEMP, EV_TC_START_ROGALLO_CONTROL, EV_TC_START_SENSOR_LOGGING, EV_TC_STOP_SENSOR_LOGGING, EV_TC_TEST_CUTTER_BACKUP, EV_TC_TEST_CUTTER_PRIMARY, EV_TC_TEST_MODE, EV_TC_WIGGLE_SERVO, EV_TEST_CUTTER_BACKUP, EV_TEST_CUTTER_PRIMARY, EV_TEST_MODE, EV_TIMEOUT_CUTTING, EV_TIMEOUT_END_MISSION, EV_TIMEOUT_NC_OPEN, EV_TIMEOUT_PRESS_STABILIZATION, EV_TIMEOUT_SERVO_RESET, EV_TIMEOUT_SHADOW_MODE, EV_UMBILICAL_DETACHED, EV_WIGGLE_SERVO}; +const std::vector<uint8_t> EVENT_LIST{ + EV_ADA_APOGEE_DETECTED, + EV_ADA_DISABLE_ABK, + EV_ADA_DPL_ALT_DETECTED, + EV_ADA_READY, + EV_APOGEE, + EV_ARMED, + EV_CALIBRATE, + EV_CALIBRATE_ADA, + EV_CALIBRATE_NAS, + EV_CALIBRATION_OK, + EV_CUTTING_TIMEOUT, + EV_CUT_DROGUE, + EV_DISABLE_ABK, + EV_DISARMED, + EV_DPL_ALTITUDE, + EV_INIT_ERROR, + EV_INIT_OK, + EV_LANDED, + EV_LIFTOFF, + EV_NAS_READY, + EV_NC_DETACHED, + EV_NC_OPEN, + EV_NC_OPEN_TIMEOUT, + EV_RESET_SERVO, + EV_SEND_HR_TM, + EV_SEND_HR_TM_OVER_SERIAL, + EV_SEND_LR_TM, + EV_SEND_SENS_TM, + EV_SEND_TEST_TM, + EV_SENSORS_READY, + EV_SHADOW_MODE_TIMEOUT, + EV_STATS_TIMEOUT, + EV_TC_ABK_DISABLE, + EV_TC_ABK_RESET_SERVO, + EV_TC_ABK_WIGGLE_SERVO, + EV_TC_ARM, + EV_TC_CALIBRATE_ALGOS, + EV_TC_CALIBRATE_SENSORS, + EV_TC_CLOSE_LOG, + EV_TC_CUT_DROGUE, + EV_TC_DISARM, + EV_TC_DPL_RESET_SERVO, + EV_TC_DPL_WIGGLE_SERVO, + EV_TC_END_MISSION, + EV_TC_FORCE_INIT, + EV_TC_LAUNCH, + EV_TC_NC_OPEN, + EV_TC_RADIO_TM, + EV_TC_RESET_BOARD, + EV_TC_SERIAL_TM, + EV_TC_START_SENSOR_TM, + EV_TC_START_LOG, + EV_TC_STOP_SENSOR_TM, + EV_TC_TEST_ABK, + EV_TC_TEST_MODE, + EV_TEST_ABK, + EV_TEST_TIMEOUT, + EV_TIMEOUT_END_MISSION, + EV_TIMEOUT_PRESS_STABILIZATION, + EV_TIMEOUT_SHADOW_MODE, + EV_UMBILICAL_DETACHED, + EV_WIGGLE_SERVO, +}; /** * @brief Returns the name of the provided event - * - * @param event - * @return string + * + * @param event + * @return string */ -string getEventString(uint8_t event); - -} +string getEventString(uint8_t event); \ No newline at end of file diff --git a/src/boards/DeathStack/events/Topics.h b/src/boards/DeathStack/events/Topics.h index 48f9f322c3db48c9752d4a8013e6183eef7d3770..81b65bd653597d1089fc47a0b993076270072fbc 100644 --- a/src/boards/DeathStack/events/Topics.h +++ b/src/boards/DeathStack/events/Topics.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -26,36 +26,35 @@ ****************************************************************************** */ -// Generated from: https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0 -// Autogen date: 2019-11-06 23:09:09.820014 +// Autogen date: 2021-09-08 23:46:23.104837 #pragma once -#include <stdint.h> +#include <cstdint> #include <string> #include <vector> using std::string; -namespace DeathStackBoard -{ /** * Definition of various event topics to use in the EventBroker */ enum Topics : uint8_t { + TOPIC_ABK, TOPIC_ADA, - TOPIC_CAN, - TOPIC_DEPLOYMENT, + TOPIC_DPL, TOPIC_FLIGHT_EVENTS, TOPIC_FMM, - TOPIC_IGNITION, + TOPIC_NAS, TOPIC_STATS, - TOPIC_TC, - TOPIC_TMTC + TOPIC_TMTC, }; -const std::vector<uint8_t> TOPIC_LIST {TOPIC_ADA, TOPIC_CAN, TOPIC_DEPLOYMENT, TOPIC_FLIGHT_EVENTS, TOPIC_FMM, TOPIC_IGNITION, TOPIC_STATS, TOPIC_TC, TOPIC_TMTC}; +const std::vector<uint8_t> TOPIC_LIST{ + TOPIC_ABK, TOPIC_ADA, TOPIC_DPL, TOPIC_FLIGHT_EVENTS, + TOPIC_FMM, TOPIC_NAS, TOPIC_STATS, TOPIC_TMTC, +}; /** * @brief Returns the name of the provided event @@ -63,7 +62,4 @@ const std::vector<uint8_t> TOPIC_LIST {TOPIC_ADA, TOPIC_CAN, TOPIC_DEPLOYMENT, T * @param event * @return string */ -string getTopicString(uint8_t topic); - -} // namespace DeathStackBoard - +string getTopicString(uint8_t topic); \ No newline at end of file diff --git a/src/entrypoints/death-stack-testsuite.cpp b/src/entrypoints/death-stack-testsuite.cpp deleted file mode 100644 index 5d2fb752173fe8462ef596da2f47a0ac94467823..0000000000000000000000000000000000000000 --- a/src/entrypoints/death-stack-testsuite.cpp +++ /dev/null @@ -1,180 +0,0 @@ -#include "DeathStack/configs/SensorManagerConfig.h" - -#include "DeathStack/SensorManager/Sensors/AD7994Wrapper.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapper.h" - -#include <drivers/piksi/piksi.h> -#include <sensors/ADIS16405/ADIS16405.h> -#include <sensors/LM75B.h> -#include <sensors/MPU9250/MPU9250.h> -#include <sensors/SensorSampling.h> - -#include <interfaces-impl/hwmapping.h> - -#include <miosix.h> -#include <cstdio> -#include <iostream> -#include <string> -#include <sstream> - -#include "drivers/HardwareTimer.h" -#include "drivers/Xbee/Xbee.h" -#include "math/Stats.h" - -#include <drivers/BusTemplate.h> - -#include <miosix.h> -#include "DeathStack/DeploymentController/ThermalCutter/Cutter.h" -#include <interfaces-impl/hwmapping.h> -#include "DeathStack/SensorManager/Sensors/ADCWrapper.h" -#include <iostream> - -#include "DeathStack/DeploymentController/Motor/MotorDriver.h" - -#include <cstdio> -#include <logger/Logger.h> -#include <diagnostic/CpuMeter.h> -#include "skyward-boardcore/src/tests/logger/test-logger.h" - -#include "Common.h" -#include "DeathStack/ADA/ADA.h" -#include "DeathStack/LoggerService/LoggerService.h" -#include "DeathStack/SensorManager/SensorManager.h" -#include "diagnostic/CpuMeter.h" -#include "events/EventBroker.h" - -#include <boards/DeathStack/TMTCManager/TMTCManager.h> - -using namespace std; -using namespace miosix; - -using namespace DeathStackBoard; - -using namespace std; -using namespace miosix; - -namespace sensortest -{ - #include "../tests/drivers/test-all-sensors.cpp" -} - -namespace thermotest -{ - #include "../tests/drivers/test-cutter.cpp" -} - - -namespace motortest -{ - #include "../tests/drivers/test-motor.cpp" -} - -namespace loggertest -{ - #include "skyward-boardcore/src/tests/logger/test-logger.cpp" -} - - -namespace xbeetest -{ - #include "skyward-boardcore/src/tests/misc/xbee-send-rcv.cpp" -} - -namespace sm_tmtc -{ - #include "../tests/test-sm+tmtc.cpp" -} - - -void banner() -{ - printf(" _____ _ _ _____ _ _ \n"); - printf("| __ \\ | | | | / ____| | | | \n"); - printf("| | | | ___ __ _| |_| |__ | (___ | |_ __ _ ___| | __\n"); - printf("| | | |/ _ \\/ _` | __| '_ \\ \\___ \\| __/ _` |/ __| |/ /\n"); - printf("| |__| | __/ (_| | |_| | | | ____) | || (_| | (__| < \n"); - printf("|_____/ \\___|\\__,_|\\__|_| |_| |_____/ \\__\\__,_|\\___|_|\\_\\\n\n"); -} - -int main() -{ - banner(); - - while(true) - { - printf("Choose a test:\n"); - printf(" s - Test All Sensors\n"); - printf(" t - Thermal Cutter test\n"); - printf(" m - Nosecone Motor\n"); - printf(" g - Sensors + TMTC (telemetry/telecommands)\n"); - printf(" x - XBee send/rcv\n"); - printf(" l - Logger\n"); - printf("\nOther:\n"); - printf(" r - Reboot\n"); - printf(" f - Pay Respect\n"); - - // Do not directly use cin -- use getline - char c; - string temp; - getline(cin, temp); - stringstream(temp) >> c; - - switch(c) - { - case 's': - { - sensortest::main(); - break; - } - case 't': - { - thermotest::main(); - break; - } - case 'm': - { - motortest::main(); - break; - } - case 'x': - { - xbeetest::main(); - break; - } - case 'l': - { - loggertest::main(); - break; - } - case 'g': - { - sm_tmtc::main(); - break; - } - case 'r': - { - printf("Rebooting\n"); - miosix::reboot(); - break; - } - case 'f': - { - printf("....................../´¯/) \n"); - printf("....................,/¯../ \n"); - printf(".................../..../ \n"); - printf("............./´¯/'...'/´¯¯`·¸ \n"); - printf("........../'/.../..../......./¨¯\\ \n"); - printf("........('(...´...´.... ¯~/'...') \n"); - printf(".........\\.................'...../ \n"); - printf("..........''...\\.......... _.·´ \n"); - printf("............\\..............( \n"); - printf("..............\\.............\\...\n\n"); - break; - } - default: - break; - } -} - - -} \ No newline at end of file diff --git a/src/entrypoints/death-stack-x-decoder/.gitignore b/src/entrypoints/death-stack-x-decoder/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..8b193293c00f3d4483c23b0d5c2e705d0617e209 --- /dev/null +++ b/src/entrypoints/death-stack-x-decoder/.gitignore @@ -0,0 +1 @@ +logdecoder \ No newline at end of file diff --git a/src/entrypoints/death-stack-x-decoder/LogTypes.h b/src/entrypoints/death-stack-x-decoder/LogTypes.h new file mode 100644 index 0000000000000000000000000000000000000000..a6f1c0fe53e87d9c7af37b9797a65957b679adf7 --- /dev/null +++ b/src/entrypoints/death-stack-x-decoder/LogTypes.h @@ -0,0 +1,159 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/adc/ADS1118/ADS1118Data.h> +#include <drivers/gps/ublox/UbloxGPSData.h> +#include <sensors/BMX160/BMX160Data.h> +#include <sensors/BMX160/BMX160WithCorrectionData.h> +#include <sensors/LIS3MDL/LIS3MDLData.h> +#include <sensors/MS5803/MS5803Data.h> +#include <sensors/analog/battery/BatteryVoltageSensorData.h> +#include <sensors/analog/current/CurrentSensorData.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAAData.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDAData.h> + +#include <fstream> +#include <iostream> + +#include "ApogeeDetectionAlgorithm/ADAData.h" +#include "AirBrakes/AirBrakesData.h" +//#include "AirBrakes/WindData.h" +#include "DeathStackStatus.h" +#include "Deployment/DeploymentData.h" +#include "FlightModeManager/FMMStatus.h" +#include "LogStats.h" +#include "Main/SensorsData.h" +#include "NavigationAttitudeSystem/NASData.h" +#include "PinHandler/PinHandlerData.h" +#include "System/SystemData.h" +#include "diagnostic/PrintLoggerData.h" +#include "diagnostic/StackData.h" +#include "drivers/Xbee/APIFramesLog.h" +#include "drivers/Xbee/XbeeStatus.h" +#include "drivers/mavlink/MavlinkStatus.h" +#include "events/EventData.h" +#include "logger/Deserializer.h" +#include "scheduler/TaskSchedulerData.h" +#include "FlightStatsRecorder/FSRData.h" +#include "../../hardware_in_the_loop/HIL_sensors/HILSensorsData.h" + +// Serialized classes +using std::ofstream; + +using namespace DeathStackBoard; + +template <typename T> +void print(T& t, ostream& os) +{ + t.print(os); +} + +template <typename T> +void registerType(Deserializer& ds) +{ + ds.registerType<T>(print<T>, T::header()); +} + +void registerTypes(Deserializer& ds) +{ + // Disagnostic + registerType<TaskStatResult>(ds); + registerType<StackData>(ds); + registerType<LoggingString>(ds); + registerType<SystemData>(ds); + + // Sensors + registerType<CurrentSensorData>(ds); + registerType<BatteryVoltageSensorData>(ds); + registerType<UbloxGPSData>(ds); + registerType<BMX160Data>(ds); + registerType<BMX160WithCorrectionData>(ds); + registerType<BMX160GyroscopeCalibrationBiases>(ds); + registerType<BMX160Temperature>(ds); + registerType<BMX160FifoStats>(ds); + registerType<MS5803Data>(ds); + registerType<MPXHZ6130AData>(ds); + registerType<SSCDRRN015PDAData>(ds); + registerType<SSCDANN030PAAData>(ds); + registerType<LIS3MDLData>(ds); + registerType<ADS1118Data>(ds); + registerType<AirSpeedPitot>(ds); + + // Statuses + registerType<AirBrakesControllerStatus>(ds); + registerType<DeathStackStatus>(ds); + registerType<SensorsStatus>(ds); + registerType<FMMStatus>(ds); + registerType<PinStatus>(ds); + registerType<LogStats>(ds); + registerType<DeploymentStatus>(ds); + registerType<ADAControllerStatus>(ds); + + // XBee + registerType<Xbee::APIFrameLog>(ds); + registerType<Xbee::ATCommandFrameLog>(ds); + registerType<Xbee::ATCommandResponseFrameLog>(ds); + registerType<Xbee::ModemStatusFrameLog>(ds); + registerType<Xbee::TXRequestFrameLog>(ds); + registerType<Xbee::TXStatusFrameLog>(ds); + registerType<Xbee::RXPacketFrameLog>(ds); + registerType<Xbee::XbeeStatus>(ds); + registerType<MavlinkStatus>(ds); + + // ADA + registerType<ADAKalmanState>(ds); + registerType<ADAData>(ds); + registerType<ADAReferenceValues>(ds); + registerType<TargetDeploymentAltitude>(ds); + registerType<ApogeeDetected>(ds); + registerType<DplAltitudeReached>(ds); + + // NAS + registerType<NASStatus>(ds); + registerType<NASKalmanState>(ds); + registerType<NASReferenceValues>(ds); + registerType<NASTriadResult>(ds); + registerType<NASData>(ds); + + // Airbrakes + registerType<AirBrakesData>(ds); + registerType<AirBrakesAlgorithmData>(ds); + registerType<AirBrakesChosenTrajectory>(ds); + + // FlightStatsRecorder + registerType<LiftOffStats>(ds); + registerType<ApogeeStats>(ds); + registerType<DrogueDPLStats>(ds); + registerType<MainDPLStats>(ds); + + // HIL + registerType<HILImuData>(ds); + registerType<HILGpsData>(ds); + registerType<HILBaroData>(ds); + + // Others + registerType<EventData>(ds); + //registerType<WindData>(ds); +} diff --git a/src/entrypoints/death-stack-x-decoder/Makefile b/src/entrypoints/death-stack-x-decoder/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..ccefd3f4b49ea95ee18391bea8cdfaab91791bf2 --- /dev/null +++ b/src/entrypoints/death-stack-x-decoder/Makefile @@ -0,0 +1,21 @@ +BASE := ../../../ +BOARDCORE := $(BASE)skyward-boardcore/ + +all: + g++ -std=c++17 -O2 -o logdecoder death-stack-x-decoder.cpp \ + -DCOMPILE_FOR_X86 \ + $(BOARDCORE)libs/tscpp/stream.cpp \ + -I$(BOARDCORE)src/shared/drivers/Xbee \ + -I$(BOARDCORE)src/shared/logger \ + -I$(BASE)src/boards/DeathStack \ + -I$(BASE)src/boards/DeathStack/NavigationSystem \ + -I$(BOARDCORE)src/shared \ + -I$(BOARDCORE)src/tests \ + -I$(BOARDCORE)libs \ + -I$(BOARDCORE)libs/miosix-kernel/miosix \ + -I$(BOARDCORE)libs/eigen \ + -I$(BOARDCORE)src/shared/sensors/analog/battery \ + -I$(BOARDCORE)src/shared/sensors/analog/current + +clean: + rm logdecoder diff --git a/src/boards/DeathStack/TMTCManager/XbeeInterrupt.h b/src/entrypoints/death-stack-x-decoder/death-stack-x-decoder.cpp similarity index 81% rename from src/boards/DeathStack/TMTCManager/XbeeInterrupt.h rename to src/entrypoints/death-stack-x-decoder/death-stack-x-decoder.cpp index 8e5fdc24409ae891e2dd41ed4a583380cbbba9fa..1d6889d00e7df536d1c038fa931857232de59665 100644 --- a/src/boards/DeathStack/TMTCManager/XbeeInterrupt.h +++ b/src/entrypoints/death-stack-x-decoder/death-stack-x-decoder.cpp @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,16 +13,12 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#pragma once - -namespace DeathStackBoard -{ -void enableXbeeInterrupt(); -} \ No newline at end of file +#include "LogTypes.h" +#include "logger/decoder/logdecoder.cpp" \ No newline at end of file diff --git a/src/entrypoints/death-stack-entry.cpp b/src/entrypoints/death-stack-x-entry.cpp similarity index 66% rename from src/entrypoints/death-stack-entry.cpp rename to src/entrypoints/death-stack-x-entry.cpp index 97206f6d58fc3f985ad9598e706185d0b9fdad3d..b21dd0366f8b985ab4484f12e29e9e1e0fc64fa7 100644 --- a/src/entrypoints/death-stack-entry.cpp +++ b/src/entrypoints/death-stack-x-entry.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,43 +13,48 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <Common.h> -#include <DeathStack/DeathStack.h> -#include <DeathStack/TMTCManager/TMBuilder.h> -#include <DeathStack/TMTCManager/XbeeInterrupt.h> + +#include <DeathStack.h> +#include <Debug.h> +#include <FlightStatsRecorder/FSRController.h> #include <diagnostic/CpuMeter.h> #include <math/Stats.h> -#include "DeathStack/System/StackLogger.h" -#include "DeathStack/System/SystemData.h" -#include "DeathStack/events/EventInjector.h" +#include <miosix.h> -using namespace DeathStackBoard; using namespace miosix; - -DeathStack* board; - -StatsResult cpu_stat_res; - - -D(EventInjector debug_console;) +using namespace DeathStackBoard; +// using namespace GlobalBuffers; int main() { - D(debug_console.start()); + PrintLogger log = Logging::getLogger("main"); +#ifndef DEBUG // if not debugging, output to file only INFO level or higher + unique_ptr<LogSink> log_sink = std::make_unique<FileLogSinkBuffered>(); + log_sink->setLevel(LOGL_INFO); + Logging::addLogSink(log_sink); +#endif Stats cpu_stat; - board = DeathStack::getInstance(); - - // Log CPU Usage + StatsResult cpu_stat_res; SystemData system_data; - while (1) + + // Instantiate the stack + DeathStack::getInstance()->start(); + LOG_INFO(log, "Death stack started"); + + LoggerService* logger_service = LoggerService::getInstance(); + + for (;;) { + Thread::sleep(1000); + logger_service->log(logger_service->getLogger().getLogStats()); + StackLogger::getInstance()->updateStack(THID_ENTRYPOINT); system_data.timestamp = miosix::getTick(); @@ -65,16 +69,8 @@ int main() system_data.min_free_heap = MemoryProfiling::getAbsoluteFreeHeap(); system_data.free_heap = MemoryProfiling::getCurrentFreeHeap(); - board->logger->log(system_data); - - // Log logger stats - LogStats stats = Logger::instance().getLogStats(); - stats.timestamp = miosix::getTick(); - board->logger->log(stats); - - // Log threads stack data + logger_service->log(system_data); + StackLogger::getInstance()->log(); - // printf("CPU: %.2f\n", system_data.cpu_usage); - Thread::sleep(1000); } } \ No newline at end of file diff --git a/src/entrypoints/death-stack-x-testsuite.cpp b/src/entrypoints/death-stack-x-testsuite.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c2184e33a249d056c6d436deb222cc7dd69c41e1 --- /dev/null +++ b/src/entrypoints/death-stack-x-testsuite.cpp @@ -0,0 +1,113 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <AirBrakes/AirBrakesServo.h> +#include <Common.h> +#include <Deployment/DeploymentServo.h> +#include <drivers/adc/ADS1118/ADS1118.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/hbridge/HBridge.h> +#include <drivers/spi/SPIDriver.h> +#include <interfaces-impl/hwmapping.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/MS5803/MS5803.h> +#include <sensors/analog/battery/BatteryVoltageSensor.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h> + +#include <ctime> +#include <iostream> +#include <sstream> +#include <vector> + +#include "PinHandler/PinHandler.h" +#include "drivers/servo/servo.h" +#include "math/Stats.h" + +using namespace std; + +namespace PowerBoardTest +{ +#include "../tests/deathstack-boards/test-power-board.cpp" +} + +namespace STMBoardTest +{ +#include "../tests/deathstack-boards/test-stm-board.cpp" +} + +namespace RFBoardTest +{ +#include "../tests/deathstack-boards/test-rf-board.cpp" +} + +namespace AnalogBoardTest +{ +#include "../tests/deathstack-boards/test-analog-board.cpp" +} + +int menu(); + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + switch (menu()) + { + case 1: + PowerBoardTest::main(); + break; + case 2: + STMBoardTest::main(); + break; + case 3: + RFBoardTest::main(); + break; + case 4: + AnalogBoardTest::main(); + break; + + default: + break; + } +} + +int menu() +{ + string temp; + int choice; + + printf("\n\nWhat do you want to do?\n"); + printf("1. Test power board\n"); + printf("2. Test stm board\n"); + printf("3. Test rf board\n"); + printf("4. Test analog board\n"); + printf("\n>> "); + getline(cin, temp); + stringstream(temp) >> choice; + + return choice; +} diff --git a/src/entrypoints/deserializer/LogTypes.h b/src/entrypoints/deserializer/LogTypes.h deleted file mode 100644 index 80bfd907f921d0e647816bd90071d5791c497c38..0000000000000000000000000000000000000000 --- a/src/entrypoints/deserializer/LogTypes.h +++ /dev/null @@ -1,127 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef SRC_SHARED_BOARDS_HELITEST_ROGALLINA_DESERIALIZE_H -#define SRC_SHARED_BOARDS_HELITEST_ROGALLINA_DESERIALIZE_H - -#include <fstream> -#include <iostream> - -#include <diagnostic/StackData.h> -#include "DeathStack//System/SystemData.h" -#include "DeathStack/ADA/ADAStatus.h" -#include "DeathStack/DeathStackStatus.h" -#include "DeathStack/DeploymentController/DeploymentData.h" -#include "DeathStack/FlightModeManager/FMMStatus.h" -#include "DeathStack/LoggerService/FlightStatsData.h" -#include "DeathStack/PinHandler/PinHandlerData.h" -#include "DeathStack/SensorManager/SensorManagerData.h" -#include "DeathStack/SensorManager/Sensors/AD7994WrapperData.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapperData.h" -#include "DeathStack/SensorManager/Sensors/PiksiData.h" -#include "DeathStack/events/EventData.h" -#include "drivers/mavlink/MavlinkStatus.h" -#include "logger/Deserializer.h" -#include "logger/LogStats.h" -#include "scheduler/TaskSchedulerData.h" -#include "sensors/MPU9250/MPU9250Data.h" -#include "sensors/MS580301BA07/MS580301BA07Data.h" - -using std::ofstream; - -using namespace DeathStackBoard; - -template <typename T> -void print(T& t, ostream& os) -{ - t.print(os); -} - -void registerTypes(Deserializer& ds) -{ - ds.registerType<LogStats>(print<LogStats>, LogStats::header()); - ds.registerType<SensorManagerStatus>(print<SensorManagerStatus>, - SensorManagerStatus::header()); - ds.registerType<LM75BData>(print<LM75BData>, LM75BData::header()); - // ds.registerType<SensorStatus>(print<SensorStatus>, - // SensorStatus::header()); - - ds.registerType<AD7994WrapperData>(print<AD7994WrapperData>, - AD7994WrapperData::header()); - - ds.registerType<BatteryVoltageData>(print<BatteryVoltageData>, - BatteryVoltageData::header()); - ds.registerType<CurrentSenseData>(print<CurrentSenseData>, - CurrentSenseData::header()); - - ds.registerType<MPU9250Data>(print<MPU9250Data>, MPU9250Data::header()); - - ds.registerType<PiksiData>(print<PiksiData>, PiksiData::header()); - ds.registerType<TaskStatResult>(print<TaskStatResult>, - TaskStatResult::header()); - - ds.registerType<SystemData>(print<SystemData>, SystemData::header()); - - ds.registerType<ApogeeDetected>(print<ApogeeDetected>, - ApogeeDetected::header()); - ds.registerType<DplAltitudeReached>(print<DplAltitudeReached>, - DplAltitudeReached::header()); - ds.registerType<ADAControllerStatus>(print<ADAControllerStatus>, - ADAControllerStatus::header()); - ds.registerType<KalmanState>(print<KalmanState>, KalmanState::header()); - ds.registerType<ADAData>(print<ADAData>, ADAData::header()); - ds.registerType<TargetDeploymentAltitude>( - print<TargetDeploymentAltitude>, TargetDeploymentAltitude::header()); - ds.registerType<ReferenceValues>(print<ReferenceValues>, - ReferenceValues::header()); - - ds.registerType<DeploymentStatus>(print<DeploymentStatus>, - DeploymentStatus::header()); - - ds.registerType<FMMStatus>(print<FMMStatus>, FMMStatus::header()); - - ds.registerType<LiftOffStats>(print<LiftOffStats>, LiftOffStats::header()); - ds.registerType<ApogeeStats>(print<ApogeeStats>, ApogeeStats::header()); - ds.registerType<DrogueDPLStats>(print<DrogueDPLStats>, - DrogueDPLStats::header()); - ds.registerType<MainDPLStats>(print<MainDPLStats>, MainDPLStats::header()); - - ds.registerType<PinStatus>(print<PinStatus>, PinStatus::header()); - - ds.registerType<MavlinkStatus>(print<MavlinkStatus>, - MavlinkStatus::header()); - - ds.registerType<EventData>(print<EventData>, EventData::header()); - ds.registerType<DeathStackStatus>(print<DeathStackStatus>, - DeathStackStatus::header()); - - // ds.registerType<StackData>(print<StackData>, StackData::header()); - ds.registerType<ReferenceValues>(print<ReferenceValues>, - ReferenceValues::header()); - - ds.registerType<MS5803Data>(print<MS5803Data>, MS5803Data::header()); - ds.registerType<StackData>(print<StackData>, StackData::header()); - ds.registerType<CutterTestStats>(print<CutterTestStats>, - CutterTestStats::header()); -} - -#endif diff --git a/src/entrypoints/deserializer/Makefile b/src/entrypoints/deserializer/Makefile deleted file mode 100644 index c128fea312e33b96272220d8d12564412ced3951..0000000000000000000000000000000000000000 --- a/src/entrypoints/deserializer/Makefile +++ /dev/null @@ -1,10 +0,0 @@ - -all: - g++ -std=c++11 -O2 -o logdecoder logdecoder.cpp \ - ../../../skyward-boardcore/libs/tscpp/stream.cpp \ - -I../../../skyward-boardcore/libs -I../../boards \ - -I../../../skyward-boardcore/src/shared -I../../ \ - -I../../../ - -clean: - rm logdecoder diff --git a/src/entrypoints/deserializer/logdecoder.cpp b/src/entrypoints/deserializer/logdecoder.cpp index 0155def2d576865455c7ed653c8a1144d8b12e58..cf6d35e2f36b57aab26fcdccebe29d4907f36959 100644 --- a/src/entrypoints/deserializer/logdecoder.cpp +++ b/src/entrypoints/deserializer/logdecoder.cpp @@ -1,29 +1,24 @@ -/*************************************************************************** - * Copyright (C) 2018 by Terraneo Federico * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * As a special exception, if other files instantiate templates or use * - * macros or inline functions from this file, or you compile this file * - * and link it with other works to produce a work based on this file, * - * this file does not by itself cause the resulting work to be covered * - * by the GNU General Public License. However the source code for this * - * file must still be made available in accordance with the GNU General * - * Public License. This exception does not invalidate any other reasons * - * why a work based on this file might be covered by the GNU General * - * Public License. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, see <http://www.gnu.org/licenses/> * - ***************************************************************************/ +/* Copyright (c) 2018-2019 Skyward Experimental Rocketry + * Authors: Federico Terraneo, Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ /* * This is a stub program for the program that will decode the logged data. @@ -33,6 +28,7 @@ #include <sys/stat.h> #include <sys/types.h> #include <tscpp/stream.h> + #include <fstream> #include <iostream> #include <stdexcept> @@ -73,7 +69,7 @@ bool deserializeAll() struct stat st; if (stat(fnext, &st) != 0) { - //cout << "Skipping " << string(fnext) << "\n "; + // cout << "Skipping " << string(fnext) << "\n "; continue; // File not found } // File found diff --git a/src/entrypoints/hardware_in_the_loop/HILSimulationConfig.h b/src/entrypoints/hardware_in_the_loop/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..e23bffcad1ebcc01fb3cced489f257edfa615363 --- /dev/null +++ b/src/entrypoints/hardware_in_the_loop/HILSimulationConfig.h @@ -0,0 +1,148 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" +#include "sensors/SensorInfo.h" + +struct SensorConfig : public SensorInfo +{ + SensorConfig(const std::string s, const uint32_t period) + : SensorInfo{s, period, []() {}, false, false} + { + } +}; + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 3; + +/** baudrate of connection */ +const int SIM_BAUDRATE = 115200; + +/** Period of simulation in milliseconds */ +const int SIMULATION_PERIOD = 100; + +/** sample frequency of sensor (samples/second, Hz) */ +const int IMU_FREQ = 100; +const int ACCEL_FREQ = 100; +const int GYRO_FREQ = 100; +const int MAGN_FREQ = 100; +const int BARO_FREQ = 20; +const int GPS_FREQ = 10; + +/** update frequency of the Navigation System */ +// const int KALMAN_FREQ = 100; +/** update frequency of airbrakes control algorithm */ +// const int CONTROL_FREQ = 10; +/** update frequency of airbrakes control algorithm */ +// const int ADA_FREQ = 20; + +/** Sensors sample periods (in ms) */ +const int HIL_IMU_PERIOD = 1000 / IMU_FREQ; +const int HIL_BARO_PERIOD = 1000 / BARO_FREQ; +const int HIL_GPS_PERIOD = 1000 / GPS_FREQ; + +/** ADA configs + * [TODO?] Prendi valori da config ADA + */ +const float deploymentAltitude = 450; +const float referenceAltitude = 109; // launchpad Altitude for Pont De Sor? +const float referenceTemperature = 15; + +/** sensors configuration */ +const SensorConfig imuConfig("imu", 1000 / IMU_FREQ); +const SensorConfig baroConfig("baro", 1000 / BARO_FREQ); +const SensorConfig gpsConfig("gps", 1000 / GPS_FREQ); + +/** Number of samples per sensor at each simulator iteration */ +const int N_DATA_IMU = (IMU_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; +// const int N_DATA_KALMAN = (KALMAN_FREQ * SIMULATION_PERIOD) / 1000; + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + * + * NOTE : all the fields contained in the struct are floats because + * it is the data type sent over serial by MATLAB + */ +struct SimulatorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + float fix; + float num_satellites; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/entrypoints/hardware_in_the_loop/hil-entry.cpp b/src/entrypoints/hardware_in_the_loop/hil-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..42780794c3270dffdf7c560ad73501464a7728fa --- /dev/null +++ b/src/entrypoints/hardware_in_the_loop/hil-entry.cpp @@ -0,0 +1,245 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#define EIGEN_NO_MALLOC // enable eigen malloc usage assert + +#include <Common.h> +#include <events/EventBroker.h> +#include <events/Events.h> +#include <events/utils/EventCounter.h> + +#include "TimestampTimer.h" +#include "miosix.h" +#include "scheduler/TaskScheduler.h" +#include "sensors/SensorManager.h" + +/* HIL includes */ +#include "hardware_in_the_loop/HIL.h" +#include "hardware_in_the_loop/HIL_algorithms/HILMockKalman.h" + +/* DeathStack includes */ +#include "ADA/ADAController.h" +#include "AirBrakesController/AirBrakesController.h" +#include "DeploymentController/DeploymentController.h" +#include "FlightModeManager/FlightModeManager.h" +#include "NavigationSystem/NASController.h" +#include "PinHandler/PinHandler.h" +#include "diagnostic/CpuMeter.h" + +using namespace std; +using namespace miosix; +using namespace DeathStackBoard; + +/** + * structure that contains all the sensors used in the simulation + */ +struct Sensors +{ + HILImu* imu; + HILBarometer* barometer; + HILGps* gps; +}; + +uint8_t getNextSchedulerId(TaskScheduler* s) +{ + return (s->getTaskStats()).back().id + 1; +} + +void threadFunc(void* arg) +{ + UNUSED(arg); + + /*-------------- [FPM] Flight Phases Manager --------------*/ + HILFlightPhasesManager* flightPhasesManager = + HIL::getInstance()->flightPhasesManager; + + /*flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&setIsSimulationRunning, false));*/ + + /*-------------- [TS] Task Scheduler --------------*/ + + TaskScheduler scheduler; + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); + + /*-------------- [HIL] HILTransceiver --------------*/ + HILTransceiver* simulator = HIL::getInstance()->simulator; + + /*-------------- Sensors & Actuators --------------*/ + + // Definition of the fake sensors for the simulation + Sensors sensors; + sensors.imu = new HILImu(simulator, N_DATA_IMU); + sensors.barometer = new HILBarometer(simulator, N_DATA_BARO); + sensors.gps = new HILGps(simulator, N_DATA_GPS); + + /*-------------- [SM] Sensor Manager --------------*/ + + // instantiate the sensor manager with the given scheduler + SensorManager SM(&scheduler, {{sensors.imu, imuConfig}, + {sensors.barometer, baroConfig}, + {sensors.gps, gpsConfig}}); + + // registering the enabling of the sensors to the flightPhasesManager + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, bind(&SensorManager::enableAllSensors, &SM)); + + /*---------------- [ADA] ADA ---------------*/ + ADAController<HILBaroData, HILGpsData> ada_controller(*sensors.barometer, + *sensors.gps); + + // setDeploymentAltitude when starting the calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setDeploymentAltitude, + &ada_controller, deploymentAltitude)); + + // setReferenceAltitude when starting the calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceAltitude, + &ada_controller, referenceAltitude)); + + // setReferenceTemperature when starting the calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceTemperature, + &ada_controller, referenceTemperature)); + + /*---------------- [NAS] NavigationSystem ---------------*/ + NASController<HILImuData, HILBaroData, HILGpsData> nas_controller( + *sensors.imu, *sensors.barometer, *sensors.gps); + + HIL::getInstance()->setNAS(&nas_controller.getNAS()); + + /*-------------- [CA] Control Algorithm --------------*/ + AirBrakesController<NASData> airbrakes_controller(nas_controller.getNAS()); + + /*-------------- [DPL] Deployment Controller --------------*/ + DeploymentController dpl_controller; + + /*-------------- PinHandler --------------*/ + PinHandler pin_handler; + + /*-------------- Events --------------*/ + + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_FLIGHT_EVENTS); + counter.subscribe(TOPIC_ADA); + counter.subscribe(TOPIC_NAS); + + /* INSERT THE SUBSCRIBING FOR THE EVENTBROKER (inserted in the + * flightPhasesManager object) */ + + /*-------------- Adding tasks to scheduler --------------*/ + + // adding the updating of the NAS to the scheduler + { + TaskScheduler::function_t update_NAS{ + bind(&NASController<HILImuData, HILBaroData, HILGpsData>::update, + &nas_controller)}; + + scheduler.add(update_NAS, (uint32_t)(1000 / KALMAN_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the updating of the ADA to the scheduler + { + TaskScheduler::function_t update_ADA{bind( + &ADAController<HILBaroData, HILGpsData>::update, &ada_controller)}; + + scheduler.add(update_ADA, (uint32_t)(1000 / ADA_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the updating of the airbrakes algorithm to the scheduler + { + TaskScheduler::function_t update_Airbrake{ + bind(&AirBrakesController<NASData>::update, &airbrakes_controller)}; + + scheduler.add(update_Airbrake, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + /*---------- [FMM] FlightModeManager --------*/ + + FlightModeManager fmm; + + /*---------- Starting threads --------*/ + + sEventBroker->start(); + fmm.start(); + ada_controller.start(); + nas_controller.start(); + airbrakes_controller.start(); + // dpl_controller.start(); + // pin_handler.start(); + scheduler.start(); // started only the scheduler instead of the SM + + sEventBroker->post({EV_INIT_OK}, TOPIC_FMM); + Thread::sleep(100); + + HIL::getInstance()->start(); // wait for first message from simulator + + /*---------- Wait for simulator to startup ----------*/ + while (!flightPhasesManager->isSimulationStarted()) + ; + + /*---------- Normal execution --------*/ + + while (flightPhasesManager->isSimulationRunning()) + { + Thread::sleep(1000); + } + + scheduler.stop(); + + sEventBroker->post({EV_LANDED}, TOPIC_FLIGHT_EVENTS); + + Thread::sleep(1000); +} + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + Thread::create(threadFunc, STACK_MIN_FOR_SKYWARD, MAIN_PRIORITY, nullptr); + + unsigned int counter = 0; + for (;;) + { + if (counter == 10) + { + TRACE("CPU : %.2f \n", averageCpuUtilization()); + counter = 0; + } + else + { + counter++; + } + + Thread::sleep(500); + } + + return 0; +} \ No newline at end of file diff --git a/src/entrypoints/lynx-mock-telemetry.cpp b/src/entrypoints/lynx-mock-telemetry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ed988227afe865be8bb59c8f9eb03ffa13386b99 --- /dev/null +++ b/src/entrypoints/lynx-mock-telemetry.cpp @@ -0,0 +1,156 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <ActiveObject.h> +#include <miosix.h> + +#include <cstdio> + +#define _USE_MATH_DEFINES +#include <cmath> + +#include "Radio/Mavlink.h" + +using namespace miosix; + +FastMutex mutex; + +/** + * @brief Generate a sinusoidal signal + * + * @param offs Y axis offset + * @param amp Amplittude of the wave + * @param freq Frequency [Hz] + * @param phase Phase [deg] + * @return float + */ +float sine(float offs, float amp, float freq, float phase) +{ + float t = getTick() / 1000.0f; + + return offs + amp * sin(2 * M_PI * freq * t + phase * M_PI / 180.0f); +} + +void packSensorsTM(mavlink_message_t* out) +{ + mavlink_sensors_tm_t tm; + tm.timestamp = getTick(); + tm.bmx160_acc_x = sine(0.2, 2, 0.1, 45); + tm.bmx160_acc_y = sine(-0.2, 1.7, 0.05, 123); + tm.bmx160_acc_z = sine(9.81, 3.3, 0.11, 0); + + tm.bmx160_gyro_x = sine(0.2, 2, 0.1, 0); + tm.bmx160_gyro_y = sine(-0.2, 1.7, 0.17, 66); + tm.bmx160_gyro_z = sine(0.1, 3.3, 0.4, 145); + + tm.bmx160_mag_x = sine(30, 7, 0.1, 87); + tm.bmx160_mag_y = sine(20, 3, 0.17, 33); + tm.bmx160_mag_z = sine(-25, 5, 0.4, 117); + + tm.bmx160_temp = sine(25, 5, 0.1, 117); + tm.ms5803_temp = sine(27, 3, 0.04, 12); + tm.lis3mdl_temp = sine(21, 4, 0.2, 85); + + tm.ms5803_press = sine(99000, 500, 0.06, 33); + tm.dpl_press = sine(101000, 1500, 0.1, 67); + tm.static_press = sine(95000, 1500, 0.2, 200); + tm.pitot_press = sine(0, 700, 0.4, 176); + + tm.lis3mdl_mag_x = sine(30, 7, 0.1, 11); + tm.lis3mdl_mag_y = sine(20, 3, 0.17, 85); + tm.lis3mdl_mag_z = sine(-25, 5, 0.4, 222); + + tm.gps_lat = sine(45, 0.001, 0.1, 123); + tm.gps_lon = sine(8, 0.0012, 0.08, 1); + tm.gps_alt = sine(1234, 40, 0.1, 0); + tm.gps_fix = sine(0.5, 1, 0.03, 0) > 0; + + tm.c_sense_1 = sine(2, 0.3, 1, 0); + tm.c_sense_2 = sine(1.3, 0.4, 2, 77); + + tm.vbat = sine(11, 1, 0.4, 0); + tm.vbat_5v = sine(11, 1, 0.4, 0); + + mavlink_msg_sensors_tm_encode(0, 0, out, &tm); +} + +void writeMessage(mavlink_message_t& msg) +{ + uint8_t buf[256]; + uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + + { + Lock<FastMutex> l(mutex); + fwrite(buf, sizeof(uint8_t), len, stdout); + fflush(stdout); + } +} + +void handleMessage(mavlink_message_t& msg) +{ + mavlink_message_t ack_msg; + mavlink_ack_tm_t ack; + ack.seq_ack = msg.seq; + ack.recv_msgid = msg.msgid; + mavlink_msg_ack_tm_encode(0, 0, &ack_msg, &ack); + + writeMessage(ack_msg); +} + +class SerialReceiver : public ActiveObject +{ + +protected: + void run() override + { + mavlink_status_t status; + while (!shouldStop()) + { + uint8_t byte; + fread(&byte, sizeof(uint8_t), 1, stdin); + uint8_t res = mavlink_parse_char(0, byte, &parsing, &status); + if (res == 1) + { + handleMessage(parsing); + } + } + } + +private: + mavlink_message_t parsing; +}; + +int main() +{ + mavlink_message_t buf; + + SerialReceiver receiver; + receiver.start(); + + for (;;) + { + auto t = getTick(); + packSensorsTM(&buf); + writeMessage(buf); + Thread::sleepUntil(t + 100); + } +} \ No newline at end of file diff --git a/src/entrypoints/windtunnel-entry.cpp b/src/entrypoints/windtunnel-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8c6a8579220059a4e8725b8421c5e85f386185ba --- /dev/null +++ b/src/entrypoints/windtunnel-entry.cpp @@ -0,0 +1,55 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <miosix.h> + +#include "DeathStack.h" +// #include "Main/GlobalBuffers.h" + +using namespace miosix; +using namespace DeathStackBoard; +// using namespace GlobalBuffers; + +int main() +{ + TRACE("Starting windtunnel test....\n"); + // Instantiate the stack + DeathStack::getInstance()->start(); + TRACE("Running\n"); + + // bool printed = false; + for (;;) + { + Thread::sleep(1000); + LoggerService::getInstance()->log( + LoggerService::getInstance()->getLogger().getLogStats()); + // if(buf_adc_pitot.size() >= GLOBAL_BUF_LEN && !printed) + // { + // printed = true; + // printf("timestamp,voltage_pitot\n"); + // for(auto v : buf_adc_pitot) + // { + // printf("%llu,%f\n", v.adc_timestamp, v.voltage); + // } + // } + } +} diff --git a/src/entrypoints/windtunnel-test-decoder/.gitignore b/src/entrypoints/windtunnel-test-decoder/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..8217283873861e241c84835fc6bfaa1d15ba30e3 --- /dev/null +++ b/src/entrypoints/windtunnel-test-decoder/.gitignore @@ -0,0 +1,2 @@ +logdecoder +logs/ \ No newline at end of file diff --git a/src/entrypoints/windtunnel-test-decoder/LogTypes.h b/src/entrypoints/windtunnel-test-decoder/LogTypes.h new file mode 100644 index 0000000000000000000000000000000000000000..a093dd70ee927cec83d3b309c8789408ed4f8d1d --- /dev/null +++ b/src/entrypoints/windtunnel-test-decoder/LogTypes.h @@ -0,0 +1,86 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <fstream> +#include <iostream> + +#include "AirBrakesController/AirBrakesData.h" +#include "AirBrakesController/WindData.h" +#include "DeathStackStatus.h" +#include "drivers/Xbee/APIFramesLog.h" +#include "drivers/Xbee/XbeeStatus.h" +#include "drivers/adc/ADS1118/ADS1118Data.h" +#include "drivers/mavlink/MavlinkStatus.h" +#include "events/EventData.h" +#include "logger/Deserializer.h" +#include "logger/LogStats.h" +#include "sensors/MS580301BA07/MS580301BA07Data.h" +#include "sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h" +#include "sensors/analog/pressure/honeywell/SSCDANN030PAAData.h" +#include "sensors/analog/pressure/honeywell/SSCDRRN015PDAData.h" + +// Serialized classes +using std::ofstream; + +using namespace DeathStackBoard; + +template <typename T> +void print(T& t, ostream& os) +{ + t.print(os); +} + +template <typename T> +void registerType(Deserializer& ds) +{ + ds.registerType<T>(print<T>, T::header()); +} + +void registerTypes(Deserializer& ds) +{ + registerType<LogStats>(ds); + registerType<EventData>(ds); + + registerType<MS5803Data>(ds); + registerType<ADS1118Data>(ds); + registerType<SSCDRRN015PDAData>(ds); + registerType<SSCDANN030PAAData>(ds); + registerType<MPXHZ6130AData>(ds); + + registerType<Xbee::APIFrameLog>(ds); + registerType<Xbee::ATCommandFrameLog>(ds); + registerType<Xbee::ATCommandResponseFrameLog>(ds); + registerType<Xbee::ModemStatusFrameLog>(ds); + registerType<Xbee::TXRequestFrameLog>(ds); + registerType<Xbee::TXStatusFrameLog>(ds); + registerType<Xbee::RXPacketFrameLog>(ds); + registerType<Xbee::XbeeStatus>(ds); + registerType<Xbee::XbeeStatus>(ds); + + registerType<Xbee::XbeeStatus>(ds); + registerType<DeathStackBoard::DeathStackStatus>(ds); + registerType<MavlinkStatus>(ds); + registerType<AirBrakesData>(ds); + registerType<WindData>(ds); +} diff --git a/src/entrypoints/windtunnel-test-decoder/Makefile b/src/entrypoints/windtunnel-test-decoder/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..2ffbb4941cd64d7c3a338d187bf350475d67974f --- /dev/null +++ b/src/entrypoints/windtunnel-test-decoder/Makefile @@ -0,0 +1,15 @@ +BASE := ../../../ +BOARDCORE := $(BASE)skyward-boardcore/ + +all: + g++ -std=c++17 -O2 -o logdecoder windtunnel-decoder.cpp \ + -DCOMPILE_FOR_X86 \ + $(BOARDCORE)libs/tscpp/stream.cpp \ + -I$(BASE)src/boards/DeathStack \ + -I$(BOARDCORE)src/shared \ + -I$(BOARDCORE)src/tests \ + -I$(BOARDCORE)libs \ + -I$(BOARDCORE)libs/miosix-kernel/miosix + +clean: + rm logdecoder diff --git a/src/entrypoints/windtunnel-test-decoder/windtunnel-decoder.cpp b/src/entrypoints/windtunnel-test-decoder/windtunnel-decoder.cpp new file mode 100644 index 0000000000000000000000000000000000000000..476cc3005c863aa7451ee3e2c99ead201e764e4a --- /dev/null +++ b/src/entrypoints/windtunnel-test-decoder/windtunnel-decoder.cpp @@ -0,0 +1,24 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "LogTypes.h" +#include "logger/decoder/logdecoder.cpp" \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL.h b/src/hardware_in_the_loop/HIL.h new file mode 100644 index 0000000000000000000000000000000000000000..c60df35204d298d8bf907672a009abb9e71105d3 --- /dev/null +++ b/src/hardware_in_the_loop/HIL.h @@ -0,0 +1,90 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "hardware_in_the_loop/HIL_sensors/HILSensors.h" +#include "hardware_in_the_loop/HILFlightPhasesManager.h" +#include "hardware_in_the_loop/simulator_communication/HILTransceiver.h" +#include "hardware_in_the_loop/HILConfig.h" +#include "NavigationAttitudeSystem/NASData.h" + +/** + * @brief Single interface to the hardware-in-the-loop framework. + */ +class HIL : public Singleton<HIL> +{ + friend class Singleton<HIL>; + +public: + HILFlightPhasesManager* flightPhasesManager; + HILTransceiver* simulator; + + /** + * @brief Start the needed hardware-in-the-loop components. + */ + bool start() { return simulator->start(); } + + void stop() { simulator->stop(); } + + void send(ActuatorData d) + { + simulator->setActuatorData(d); + } + + /** + * @brief method that signals to the simulator that the liftoff pin has + * signaled the detachment + */ + void signalLiftoff() + { + flightPhasesManager->setFlagFlightPhase(LIFTOFF_PIN_DETACHED, true); + simulator->setActuatorData(-1); // start code + } + + void setNAS(Sensor<DeathStackBoard::NASData>* nas) + { + flightPhasesManager->setSourceForOutcomes(nas); + } + + bool isSimulationStarted() + { + return flightPhasesManager->isSimulationStarted(); + } + + bool isSimulationStopped() + { + return flightPhasesManager->isSimulationStopped(); + } + + bool isSimulationRunning() + { + return flightPhasesManager->isSimulationRunning(); + } + +private: + HIL() + { + flightPhasesManager = new HILFlightPhasesManager(); + simulator = new HILTransceiver(flightPhasesManager); + } +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HILConfig.h b/src/hardware_in_the_loop/HILConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..d1ba624030a25f6e8081e0e4e03f31393474cfa0 --- /dev/null +++ b/src/hardware_in_the_loop/HILConfig.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +/** + * @brief Configuration file that includes only the right structures described + * in the config file of the test. + * + * Usage: + * #elif <Flag> + * #include "<test-directory>/HILSimulationConfig.h" + * + * REMEMBER: + * when defining the entry in "sbs.conf" you should add to the defines -D<Flag> + * + * WARNING: + * You should always CLEAN your board before flashing a new entrypoint. Some + * flags could still be in memory + */ + +/* Hardware in the loop entrypoint */ +#if defined(HARDWARE_IN_THE_LOOP) +#include "entrypoints/hardware_in_the_loop/HILSimulationConfig.h" +/* serial comunication test */ +#elif defined(HIL_SERIALINTERFACE) +#include "hardware_in_the_loop/test-SerialInterface/HILSimulationConfig.h" +/* serial simulation with sample manager */ +#elif defined(HIL) +#include "hardware_in_the_loop/test-HIL/HILSimulationConfig.h" +#elif defined(HIL_ADA) +#include "hardware_in_the_loop/test-HIL+ADA/HILSimulationConfig.h" +#elif defined(HIL_AEROBRAKE) +#include "hardware_in_the_loop/test-HIL+Airbrake/HILSimulationConfig.h" +#elif defined(HIL_ADA_AEROBRAKE) +#include "hardware_in_the_loop/test-HIL+ADA+Airbrake/HILSimulationConfig.h" +#elif defined(HIL_ADA_AEROBRAKECONTROLLER) +#include "hardware_in_the_loop/test-HIL+ADA+AirbrakeController/HILSimulationConfig.h" +#elif defined(HIL_ADA_AEROBRAKECONTROLLER_NAS) +#include "hardware_in_the_loop/test-HIL+ADA+AirbrakeController+nas/HILSimulationConfig.h" +/* +#elif defined(HIL_<tuoFlag>) +#include "<test-directory>/HILSimulationConfig.h" +*/ +#else +#error You have add the flag of your configuration file for the HIL testing! +#endif \ No newline at end of file diff --git a/src/hardware_in_the_loop/HILFlightPhasesManager.h b/src/hardware_in_the_loop/HILFlightPhasesManager.h new file mode 100644 index 0000000000000000000000000000000000000000..0da4add27129c47b468894cb315cc5b1156938b8 --- /dev/null +++ b/src/hardware_in_the_loop/HILFlightPhasesManager.h @@ -0,0 +1,357 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <events/Events.h> +#include <events/utils/EventCounter.h> + +#include <iostream> +#include <map> + +#include "ActiveObject.h" +#include "Algorithm.h" +#include "HIL_sensors/HILSensors.h" +#include "NavigationAttitudeSystem/NASData.h" +#include "Singleton.h" +#include "TimestampTimer.h" +#include "hardware_in_the_loop/HILConfig.h" +#include "miosix.h" +#include "sensors/Sensor.h" + +using namespace miosix; +using namespace std; + +typedef function<void()> TCallback; + +enum FlightPhases +{ + SIMULATION_STARTED, + CALIBRATION, + LIFTOFF_PIN_DETACHED, + FLYING, + ASCENT, + BURNING, + AEROBRAKES, + SIM_AEROBRAKES, + APOGEE, + PARA1, + PARA2, + SIMULATION_STOPPED +}; + +struct Outcomes +{ + uint64_t t = 0; + float z = 0; + float vz = 0; + + Outcomes() : t(0), z(0), vz(0) {} + Outcomes(float z, float vz) + : t(TimestampTimer::getTimestamp()), z(z), vz(vz) + { + } + + void print(uint64_t t_start) const + { + TRACE("@time : %f [sec]\n", (double)(t - t_start) / 1000000); + TRACE("@altitude : %f [m]\n", z); + TRACE("@velocity : %f [m/s]\n\n", vz); + } +}; + +/** + * @brief Singleton object that manages all the phases of the simulation + */ +class HILFlightPhasesManager +{ + using FlightPhasesFlags = SimulatorData::Flags; + +public: + HILFlightPhasesManager() + { + updateFlags({0, 0, 0, 0, 0, 0}); + counter_flight_events = new EventCounter(*sEventBroker); + counter_flight_events->subscribe(TOPIC_FLIGHT_EVENTS); + + counter_airbrakes = new EventCounter(*sEventBroker); + counter_airbrakes->subscribe(TOPIC_ABK); + + counter_ada = new EventCounter(*sEventBroker); + counter_ada->subscribe(TOPIC_ADA); + + counter_dpl = new EventCounter(*sEventBroker); + counter_dpl->subscribe(TOPIC_DPL); + } + + bool isSimulationStarted() { return flagsFlightPhases[SIMULATION_STARTED]; } + + bool isSimulationStopped() { return flagsFlightPhases[SIMULATION_STOPPED]; } + + bool isSimulationRunning() + { + return flagsFlightPhases[SIMULATION_STARTED] && + !flagsFlightPhases[SIMULATION_STOPPED]; + } + + void registerToFlightPhase(FlightPhases flag, TCallback func) + { + callbacks[flag].push_back(func); + } + + void setFlagFlightPhase(FlightPhases flag, bool isEnable) + { + flagsFlightPhases[flag] = isEnable; + } + + void processFlags(FlightPhasesFlags hil_flags) + { + updateFlags(hil_flags); + + // check if the obsw triggered relevant flight events + checkEvents(); + + vector<FlightPhases> changed_flags; + + // set true when the first packet from the simulator arrives + if (isSetTrue(SIMULATION_STARTED)) + { + t_start = TimestampTimer::getTimestamp(); + + TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n"); + changed_flags.push_back(SIMULATION_STARTED); + } + + if (isSetTrue(CALIBRATION)) + { + TRACE("[HIL] ------- CALIBRATION ! ------- \n"); + changed_flags.push_back(CALIBRATION); + } + + if (isSetFalse(CALIBRATION)) // calibration finalized + { + TRACE("[HIL] ------- READY TO LAUNCH ! ------- \n"); + } + + if (isSetTrue(LIFTOFF_PIN_DETACHED)) + { + TRACE("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n"); + changed_flags.push_back(LIFTOFF_PIN_DETACHED); + } + + if (flagsFlightPhases[FLYING]) + { + if (isSetTrue(FLYING)) + { + t_liftoff = TimestampTimer::getTimestamp(); + sEventBroker->post({EV_UMBILICAL_DETACHED}, + TOPIC_FLIGHT_EVENTS); + + TRACE("[HIL] ------- LIFTOFF ! ------- \n"); + changed_flags.push_back(FLYING); + + TRACE("[HIL] ------- ASCENT ! ------- \n"); + changed_flags.push_back(ASCENT); + } + if (isSetFalse(BURNING)) + { + registerOutcomes(BURNING); + TRACE("[HIL] ------- STOPPED BURNING ! ------- \n"); + changed_flags.push_back(BURNING); + } + if (isSetTrue(SIM_AEROBRAKES)) + { + registerOutcomes(SIM_AEROBRAKES); + changed_flags.push_back(SIM_AEROBRAKES); + } + if (isSetTrue(AEROBRAKES)) + { + registerOutcomes(AEROBRAKES); + TRACE("[HIL] ------- AEROBRAKES ENABLED ! ------- \n"); + changed_flags.push_back(AEROBRAKES); + } + if (isSetTrue(APOGEE)) + { + registerOutcomes(APOGEE); + TRACE("[HIL] ------- APOGEE DETECTED ! ------- \n"); + changed_flags.push_back(APOGEE); + } + if (isSetTrue(PARA1)) + { + registerOutcomes(PARA1); + TRACE("[HIL] ------- PARACHUTE 1 ! ------- \n"); + changed_flags.push_back(PARA1); + } + if (isSetTrue(PARA2)) + { + registerOutcomes(PARA2); + TRACE("[HIL] ------- PARACHUTE 2 ! ------- \n"); + changed_flags.push_back(PARA2); + } + } + else if (isSetTrue(SIMULATION_STOPPED)) + { + changed_flags.push_back(SIMULATION_STOPPED); + t_stop = TimestampTimer::getTimestamp(); + TRACE("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n", + (double)t_stop / 1000000.0f); + printOutcomes(); + } + + /* calling the callbacks subscribed to the changed flags */ + for (unsigned int i = 0; i < changed_flags.size(); i++) + { + vector<TCallback> callbacksToCall = callbacks[changed_flags[i]]; + for (unsigned int j = 0; j < callbacksToCall.size(); j++) + { + callbacksToCall[j](); + } + } + + prev_flagsFlightPhases = flagsFlightPhases; + } + + void setSourceForOutcomes(Sensor<DeathStackBoard::NASData>* nas) + { + this->nas = nas; + } + +private: + void registerOutcomes(FlightPhases phase) + { + outcomes[phase] = + Outcomes(nas->getLastSample().z, nas->getLastSample().vz); + } + + void printOutcomes() + { + TRACE("OUTCOMES: (times dt from liftoff)\n\n"); + TRACE("Simulation time: %.3f [sec]\n\n", + (double)(t_stop - t_start) / 1000000.0f); + + TRACE("Motor stopped burning (simulation flag): \n"); + outcomes[BURNING].print(t_liftoff); + + TRACE("Airbrakes exit shadowmode: \n"); + outcomes[AEROBRAKES].print(t_liftoff); + + TRACE("Apogee: \n"); + outcomes[APOGEE].print(t_liftoff); + + TRACE("Parachute 1: \n"); + outcomes[PARA1].print(t_liftoff); + + TRACE("Parachute 2: \n"); + outcomes[PARA2].print(t_liftoff); + } + + /** + * @brief Updates the flags of the object with the flags sent from matlab + * and checks for the apogee + */ + void updateFlags(FlightPhasesFlags hil_flags) + { + flagsFlightPhases[ASCENT] = hil_flags.flag_ascent; + flagsFlightPhases[FLYING] = hil_flags.flag_flight; + flagsFlightPhases[BURNING] = hil_flags.flag_burning; + + /* Flags PARA1, PARA2 and SIM_AEROBRAKES ignored from matlab */ + // flagsFlightPhases[SIM_AEROBRAKES] = hil_flags.flag_airbrakes; + // flagsFlightPhases[PARA1] = hil_flags.flag_para1; + // flagsFlightPhases[PARA2] = hil_flags.flag_para2; + + flagsFlightPhases[SIMULATION_STOPPED] = + isSetFalse(FLYING) || prev_flagsFlightPhases[SIMULATION_STOPPED]; + } + + void checkEvents() + { + // calibration flag set to true at the beginning of the simulation + if (!prev_flagsFlightPhases[CALIBRATION] && + counter_flight_events->getCount(EV_CALIBRATION_OK) == 0 && + counter_flight_events->getCount(EV_TC_CALIBRATE_SENSORS) > 0) + { + setFlagFlightPhase(CALIBRATION, true); + } + + // calibration flag turned false when calibration finishes + if (prev_flagsFlightPhases[CALIBRATION] && + counter_flight_events->getCount(EV_CALIBRATION_OK) > 0) + { + setFlagFlightPhase(CALIBRATION, false); + } + + // airbrakes flag turned true when the shadow mode ended + if (!prev_flagsFlightPhases[AEROBRAKES] && + counter_airbrakes->getCount(EV_SHADOW_MODE_TIMEOUT) > 0) + { + setFlagFlightPhase(AEROBRAKES, true); + } + + // apogee flag turned true when apogee is detected + if (!prev_flagsFlightPhases[APOGEE] && + counter_ada->getCount(EV_ADA_APOGEE_DETECTED) > 0) + { + setFlagFlightPhase(APOGEE, true); + } + + // parachute 1 flag turned true when apogee is detected + if (!prev_flagsFlightPhases[PARA1] && + counter_dpl->getCount(EV_NC_OPEN) > 0) + { + setFlagFlightPhase(PARA1, true); + } + + // parachute 2 flag turned true with event deployment altitude detected + if (!prev_flagsFlightPhases[PARA2] && + counter_ada->getCount(EV_ADA_DPL_ALT_DETECTED) > 0) + { + setFlagFlightPhase(PARA2, true); + } + } + + bool isSetTrue(FlightPhases phase) + { + return flagsFlightPhases[phase] == true && + prev_flagsFlightPhases[phase] == false; + } + + bool isSetFalse(FlightPhases phase) + { + return flagsFlightPhases[phase] == false && + prev_flagsFlightPhases[phase] == true; + } + + uint64_t t_start = 0; + uint64_t t_liftoff = 0; + uint64_t t_stop = 0; + map<FlightPhases, bool> flagsFlightPhases; + map<FlightPhases, bool> prev_flagsFlightPhases; + map<FlightPhases, vector<TCallback>> callbacks; + map<FlightPhases, Outcomes> outcomes; + Sensor<DeathStackBoard::NASData>* nas; + + EventCounter* counter_flight_events; + EventCounter* counter_airbrakes; + EventCounter* counter_ada; + EventCounter* counter_dpl; +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_actuators/HILServo.h b/src/hardware_in_the_loop/HIL_actuators/HILServo.h new file mode 100644 index 0000000000000000000000000000000000000000..1b407fff8a8556cdbe46a7a6d5e1efbdcda8fe5e --- /dev/null +++ b/src/hardware_in_the_loop/HIL_actuators/HILServo.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "DeathStack/ServoInterface.h" +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/simulator_communication/HILTransceiver.h" + +class HILServo : public DeathStackBoard::ServoInterface +{ +public: + /** + * @brief constructor of the fake actuator used for the simulation. + * + * @param matlab reference of the MatlabTransceiver object that deals with + * the simulator + */ + HILServo(HILTransceiver *matlab) + : DeathStackBoard::ServoInterface(MinAlphaDegree, MaxAlphaDegree) + { + this->matlab = matlab; + } + + void enable() override + { + reset(); + isEnabled = true; + } + + void disable() override + { + reset(); + isEnabled = false; + } + + /** + * @brief Initializes the fake actuator + */ + bool init() + { + initialized = true; + return true; + } + + void selfTest() override { return; } + +protected: + /** + * @brief sets the actuator data in the MatlabTransceiver object, then will + * be sent to the simulator + * + * @param value opening in radiants + */ + void setPosition(ActuatorData value) { matlab->setActuatorData(value); } + + /* + * converts the value that the real servo driver accepts to the value the + * matlab simulator accepts + */ + // float convertToDegree(float x) { return (x * 180 / PI); } + + HILTransceiver *matlab; /**< reference to the MatlabTranceiver */ + bool initialized = false; + bool isEnabled = true; +}; diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..f9269dfeff5712c83a1cf5626222684b9cb0b0ca --- /dev/null +++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h @@ -0,0 +1,93 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "DeathStack/Algorithm.h" +#include "DeathStack/ServoInterface.h" +#include "hardware_in_the_loop/HILConfig.h" +#include "miosix.h" +#include "sensors/Sensor.h" + +using namespace DeathStackBoard; +using namespace miosix; + +/** + * @brief Example of how a control algorithm should be created. + * + * The control algorithm takes data from the Sensors, elaborates it and sends + * the result to the actuator. + */ +template <typename KD> +class MockAirbrakeAlgorithm : public Algorithm +{ +public: + /** + * @brief constructor that takes the state of the rocket and the actuator + * @param kalman HILSensor that has as data structure the first templated + * parameter + * @param servo the actuator that will communicate with the Simulator + */ + MockAirbrakeAlgorithm(Sensor<KD>* kalman, ServoInterface* servo) + { + /* [TODO] + * Check if the sensors have the data we need + */ + this->kalman = kalman; + this->servo = servo; + lastSample.timestamp = 0; + } + + bool init() override { return true; } + +protected: + /** + * @brief Example of computation on the data sent by the Simulator + */ + void step() override + { + KD state; + { + // [REVIEW] PauseKernelLock kLock; + state = kalman->getLastSample(); + } + + if (lastSample.timestamp < state.timestamp) + { + lastSample.timestamp = state.timestamp; + + /* + * In here you can put the control algorithm + */ + + actuatorData = MaxAlphaDegree / 2; // it's more or less 25 degrees + } + + servo->set(actuatorData); + } + +private: + ActuatorData actuatorData; + TimestampData lastSample; /**< keeps track of the last timestamp */ + Sensor<KD>* kalman; /**< reference to the kalman object */ + ServoInterface* servo; /**< reference to the actuator object */ +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockKalman.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockKalman.h new file mode 100644 index 0000000000000000000000000000000000000000..d80a17feea12b6f7c7147f390064e81573679aa9 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockKalman.h @@ -0,0 +1,67 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "hardware_in_the_loop/HIL_sensors/HILAccelerometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILGps.h" +#include "hardware_in_the_loop/HIL_sensors/HILGyroscope.h" +#include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILSensor.h" + +struct HILKalmanData : public TimestampData +{ + HILKalmanData() : TimestampData{0} {} + + HILKalmanData(uint64_t t) : TimestampData{t} {} + + float z; + float vz; + float vMod; +}; + +class HILKalman : public HILSensor<HILKalmanData> +{ +public: + HILKalman(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) + { + } + +protected: + HILKalmanData updateData() override + { + /* + * In here you put the kalman algorithm that from the sensorData + * extracts the state + */ + + HILKalmanData tempData; + tempData.z = sensorData->kalman.z; + tempData.vz = sensorData->kalman.vz; + tempData.vMod = sensorData->kalman.vMod; + tempData.timestamp = updateTimestamp(); + + return tempData; + } +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h new file mode 100644 index 0000000000000000000000000000000000000000..07d212a5a0ce9fca59500120abeb360cfd49dd25 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h @@ -0,0 +1,77 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "NavigationSystem/NASController.h" +#include "hardware_in_the_loop/HIL_sensors/HILAccelerometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILGps.h" +#include "hardware_in_the_loop/HIL_sensors/HILGyroscope.h" +#include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILSensor.h" + +using namespace DeathStackBoard; + +struct HILNasData : public TimestampData +{ + HILNasData() : TimestampData{0} {} + + HILNasData(uint64_t t) : TimestampData{t} {} + + float z; + float vz; + float vMod; +}; + +class HILNas : public HILSensor<HILNasData> +{ +public: + HILNas(HILTransceiver *matlab, int n_data_kalm, + NASController<HILImuData, HILBaroData, HILGpsData> *nas) + : HILSensor(matlab, n_data_kalm) + { + this->nas = nas->getNAS(); + } + +protected: + /** + * @brief Updates the internal structure of the fake sensor from the + * structure received from the simulator. + * + * Takes the next unread sample available, continues sending the last sample + * with the old timestamp if we already read all the samples. + */ + HILNasData updateData() override + { + HILNasData tempData; + NASData nas_data = nas->getNasData(); + tempData.z = nas_data.pz; + tempData.vz = nas_data.vz; + tempData.vMod = nas_data.vmod; + tempData.timestamp = nas_data.timestamp; + + return tempData; + } + + NAS<HILImuData, HILBaroData, HILGpsData> *nas; +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h b/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h new file mode 100644 index 0000000000000000000000000000000000000000..69f2ffabccbc093dcf61360dbde6b888857dd374 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILAccelerometer.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "HILSensor.h" +#include "math/Vec3.h" + +/** + * @brief fake accelerometer sensor used for the simulation. + * + * This class is used to simulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a simulator. + */ +class HILAccelerometer : public HILSensor<HILAccelData> +{ +public: + HILAccelerometer(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) + { + } + +protected: + HILAccelData updateData() override + { + HILAccelData tempData; + + /* I make a copy of the vector i have to memorize in the sensor + * struct */ + Vec3 matlabData = sensorData->accelerometer.measures[sampleCounter]; + + tempData.accel_x = matlabData.getX(); + tempData.accel_y = matlabData.getY(); + tempData.accel_z = matlabData.getZ(); + tempData.accel_timestamp = updateTimestamp(); + + return tempData; + } +}; \ No newline at end of file diff --git a/src/boards/DeathStack/SensorManager/Sensors/AD7994WrapperData.h b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h similarity index 54% rename from src/boards/DeathStack/SensorManager/Sensors/AD7994WrapperData.h rename to src/hardware_in_the_loop/HIL_sensors/HILBarometer.h index 21c474697f90b7ddf7f03dc2001f2d892ab74ca5..3c23d72114bde57378b8eeffb74f151d55349bd3 100644 --- a/src/boards/DeathStack/SensorManager/Sensors/AD7994WrapperData.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -23,32 +22,32 @@ #pragma once -#include <cstdint> -#include <ostream> +#include "HILSensor.h" +#include "math/Vec3.h" -struct AD7994WrapperData +/** + * @brief fake barometer sensor used for the simulation. + * + * This class is used to simulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a simulator. + */ +class HILBarometer : public HILSensor<HILBaroData> { - long long timestamp; - - uint16_t honeywell_baro_volt; - uint16_t nxp_baro_volt; - - float honeywell_baro_pressure; - float nxp_baro_pressure; - - bool honeywell_baro_flag; - bool nxp_baro_flag; - - static std::string header() +public: + HILBarometer(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) { - return "timestamp,honeywell_baro_volt,nxp_baro_volt,honeywell_baro_" - "pressure,nxp_baro_pressure,honeywell_baro_flag,nxp_baro_flag\n"; } - void print(std::ostream& os) const +protected: + HILBaroData updateData() override { - os << timestamp << "," << honeywell_baro_volt << "," << nxp_baro_volt - << "," << honeywell_baro_pressure << "," << nxp_baro_pressure << "," - << honeywell_baro_flag << "," << nxp_baro_flag << "\n"; + HILBaroData tempData; + + tempData.press = sensorData->barometer.measures[sampleCounter]; + tempData.press_timestamp = updateTimestamp(); + + return tempData; } -}; +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGps.h b/src/hardware_in_the_loop/HIL_sensors/HILGps.h new file mode 100644 index 0000000000000000000000000000000000000000..093e547799c1a999ddaa898f5cc89dd89e7a1504 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILGps.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "HILSensor.h" +#include "math/Vec3.h" + +/** + * @brief fake gps sensor used for the HILulation. + * + * This class is used to HILulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a HILulator. + */ +class HILGps : public HILSensor<HILGpsData> +{ +public: + HILGps(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) + { + } + +protected: + HILGpsData updateData() override + { + HILGpsData tempData; + + /* I make a copy of the vector i have to memorize in the sensor + * struct */ + Vec3 matlabData = sensorData->gps.positionMeasures[sampleCounter]; + tempData.latitude = + matlabData.getX(); // divide by earth radius + tempData.longitude = matlabData.getY(); + tempData.height = matlabData.getZ(); + + matlabData = sensorData->gps.velocityMeasures[sampleCounter]; + tempData.velocity_north = matlabData.getX(); + tempData.velocity_east = matlabData.getY(); + tempData.velocity_down = matlabData.getZ(); + + tempData.fix = (bool)sensorData->gps.fix; + tempData.num_satellites = (uint8_t)sensorData->gps.num_satellites; + + tempData.gps_timestamp = updateTimestamp(); + + return tempData; + } +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h new file mode 100644 index 0000000000000000000000000000000000000000..2446badd8684cc8d65437800b05b85c72c4f0b74 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "HILSensor.h" +#include "math/Vec3.h" + +/** + * @brief fake gyroscope sensor used for the simulation. + * + * This class is used to simulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a simulator. + */ +class HILGyroscope : public HILSensor<HILGyroscopeData> +{ +public: + HILGyroscope(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) + { + } + +protected: + HILGyroscopeData updateData() override + { + HILGyroscopeData tempData; + + /* I make a copy of the vector i have to memorize in the sensor + * struct */ + Vec3 matlabData = sensorData->gyro.measures[sampleCounter]; + + tempData.gyro_x = matlabData.getX(); + tempData.gyro_y = matlabData.getY(); + tempData.gyro_z = matlabData.getZ(); + tempData.gyro_timestamp = updateTimestamp(); + + return tempData; + } +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILImu.h b/src/hardware_in_the_loop/HIL_sensors/HILImu.h new file mode 100644 index 0000000000000000000000000000000000000000..8a68c9c7ae39a79444f42bfdb5a277876a463dc0 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILImu.h @@ -0,0 +1,77 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "HILAccelerometer.h" +#include "HILGyroscope.h" +#include "HILMagnetometer.h" +#include "HILSensor.h" +#include "math/Vec3.h" + +/** + * @brief fake 9-axis IMU sensor used for the simulation. + * + * This class is used to simulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a simulator. + */ +class HILImu : public HILSensor<HILImuData> +{ +public: + HILImu(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) + { + } + +protected: + HILImuData updateData() override + { + HILImuData tempData; + Vec3 matlabData; + + /* I make a copy of the vector i have to memorize in the sensor + * struct */ + matlabData = sensorData->accelerometer.measures[sampleCounter]; + tempData.accel_x = matlabData.getX(); + tempData.accel_y = matlabData.getY(); + tempData.accel_z = matlabData.getZ(); + + matlabData = sensorData->gyro.measures[sampleCounter]; + tempData.gyro_x = matlabData.getX(); + tempData.gyro_y = matlabData.getY(); + tempData.gyro_z = matlabData.getZ(); + + matlabData = sensorData->magnetometer.measures[sampleCounter]; + tempData.mag_x = matlabData.getX(); + tempData.mag_y = matlabData.getY(); + tempData.mag_z = matlabData.getZ() / 1000.0f; // from nanotesla to microtesla + + // only update the timestamp once and use it for all the 3 sensors + // (this sensor assumes the same frequency for accel, gyro and mag) + tempData.accel_timestamp = updateTimestamp(); + tempData.gyro_timestamp = tempData.accel_timestamp; + tempData.mag_timestamp = tempData.accel_timestamp; + + return tempData; + } +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h new file mode 100644 index 0000000000000000000000000000000000000000..91d4beff5836f61c7a352c5639970ac7f8f9d945 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "HILSensor.h" +#include "math/Vec3.h" + +/** + * @brief fake magnetometer sensor used for the simulation. + * + * This class is used to simulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a simulator. + */ +class HILMagnetometer : public HILSensor<HILMagnetometerData> +{ +public: + HILMagnetometer(HILTransceiver *matlab, int n_data_sensor) + : HILSensor(matlab, n_data_sensor) + { + } + +protected: + HILMagnetometerData updateData() override + { + HILMagnetometerData tempData; + + /* I make a copy of the vector i have to memorize in the sensor + * struct */ + Vec3 matlabData = sensorData->magnetometer.measures[sampleCounter]; + + tempData.mag_x = matlabData.getX(); + tempData.mag_y = matlabData.getY(); + tempData.mag_z = matlabData.getZ() / 1000.0f; // from nanotesla to microtesla + tempData.mag_timestamp = updateTimestamp(); + + return tempData; + } +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h new file mode 100644 index 0000000000000000000000000000000000000000..57a1f0768e50e35f3250bbc2418283915a50a126 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h @@ -0,0 +1,159 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <typeinfo> + +#include "HILTimestampManagement.h" +#include "TimestampTimer.h" +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/simulator_communication/HILTransceiver.h" +#include "math/Vec3.h" +#include "sensors/Sensor.h" +#include "sensors/SensorData.h" +#include "HILSensorsData.h" + +/** + * @brief Fake sensor base used for the simulation. Every sensor for the + * simulation should extend this class. + * + * This class is used to simulate as near as possible the situation of the + * OBSW during the flight, using fake sensors classes instead of the real + * ones, taking their data from the data received from a simulator. + */ +template <typename HILSensorData> +class HILSensor : public virtual HILTimestampManagement, + public virtual Sensor<HILSensorData> +{ +public: + /** + * @brief constructor of the fake sensor used for the simulation. + * + * @param matlab reference of the MatlabTransceiver object that deals with + * the simulator + * @param n_data_sensor number of samples in every period of simulation + */ + HILSensor(HILTransceiver *matlab, int n_data_sensor) + { + this->sensorData = matlab->getSensorData(); + this->n_data_sensor = n_data_sensor; + + /* Registers the sensor on the MatlabTransceiver to be notified when a + * new packet of simulated data arrives */ + matlab->addResetSampleCounter(this); + } + + /** + * @brief sets the sample counter to 0. + * + * Updates the reference timestamp, resets the sampleCounter and clears the + * last_error variable. Called by the HILTransceiver when receives a new + * simulated period. + */ + void resetSampleCounter() override + { + this->last_error = SensorErrors::NO_ERRORS; + sampleCounter = 0; + } + + /** + * @brief Initializes the fake sensor. + */ + bool init() override + { + if (initialized) + { + this->last_error = SensorErrors::ALREADY_INIT; + TRACE("ALREADY INITIALIZED!"); + } + else + { + initialized = true; + } + + return initialized; + } + + bool selfTest() override { return true; } + +protected: + /** + * @brief Updates the internal structure of the fake sensor from the + * structure received from the simulator. + * + * Takes the next unread sample available, continues sending the last sample + * with the old timestamp if we already read all the samples. + */ + HILSensorData sampleImpl() override + { + if (initialized) + { + /* updates the last_sensor only if there is still data to be read */ + if (sampleCounter >= n_data_sensor) + { + this->last_error = SensorErrors::NO_NEW_DATA; + /*TRACE("[%s] NO NEW DATA! Simulation error\n", + typeid(this).name());*/ + } + else if (this->last_error != SensorErrors::NO_NEW_DATA) + { + return updateData(); + } + } + else + { + this->last_error = SensorErrors::NOT_INIT; + TRACE( + "[HILSensor] sampleImpl() : not initialized, unable to " + "sample data \n"); + } + + return this->last_sample; + } + + /** + * @brief updates the timestamp and increments the sampleCounter. + * WARNING: You should call this method after all the values has been + * updated, it modifies the sampleCounter! + * @return the timestamp of the sample + */ + uint64_t updateTimestamp() + { + sampleCounter++; + return TimestampTimer::getTimestamp(); + } + + /** + * @brief Function that updates the sensor structure with new data. + * + * Sensor struct updated from MatlabTransceiver::sensorData. + * WARNING: This method should call **AT THE END** the updateTimestamp + * method. + */ + virtual HILSensorData updateData() = 0; + + bool initialized = false; + int sampleCounter = 0; /**< counter of the next sample to take */ + int n_data_sensor; /**< number of samples in every period */ + SimulatorData *sensorData; /**< reference to the SensorData structure */ +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensors.h b/src/hardware_in_the_loop/HIL_sensors/HILSensors.h new file mode 100644 index 0000000000000000000000000000000000000000..f33700392d7f5e521aefc6a6ce5d38b2993b8a84 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILSensors.h @@ -0,0 +1,32 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "HILAccelerometer.h" +#include "HILBarometer.h" +#include "HILGps.h" +#include "HILGyroscope.h" +#include "HILImu.h" +#include "HILMagnetometer.h" +#include "HILSensor.h" +#include "HILTimestampManagement.h" \ No newline at end of file diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h new file mode 100644 index 0000000000000000000000000000000000000000..9acedad4f9115691231fe6356f0e8286031c7953 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h @@ -0,0 +1,114 @@ + +#include <sensors/SensorData.h> + +struct HILAccelData : public AccelerometerData +{ + HILAccelData() : AccelerometerData{0, 0.0, 0.0, 0.0} {} + + HILAccelData(uint64_t t, float x, float y, float z) + : AccelerometerData{t, x, y, z} + { + } + + static std::string header() { return "timestamp,x,y,z\n"; } + + void print(std::ostream& os) const + { + os << accel_timestamp << "," << accel_x << "," << accel_y << "," + << accel_z << "\n"; + } +}; + +struct HILGyroscopeData : public GyroscopeData +{ + HILGyroscopeData() : GyroscopeData{0, 0.0, 0.0, 0.0} {} + + HILGyroscopeData(uint64_t t, float x, float y, float z) + : GyroscopeData{t, x, y, z} + { + } + + static std::string header() { return "timestamp,x,y,z\n"; } + + void print(std::ostream& os) const + { + os << gyro_timestamp << "," << gyro_x << "," << gyro_y << "," << gyro_z + << "\n"; + } +}; + +struct HILMagnetometerData : public MagnetometerData +{ + HILMagnetometerData() : MagnetometerData{0, 0.0, 0.0, 0.0} {} + + HILMagnetometerData(uint64_t t, float x, float y, float z) + : MagnetometerData{t, x, y, z} + { + } + + static std::string header() { return "timestamp,x,y,z\n"; } + + void print(std::ostream& os) const + { + os << mag_timestamp << "," << mag_x << "," << mag_y << "," << mag_z + << "\n"; + } +}; + +struct HILImuData : public HILAccelData, + public HILGyroscopeData, + public HILMagnetometerData +{ + static std::string header() + { + return "accel_timestamp,accel_x,accel_y,accel_z,gyro_timestamp,gyro_x," + "gyro_y,gyro_z,mag_timestamp,mag_x,mag_y,mag_z\n"; + } + + void print(std::ostream& os) const + { + os << accel_timestamp << "," << accel_x << "," << accel_y << "," << accel_z + << "," << gyro_timestamp << "," << gyro_x << "," << gyro_y << "," << gyro_z + << "," << mag_timestamp << "," << mag_x << "," << mag_y << "," << mag_z << "\n"; + } +}; + +struct HILGpsData : public GPSData +{ + HILGpsData() : GPSData{0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3, true} + { + } + + HILGpsData(uint64_t t, float x, float y, float z, float v_x, float v_y, + float v_z, float v) + : GPSData{t, x, y, z, v_x, v_y, v_z, v, 0.0, 3, true} + { + } + + static std::string header() + { + return "timestamp,x,y,z,v_x,v_y,v_z,v,track,n_sats,fix\n"; + } + + void print(std::ostream& os) const + { + os << gps_timestamp << "," << longitude << "," << latitude << "," + << height << "," << velocity_north << "," << velocity_east << "," + << velocity_down << "," << speed << "," << track << "," + << (int)num_satellites << "," << (int)fix << "\n"; + } +}; + +struct HILBaroData : public PressureData +{ + HILBaroData() : PressureData{0, 0.0} {} + + HILBaroData(uint64_t t, float p) : PressureData{t, p} {} + + static std::string header() { return "timestamp,press\n"; } + + void print(std::ostream& os) const + { + os << press_timestamp << "," << press << "\n"; + } +}; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILTimestampManagement.h b/src/hardware_in_the_loop/HIL_sensors/HILTimestampManagement.h new file mode 100644 index 0000000000000000000000000000000000000000..467dfd6554b8c241ebf2f505de5a84d2796f14d7 --- /dev/null +++ b/src/hardware_in_the_loop/HIL_sensors/HILTimestampManagement.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +/** + * @brief Interface in order to reset the parameters of the SimSensor even if + * they are templated on different SimSensorData + */ +class HILTimestampManagement +{ +public: + HILTimestampManagement() {} + + /** + * @brief sets the sample counter to 0. + * + * Updates the reference timestamp, resets the sampleCounter and clears the + * last_error variable. Called by the MatlabTransceiver when receives a new + * simulated period + */ + virtual void resetSampleCounter() = 0; +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/README.md b/src/hardware_in_the_loop/README.md new file mode 100644 index 0000000000000000000000000000000000000000..b1ada2efb793d041e7a0b5c05fa4231d0e63e34a --- /dev/null +++ b/src/hardware_in_the_loop/README.md @@ -0,0 +1,122 @@ +# Hardware in the loop framework + +This README refers to the OBSW (On-Board Software) and matlab framework used for the **Hardware In the Loop** (HIL) testing. + +## HIL framework - OBSW side + +### Dependencies: +- skyward-boardcore branch: sensors-dev +- miosix branch: gcc-9.2.0 + +### Files: +- `test-SerialInterface.cpp`: Used to make quick tests of the serial communication with the simulator. It's a pretty simple example of usage of the *SerialInterface* module, used to test if the the serial connection is working properly. +- `test-HIL.cpp`: Used to test the control algorithms. + - enable timestamp timer for the HILsensors + - define a *HILTransceiver* object + - define all the sensors and the actuators used by the simulation + - define the control algorithm + - add the algorithms to the *addNotifyToBegin* queue of the *HILTransceiver* object + - initialize all the sensors and actuators + - start the HILTransceiver thread + - sample the sensors and update the algorithm at the right rate +- `HILSimulationConfig.h`: Configuration file for the entrypoint; for every type of simulation you should create a directory with your entrypoint and one of this configuration file with these main informations: + - `SIM_BAUDRATE`: Baudrate of the connection for the simulation + - `SIM_PERIOD`: Time of each simulated step [in milliseconds] + - `SENSOR_FREQ`: Frequency of every sensor you will use in the simulation [in Hz = samples/second] + - `SensorData`: Data structure sent by the simulator + - `ActuatorData`: Data structure that the simulator expects back +- `HILConfig.h`: In this file you have to set the right config file to include when a component need the structures or other configuration parameters. You have to choose a *Flag* that represents the config file. +e.g. +``` +#if defined(HIL_SERIALINTERFACE) +#include "test-SerialInterface/HILSimulationConfig.h" +``` +- `SerialInterface.h`: Opens a serial port on the OBSW. After his initialization it's ready to send and receive the data. The default baudrate value is 256000 by default. The default port number value is 2 (for the stm32f407vg_stm32f4discovery is tx_board=PA2 rx_board=PA3) +- `HILTransceiver.h`: Is an ActiveObject and provides an easier interface with the simulator automatically creating a SerialTestSensor object (you can pass the baudrate or the USART port to the constructor) and offers these methods: + - `getSensorData` returns a pointer to the last data simulated. The struct returned (*SensorData*) reflects in number of elements, type and positions the struct sent from the simulator (the struct sent from matlab and SensorData should always correspond!). + - `setActuatorData` sends the data to the simulator + - `addNotifyToBegin` takes an *Algorithm* object as input and starts it when the first packet from the simulator is received + - `addResetSampleCounter` takes a *HILSensor* as input (or every object that implements *HILTimestampManagement*) and notifies to it that new data has arrived from the simulator +- `MockAirbrakeAlgorithm.h`: Example of control algorithm. Receives in the constructor a *HILSensor* (or a kalman that implements *HILSensor* as in this case) and a *ServoInterface*. The algorithm have to *getLastSample* from the sensor passed, elaborates it and then sends the output to the actuator calling his *set* method +- `HILSensor`: Interface implemented by all the sensors and kalmans. To the constructor (templated with the type of the struct used by the sensor to store the last sample) we have to pass the reference to the *HILTransceiver* object, the *simulation period* and the *number of simulated samples* by matlab of this sensor (corresponding to the number of rows of the corresponding field of the sensorData structure in the matlab simulatos). They have to be initialized before use. Everytime the `sample` method is invoked, the sensor takes the last unread sample from the data simulated and creates a timestamp for that sample. If we sample more data then the available one we [continue receiving the last sample/throw an exception/print an error message on the stdin]. We have to register the sensor to the `addResetSampleCounter` queue of *HILTransceiver* in order to be notified when fresh new data is arrived from the simulator, so that the sensor can start reading the data from the beginning of the array. The method `getLastSample` returns the *HILSensorData* structure with the last sample. +- `HILServo`: Is a *ServoInterface* implementation. Interfaces the control algorithm to the HILTransceiver object. Invoking the method `set`, the value passed is converted (if needed) and then sent to the simulator. + +### Usage of the hardware-in-the-loop framework: + +To use this module as-is: +- The first thing to do is to develop the control algorithm in the `step` method of *MockAirbrakeAlgorithm*. Then you should check that the *HILServo* sends back to the simulator the data in the conversion you want. +- Now you should create the entrypoint in the `sbs.conf` file. In the *Defines:* field you should add the flag of your entrypoint corresponding to the one used in `HILConfig.h` in order to choose the right config file (e.g. `Defines: -DHIL`) +- After this you should be able to build the `test-HIL.cpp` and flash the executable on the board. +- Then you should connect via serial the board to the computer (with a serial adapter). The connections between adapter and board are: + - adapter_GND to board_GND + - adapter_RX to board_TX (check table or cheatsheet for available serialPorts and relative pins) + - adapter_TX to board_RX (check table or cheatsheet for available serialPorts and relative pins) + +<center> + +| **board** | **serialPort** | **board_TX** | **board_RX** | +| ---------------------------- | -------------- | ------------ | ------------ | +| stm32f429zi_stm32f4discovery | (USART)3 | PB10 | PB11 | +| stm32f407vg_stm32f4discovery | (USART)2 | PA2 | PA3 | + +</center> + +Now the board is ready for the simulation. + +### Example of usage of the hardware-in-the-loop framework on the OBSW: + +A basic example is available in the *src/tests/test-HIL* directory. + +## HIL framework - Matlab side + +### Files: +- `serialib.h` and `serialib.cpp`: used to build the C++ code for matlab +- `serialbridge.cpp`: the file that implements the serialbridge feature in C++ (not necessary if you don't have to change the framework) +- `build.m`: used for building the serialbridge.cpp file +- `serialbridge.mexw64`: the compiled mex file that provides the serialbridge + function for matlab (this is the only file needed for the simulation) +- `structToSingles.m`: function that converts a generic struct (with also nested structs) to an array of singles, ready to be sent on the serial communication + +### Usage of the hardware-in-the-loop framework: + +First of all, in order to execute the following commands and receive the data back the board should already be flashed and connected properly as above. Then the following functions can be used to program the hardware-in-the-loop simulator: +- `serialbridge("Open", string_serialPort, uint_baudrate)` opens the serial communication with the board; usually the serial communication is opened at the beginning of `start_simulation.m`: + - **"Open"**: specifies we want to open the serial port + - *string_serialPort*: is the serial port we want to open (eg: "COM6"); In order to find the right port you can use on Windows the command `mode` and see the name of the serial port with baudrate *19200* or *256000*. + - *uint_baudrate*: is the baudrate of the port (eg: 256000) +- `serialbridge("Write", singleArray_Data)` sends the data to the board; *you should send all the data in one single write command*: + - **"Write"**: specifies that we want to write on the serial port to the board + - *singleArray_Data*: is the array of singles we want to write on serial (eg: [1 2 3 4.5 5.4]). If we want to send a struct (or even nested structs) we can use the function `structToSingles` to turn the struct into an array +- `singleArray_Data = serialbridge("Read", uint_nData)` to receive data from the board; matlab will wait on the read till all the data is received: + - **"Read"**: specifies that we want to read from the serial port + - *uint_nData*: How many floats to read from serial (eg: 1) + - *singleArray_Data*: array of floats read from the serial (eg: alpha_degree) +- `serialbridge("Close")` closes the serial communication: + - **"Close"**: specifies we want to close the serial port + +When the simulator is ready and the board is flashed and connected you can execute the simulation as a normal project in matlab. + +### Example of usage of the hardware-in-the-loop framework in matlab: +``` +serialbridge("Open", "COM6", 256000); % Opens the serial port +serialbridge("Write", [1 2 3 4]); % Sends the array "[1 2 3 4]" to the serial device +data = serialbridge("Read", 2); % Receives 2 floats and stores them in the variable "data" +serialbridge("Close"); % Closes the serial port +``` + +### WARNING: +- It's possible to open just ONE serial port with serialbridge +- The files serialib.h and serialib.cpp were modified in order to fix compiling errors on windows +- the function `structToSingles` ignores the struct fields called *time* because the obsw forges it's own timestamps for every sample + +## FAQ +Some common problems found using the framework: +- For every problem you encounter, first time reboot your board. If it doesn't work clear and flash the board again. also you can try to disconnect and reconnect the serial adapter. +- If matlab gives back an error about something wrong with array indices check the data received from the OBSW, if they exceed the limit of that value (for example giving to the airbrake aperture a value of 2000 degrees) matlab returns this error +- If you have problems on linux (on the matlab side) check if you are opening the right port and at the right baudrate; check also if the baudrate you are using is supported by your operative system +- If you are reading malformed data check the baudrate on OBSW and matlab side. they have to be the same +- If you are not able to read data and matlab stays in the *busy* state (to restart matlab usage you must reboot matlab, *ctrl+c* or *ctrl+z* doesn't work cause you are waiting in the mex file and matlab can't communicate to it... thanks matlab): + - check if you connected the right pins + - check if the USART you are using is free from any other communication (use the cheatsheet, I'm sorry) + - try to use another USART (if you are lucky and it works please update the table with all the boards, USART and pins verified to work properly) + - check if it's a problem on the obsw (use a simpler obsw test in order to check communication) \ No newline at end of file diff --git a/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h b/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h new file mode 100644 index 0000000000000000000000000000000000000000..22fd5855f5194fc2df84b19bb9d05ef95490df37 --- /dev/null +++ b/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h @@ -0,0 +1,178 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "ActiveObject.h" +#include "Algorithm.h" +#include "SerialInterface.h" +#include "TimestampTimer.h" +#include "hardware_in_the_loop/HILFlightPhasesManager.h" +#include "hardware_in_the_loop/HIL_sensors/HILTimestampManagement.h" +#include "math/Vec3.h" + +using namespace miosix; + +/** + * @brief HILTranceiver is a Singleton and provides an easy interface for the + * control algorithms to send and receive data during a simulation + */ +class HILTransceiver : public ActiveObject +{ +public: + /** + * @brief Construct a serial connection attached to a control algorithm + */ + HILTransceiver(HILFlightPhasesManager *fpMgr) + : flightPhasesManager(fpMgr), actuatorData(0.0f) + { + serial = new SerialInterface(SIM_BAUDRATE, SIM_SERIAL_PORT_NUM); + + // initializing the serial connection + if (!serial->init()) + { + TRACE("[HIL] Wrong initialization\n"); + } + } + + /** + * @brief sets the actuator data and then wakes up the MatlabTransceiver + * thread in order to send the data back to the simulator (called by the + * control algorithm) + * @param actuatorData sets the data that will be sent to the simulator + */ + void setActuatorData(ActuatorData actuatorData) + { + this->actuatorData = actuatorData; + updated = true; + condVar.signal(); + } + + /** + * @brief returns the reference of the SimulatorData + * + * @return reference to the data simulated by matlab + */ + SimulatorData *getSensorData() { return &sensorData; } + + /** + * @brief adds to the resetSampleCounter list an object that has to be + * notified when a new packet of data is arrived from the simulator + * + * @param t SimTimestampManagement object + */ + void addResetSampleCounter(HILTimestampManagement *t) + { + sensorsTimestamp.push_back(t); + } + + /** + * @brief adds to the notifyToBegin list an object that has to be started + * when the first packet from the simulator arrives. + * + * @param algorithm Algorithm object to be started + */ + /*void setIsAirbrakePhase(bool isAirbrakePhase) + { + this->isAirbrakePhase = isAirbrakePhase; + if (!isAirbrakePhase) + { + actuatorData = 0; + } + }*/ + +private: + /** + * @brief the thread deals with the communication between the simulator and + * the mock sensors + * + * The first read is done in the init() function; + * + * After the first time the data is received the loop of this thread is: + * - reads the simulated data and copies them in the SensorData structure + * - notifies every sensor that new data arrived, + * - waits for the control algorithms to update the actuator data + * - sends back the value to the simulator + */ + void run() override + { + TRACE("[HIL] Transceiver started\n"); + + while (true) + { + /* Scope of the temporary data read from serial and pausing the + * kernel in order to copy the data in the shared structure */ + { + SimulatorData tempData; + serial->recvData<SimulatorData>(&tempData); + PauseKernelLock kLock; + memmove(&sensorData, &tempData, sizeof(SimulatorData)); + } + + if (!receivedFirstPacket) + { + receivedFirstPacket = true; + flightPhasesManager->setFlagFlightPhase(SIMULATION_STARTED, + true); + } + + /* + * notify all sensors that a new set of data is arrived + * [REVIEW] Could be moved in HILFlightPhasesManager + */ + for (auto st : sensorsTimestamp) + { + st->resetSampleCounter(); + } + + // trigger events relative to the flight phases + flightPhasesManager->processFlags(sensorData.flags); + + waitActuatorData(); + serial->sendData<ActuatorData>(&actuatorData); + } + } + + /** + * @brief Waits for the control algorithm(s) to update actuatorData. + */ + void waitActuatorData() + { + Lock<FastMutex> l(mutex); + while (!updated) + { + condVar.wait(l); + } + updated = false; + } + + SerialInterface *serial; + // bool isAirbrakePhase = false; + bool receivedFirstPacket = false; + bool updated = false; + HILFlightPhasesManager *flightPhasesManager; + SimulatorData sensorData; + ActuatorData actuatorData; + std::vector<HILTimestampManagement *> sensorsTimestamp; + FastMutex mutex; + ConditionVariable condVar; +}; \ No newline at end of file diff --git a/src/hardware_in_the_loop/simulator_communication/SerialInterface.h b/src/hardware_in_the_loop/simulator_communication/SerialInterface.h new file mode 100644 index 0000000000000000000000000000000000000000..37857e3d1856f058427c4e16c24a98daecd06be1 --- /dev/null +++ b/src/hardware_in_the_loop/simulator_communication/SerialInterface.h @@ -0,0 +1,146 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Common.h> +#include <fcntl.h> +#include <stdio.h> + +#include <string> + +#include "arch/common/drivers/serial.h" +#include "filesystem/file_access.h" +#include "miosix.h" + +using namespace std; +using namespace miosix; + +/** + * @brief Creates and opens a serial port on the board and provides templated + * "sendData" and "recvData" functions in order to send and receive any data + * structure needed + */ +class SerialInterface +{ +public: + /** + * Constructor of the serial communication with default parameters + * @param baudrate port baudrate (default: 256000) + * @param serialPortNum number from 1 to 3 in order to set the USART to use + * (default: 2 [tx_board=PA2 rx_board=PA3 cts=PA0 rts=PA1]) + * @param serialPortName name of the serial port to open (default: + * "simulation") + */ + SerialInterface(int baudrate = 256000, int serialPortNum = 2, + string serialPortName = "simulation") + { + this->baudrate = baudrate; + this->serialPortNum = serialPortNum; + this->serialPortName = serialPortName; + is_init = false; + fd = -1; + } + + /** + * @brief Initializes the object if it's the first call to this function + * @return true if initialization is successful, false otherwise + */ + bool init() + { + if (is_init) + { + TRACE("[HIL] Error : serial communication already initialized!\n"); + return false; + } + else if (!serialCommSetup()) + { + TRACE("[HIL] Error : can't initialize serial communication!\n"); + return false; + } + + is_init = true; + return true; + } + + /** + * @brief Receives the data from the simulated sensors and deserializes it + * in the templated format + * @param data reference of the struct for the data to receive + */ + template <typename T> + void recvData(T *data) + { + read(fd, data, sizeof(T)); + } + + /** + * @brief Sends the position of the simulated actuators as the templated + * type + * @param data reference of the struct of the data to send + */ + template <typename U> + void sendData(U *data) + { + write(fd, data, sizeof(U)); + } + +private: + /* Creates and opens the serial port for communication between OBSW and + * simulation device */ + bool serialCommSetup() + { + // Takes the file system pointer of the devices + intrusive_ref_ptr<DevFs> devFs = + FilesystemManager::instance().getDevFs(); + + // Creates and adds the serial port to the devices + if (!devFs->addDevice(serialPortName.c_str(), + intrusive_ref_ptr<Device>( + new STM32Serial(serialPortNum, baudrate)))) + return false; + + // path string "/dev/<name_of_port>" for the port we want to open + string serialPortPath = "/dev/" + serialPortName; + + /* open serial port named simulation */ + fd = open(serialPortPath.c_str(), O_RDWR); + + if (fd < -1) + { + TRACE("Cannot open %s\n", serialPortPath.c_str()); + return false; + } + + return true; + } + + string serialPortName; /**< Port name of the serial port that has to be + created for the communication */ + int fd; /**< Stores the file descriptor of the serial port file opened for + trasmission */ + int serialPortNum; /**< Stores the USART<serialPortNum> used for the + trasmission range[1:3] */ + int baudrate; /**< Baudrate of the serial port */ + bool is_init; /**< True if init() already called successfully, false + otherwise */ +}; \ No newline at end of file diff --git a/src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h b/src/mocksensors/MockGPS.h similarity index 65% rename from src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h rename to src/mocksensors/MockGPS.h index 1f5107b2d32223780743f424c72e98960e6e4d92..6d319fef8b9a6752502097b8ef5e42d96e108193 100644 --- a/src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h +++ b/src/mocksensors/MockGPS.h @@ -1,5 +1,5 @@ /* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,23 +13,33 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ +#pragma once + #include <tests/mock_sensors/test-mock-data.h> +#include "TimestampTimer.h" +#include "sensors/Sensor.h" + namespace DeathStackBoard { -class MockGPS + +class MockGPS : public Sensor<GPSData> { public: - MockGPS(){} + MockGPS() {} + + bool init() override { return true; } - bool updateCoordinates() + bool selfTest() override { return true; } + + GPSData sampleImpl() override { // if (inside_lha) // { @@ -41,26 +51,33 @@ public: // lat = lat_outside; // lon = lon_outside; // } + + GPSData data; + + data.gps_timestamp = TimestampTimer::getTimestamp(); + data.fix = true; + if (before_liftoff) { - lat = SIMULATED_LAT[0]; - lon = SIMULATED_LON[0]; + data.latitude = SIMULATED_LAT[0]; + data.longitude = SIMULATED_LON[0]; + data.velocity_north = SIMULATED_VNORD[0]; + data.velocity_east = SIMULATED_VEAST[0]; } else if (i < GPS_DATA_SIZE) { - lat = SIMULATED_LAT[i]; - lon = SIMULATED_LON[i]; + data.latitude = SIMULATED_LAT[i]; + data.longitude = SIMULATED_LON[i]; + data.velocity_north = SIMULATED_VNORD[i]; + data.velocity_east = SIMULATED_VEAST[i]; i++; } - return true; - } - volatile bool before_liftoff = true; + return data; + } - bool inside_lha = true; + void signalLiftoff() { before_liftoff = false; } - double lat, lon; - bool fix = true; private: // Set of coordinates inside the Launch Hazard Area // const double lat_inside = 41.807487124105; @@ -70,6 +87,9 @@ private: // const double lat_outside = 41.840794; // const double lon_outside = 14.003920; + volatile bool before_liftoff = true; + bool inside_lha = true; + unsigned int i = 0; }; } // namespace DeathStackBoard diff --git a/src/mocksensors/MockIMU.h b/src/mocksensors/MockIMU.h new file mode 100644 index 0000000000000000000000000000000000000000..7861fd452aeafcf9b82f24f3507b9227b7185311 --- /dev/null +++ b/src/mocksensors/MockIMU.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Luca Conterio, Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <tests/mock_sensors/test-mock-data.h> + +#include "TimestampTimer.h" +#include "sensors/Sensor.h" + +namespace DeathStackBoard +{ + +struct MockIMUData : public AccelerometerData, + public GyroscopeData, + public MagnetometerData +{ +}; + +class MockIMU : public Sensor<MockIMUData> +{ +public: + MockIMU() {} + + bool init() override { return true; } + + bool selfTest() override { return true; } + + MockIMUData sampleImpl() override + { + if (before_liftoff) + { + index = 0; + } + + MockIMUData data; + + data.accel_timestamp = TimestampTimer::getTimestamp(); + data.accel_x = ACCELEROMETER_DATA[0][index]; + data.accel_y = ACCELEROMETER_DATA[1][index]; + data.accel_z = ACCELEROMETER_DATA[2][index]; + + data.gyro_timestamp = TimestampTimer::getTimestamp(); + data.gyro_x = GYROSCOPE_DATA[0][index]; + data.gyro_y = GYROSCOPE_DATA[1][index]; + data.gyro_z = GYROSCOPE_DATA[2][index]; + + data.mag_timestamp = TimestampTimer::getTimestamp(); + data.mag_x = MAGNETOMETER_DATA[0][index]; + data.mag_y = MAGNETOMETER_DATA[1][index]; + data.mag_z = MAGNETOMETER_DATA[2][index]; + + // when finished, go back to the beginning + index = (index + 1) % IMU_DATA_SIZE; + + return data; + } + + void signalLiftoff() { before_liftoff = false; } + +private: + unsigned int index = 0; + volatile bool before_liftoff = true; +}; + +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h b/src/mocksensors/MockPressureSensor.h similarity index 68% rename from src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h rename to src/mocksensors/MockPressureSensor.h index 4b95eedc4fc29a76c7b090c6de271a652a26d2bc..8885cfcd000fac6bccfd213da8b88f7af79f3ce8 100644 --- a/src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h +++ b/src/mocksensors/MockPressureSensor.h @@ -1,5 +1,5 @@ /* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli + * Author: Luca Mozzarelli * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,47 +13,61 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ +#pragma once + #include <Common.h> #include <sensors/Sensor.h> -// #include <tests/catch/ada/test-ada-data.h> #include <tests/mock_sensors/test-mock-data.h> + #include <random> namespace DeathStackBoard { -class MockPressureSensor + +class MockPressureSensor : public Sensor<PressureData> { public: - float getPressure() + MockPressureSensor() {} + + bool init() override { return true; } + + bool selfTest() override { return true; } + + PressureData sampleImpl() override { + float press = 0.0; + if (before_liftoff) { - return addNoise(SIMULATED_PRESSURE[0]); + press = addNoise(SIMULATED_PRESSURE[0]); } else { if (i < PRESSURE_DATA_SIZE) { - return addNoise(SIMULATED_PRESSURE[i++]); + press = addNoise(SIMULATED_PRESSURE[i++]); } else { - return addNoise(SIMULATED_PRESSURE[PRESSURE_DATA_SIZE - 1]); + press = addNoise(SIMULATED_PRESSURE[PRESSURE_DATA_SIZE - 1]); } } + + return PressureData{TimestampTimer::getTimestamp(), press}; } - volatile bool before_liftoff = true; + void signalLiftoff() { before_liftoff = false; } private: - volatile unsigned int i = 0; // Last index + volatile bool before_liftoff = true; + volatile unsigned int i = 0; // Last index std::default_random_engine generator{1234567}; std::normal_distribution<float> distribution{0.0f, 5.0f}; @@ -62,6 +76,7 @@ private: return quantization(sample + distribution(generator)); } - float quantization(float sample) { return round(sample / 30) * 30; } + float quantization(float sample) { return round(sample / 30.0) * 30.0; } }; + } // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h b/src/mocksensors/MockSensors.h similarity index 74% rename from src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h rename to src/mocksensors/MockSensors.h index 5d32885fed2b2e649097c4976af76b33333195a0..989123f1c3a1dd34eede930c737446d9923f704d 100644 --- a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h +++ b/src/mocksensors/MockSensors.h @@ -1,22 +1,19 @@ -/** - * elevation_map.h - * - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -25,11 +22,8 @@ #pragma once -#include "generated/elevation_map_data.h" - -namespace elevationmap -{ - -int getElevation(double lat, double lon); - -} // namespace elevationmap +#include "MockGPS.h" +#include "MockIMU.h" +#include "MockPressureSensor.h" +#include "tests/mock_sensors/test-mock-data.h" +#include "utils/testutils/TestSensor.h" \ No newline at end of file diff --git a/src/tests/airbrakes/test-airbrakes-algorithm.cpp b/src/tests/airbrakes/test-airbrakes-algorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b72741ae3b4c0ba502928ff3e816e752f4945aed --- /dev/null +++ b/src/tests/airbrakes/test-airbrakes-algorithm.cpp @@ -0,0 +1,165 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <miosix.h> + +#include <cmath> +#include <cstdio> +#include <iostream> +#include <sstream> +#include <string> + +#define private public + +#include <AirBrakes/AirBrakesAlgorithm.h> +#include <AirBrakes/AirBrakesServo.h> + +#include "drivers/HardwareTimer.h" +#include "sensors/Sensor.h" +#include "test_data.h" + +using namespace DeathStackBoard; +using namespace std; +class MockActuator : public AirBrakesServo +{ +public: + MockActuator() {} + // void setPosition(float alpha) { lastAlpha = round(alpha * 180 / PI); } + float error(float alphaTest) + { + int error = abs(alphaTest - currentPosition); + printf("Error: %d\n", error); + return error; + } + + // private: + // int lastAlpha; +}; + +template <typename T> +class MockSensor : public Sensor<T> +{ +private: + int ts = 0; + +protected: + T sampleImpl() override + { + input_t input = DATA[ts].input; + T sampled; + + sampled.z = input.z; + sampled.vz = input.vz; + sampled.vMod = input.vMod; + sampled.timestamp = ts; + + ts++; + + return sampled; + } + + bool init() override { return true; } + + bool selfTest() override { return true; } +}; + +class InputClass +{ +public: + float z; + float vz; + float vMod; + uint64_t timestamp; +}; + +MockActuator mockActuator; +MockSensor<InputClass> mockSensor; +AirBrakesControlAlgorithm<InputClass> algorithm(mockSensor, &mockActuator); + +int main() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable TIM5 peripheral clocks + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + } + + input_t input; + + input = DATA[0].input; + + HardwareTimer<uint32_t> hrclock( + TIM5, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)); + hrclock.setPrescaler(127); + hrclock.start(); + + mockSensor.sample(); + + uint32_t t1 = hrclock.tick(); + algorithm.begin(); + uint32_t t2 = hrclock.tick(); + + cout << "z: " << mockSensor.getLastSample().z + << "\tvz: " << mockSensor.getLastSample().vz + << "\tvMod: " << mockSensor.getLastSample().vMod + << "\tservo: " << mockActuator.getCurrentPosition() << "\n"; + + float startTime = hrclock.toMilliSeconds(t2 - t1); + float stepTime = 0; + int maxAlphaError = 0; + + for (int i = 1; i < LEN_TEST; i++) + { + mockSensor.sample(); + t1 = hrclock.tick(); + algorithm.update(); + t2 = hrclock.tick(); + + int error = mockActuator.error(DATA[i].output.alpha); + + if (error > maxAlphaError) + { + maxAlphaError = error; + } + + stepTime += hrclock.toMilliSeconds(t2 - t1); + + cout << "z: " << mockSensor.getLastSample().z + << "\tvz: " << mockSensor.getLastSample().vz + << "\tvMod: " << mockSensor.getLastSample().vMod + << "\tservo: " << mockActuator.getCurrentPosition() << "\n"; + } + + miosix::Thread::sleep(AirBrakesConfigs::ABK_UPDATE_PERIOD); + + printf( + "Init time: %f ms\nAvg iter time: %f ms\nTotal computation time: %f " + "ms\n", + startTime, stepTime / (LEN_TEST - 1), stepTime + startTime); + + // avoid reboot + while (1) + { + } +} \ No newline at end of file diff --git a/src/tests/airbrakes/test-airbrakes-interactive.cpp b/src/tests/airbrakes/test-airbrakes-interactive.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f15d6d74f51c2ab74d89a7feeeca54e4725236fe --- /dev/null +++ b/src/tests/airbrakes/test-airbrakes-interactive.cpp @@ -0,0 +1,374 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Alberto Nidasio, Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <AirBrakes/AirBrakesController.h> +#include <AirBrakes/AirBrakesServo.h> +#include <miosix.h> + +#include <cstdio> +#include <iostream> +#include <sstream> +#include <string> + +#include "events/Events.h" +#include "test_data.h" +#include "utils/testutils/TestHelper.h" + +using namespace DeathStackBoard; + +using namespace std; + +void testAlgorithm(); +void testAirBrakes(); +void wiggleServo(); +void setServoFullyOpen(); +void setServoFullyClosed(); +void resetServo(); +void manualServoControl(); +void setServoParameters(); +void resetServoParameters(); +void waitUserInput(); + +class InputClass +{ +public: + float z; + float vz; + float vMod; + uint64_t timestamp; +}; + +template <typename T> +class MockSensor : public Sensor<T> +{ +private: + int ts = 0; + +protected: + T sampleImpl() override + { + input_t input = DATA[ts].input; + T sampled; + + sampled.z = input.z; + sampled.vz = input.vz; + sampled.vMod = input.vMod; + sampled.timestamp = ts; + + ts++; + + return sampled; + } + + bool init() override { return true; } + + bool selfTest() override { return true; } +}; + +float minPosition = AirBrakesConfigs::AB_SERVO_MIN_POS; +float maxPosition = AirBrakesConfigs::AB_SERVO_MAX_POS; +float resetPosition = AirBrakesConfigs::AB_SERVO_MIN_POS; + +int main() +{ + sEventBroker->start(); + + miosix::GpioPin pwmPin = miosix::GpioPin(GPIOC_BASE, 7); + pwmPin.mode(miosix::Mode::ALTERNATE); + pwmPin.alternateFunction(3); + + string temp; + for (;;) + { + int choice; + cout << "\n\nWhat do you want to do?:\n"; + cout << "1. Airbrakes algorithm simulation (with test data)\n"; + cout << "2. Airbrakes full extension\n"; + cout << "3. Servo wiggle\n"; + cout << "4. Servo fully open\n"; + cout << "5. Servo fully closed\n"; + cout << "6. Servo reset\n"; + cout << "7. Servo manual control\n"; + cout << "8. Set servo parameters\n"; + cout << "9. Reset servo parameters (default)\n"; + + getline(cin, temp); + stringstream(temp) >> choice; + + switch (choice) + { + case 1: + testAlgorithm(); + break; + case 2: + testAirBrakes(); + break; + case 3: + wiggleServo(); + break; + case 4: + setServoFullyOpen(); + break; + case 5: + setServoFullyClosed(); + break; + case 6: + resetServo(); + break; + case 7: + manualServoControl(); + break; + case 8: + setServoParameters(); + break; + case 9: + resetServoParameters(); + break; + default: + cout << "Invalid option \n"; + break; + } + } +} + +void testAlgorithm() +{ + cout << "\n\n** AIRBRAKES ALGORITHM SIMULATION **\n\n"; + + waitUserInput(); + + // AirBrakesController initialization + MockSensor<InputClass> sensor; + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + AirBrakesController<InputClass> airbrakesController(sensor, &servo); + + // Start the state machine + airbrakesController.start(); + sensor.sample(); + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_FLIGHT_EVENTS); + counter.subscribe(TOPIC_ABK); + + // Start test + cout << "Starting airbrakes test\n"; + sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + while (counter.getCount(EV_SHADOW_MODE_TIMEOUT) < 1) + { + Thread::sleep(10); + } + Thread::sleep(100); + + for (int i = 1; i < LEN_TEST; i++) + { + sensor.sample(); + airbrakesController.update(); + cout << "z: " << sensor.getLastSample().z + << " - vz: " << sensor.getLastSample().vz + << " - vMod: " << sensor.getLastSample().vMod + << " - servo: " << servo.getCurrentPosition() + << " - cd: " << airbrakesController.getEstimatedCd() << "\n"; + Thread::sleep(AirBrakesConfigs::ABK_UPDATE_PERIOD); + } + + airbrakesController.stop(); + counter.stop(); + + cout << "\n\tTest finished!\n"; +} + +void testAirBrakes() +{ + cout << "\n\n** AIRBRAKES FULL EXTENSION **\n\n"; + + waitUserInput(); + + // AirBrakesController initialization + MockSensor<InputClass> sensor{}; + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + AirBrakesController<InputClass> airbrakesController{sensor, &servo}; + + // Start the state machine + airbrakesController.start(); + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_ABK); + + // Start test + cout << "Starting airbrakes test\n"; + sEventBroker->post({EV_TEST_ABK}, TOPIC_ABK); + + while (counter.getCount(EV_TEST_TIMEOUT) < 1) + { + Thread::sleep(100); + } + + airbrakesController.stop(); + counter.stop(); + + cout << "\n\tTest finished!\n"; +} + +void wiggleServo() +{ + string temp; + cout << "\n\n** WIGGLE SERVO **\n\n"; + + waitUserInput(); + + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.reset(); + + cout << "Wiggling ...\n"; + servo.selfTest(); + Thread::sleep(1000); + servo.disable(); + cout << "\n\tDone!\n"; +} + +void setServoFullyOpen() +{ + string temp; + cout << "\n\n** SERVO FULLY OPEN **\n\n"; + + waitUserInput(); + + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.setMaxPosition(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tDone!\n"; +} + +void setServoFullyClosed() +{ + string temp; + cout << "\n\n** SERVO FULLY CLOSED **\n\n"; + + waitUserInput(); + + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.setMinPosition(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tDone!\n"; +} + +void resetServo() +{ + string temp; + cout << "\n\n** RESET SERVO **\n\n"; + + waitUserInput(); + + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.reset(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tDone!\n"; +} + +void manualServoControl() +{ + cout << "\n\n** MANUAL SERVO CONTROL **\n\n"; + + string temp; + float angle; + do + { + cout << "Write the servo postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> angle; + } while (angle < minPosition || angle > maxPosition); + + AirBrakesServo servo{minPosition, maxPosition, resetPosition}; + + servo.enable(); + while (servo.getCurrentPosition() != angle) + { + servo.set(angle); + Thread::sleep(AirBrakesConfigs::ABK_UPDATE_PERIOD); + } + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tDone!\n"; +} + +void setServoParameters() +{ + cout << "\n\n** SET SERVO PARAMETERS **\n\n"; + + string temp; + do + { + cout << "Write the servo minimum postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> minPosition; + } while (minPosition < 0 || minPosition > 180.0f); + + do + { + cout << "Write the servo maximum postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> maxPosition; + } while (maxPosition < 0 || maxPosition > 180.0f); + + do + { + cout << "Write the servo reset postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> resetPosition; + } while (resetPosition < 0 || resetPosition > 180.0f); + + cout << "Configured parameteres:\n"; + cout << "\tminimum position: " << minPosition << "\n"; + cout << "\tmaximum position: " << maxPosition << "\n"; + cout << "\treset position: " << resetPosition << "\n"; +} + +void resetServoParameters() +{ + minPosition = AirBrakesConfigs::AB_SERVO_MIN_POS; + maxPosition = AirBrakesConfigs::AB_SERVO_MAX_POS; + resetPosition = AirBrakesConfigs::AB_SERVO_MIN_POS; + + cout << "Configured parameteres (default):\n"; + cout << "\tminimum position: " << minPosition << "\n"; + cout << "\tmaximum position: " << maxPosition << "\n"; + cout << "\treset position: " << resetPosition << "\n"; +} + +void waitUserInput() +{ + string temp; + do + { + cout << "Write 'start' to begin the test:\n"; + getline(cin, temp); + } while (temp != "start"); +} \ No newline at end of file diff --git a/src/tests/airbrakes/test_data.h b/src/tests/airbrakes/test_data.h new file mode 100644 index 0000000000000000000000000000000000000000..16df8d0d7d35cf46249adc2af65d408f0839c45f --- /dev/null +++ b/src/tests/airbrakes/test_data.h @@ -0,0 +1,202 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +static const int LEN_TEST = 153; + +struct input_t +{ + float z; + float vz; + float vMod; +}; + +struct output_t +{ + float z_setpoint; + float vz_setpoint; + int alpha; +}; + +struct test_t +{ + input_t input; + output_t output; +}; + +const test_t DATA[LEN_TEST] = { + {{1476.919744, 227.661234, 237.481598}, {1477.524202, 225.093921, 0}}, + {{1499.611517, 225.991007, 235.803693}, {1499.893098, 222.291839, 7}}, + {{1522.124236, 224.043613, 233.861447}, {1521.984860, 219.550841, 16}}, + {{1544.424910, 221.706689, 231.518296}, {1543.805442, 216.867876, 20}}, + {{1566.483904, 219.196698, 228.970551}, {1565.360505, 214.240107, 25}}, + {{1588.286547, 216.584829, 226.308174}, {1586.655434, 211.664893, 31}}, + {{1609.823077, 213.845639, 223.514207}, {1607.695362, 209.139770, 34}}, + {{1631.082504, 211.037728, 220.640346}, {1628.485180, 206.662439, 37}}, + {{1652.060323, 208.212866, 217.742637}, {1649.029561, 204.230749, 37}}, + {{1672.758071, 205.443626, 214.895594}, {1669.332966, 201.842688, 37}}, + {{1693.181374, 202.730057, 212.106865}, {1689.399664, 199.496372, 42}}, + {{1713.333908, 200.038012, 209.343597}, {1709.233740, 197.190032, 37}}, + {{1733.224618, 197.505007, 206.734062}, {1728.839107, 194.922008, 42}}, + {{1752.860313, 194.928643, 204.098660}, {1748.219519, 192.690738, 42}}, + {{1772.240360, 192.399323, 201.502227}, {1776.876186, 189.409557, 42}}, + {{1791.369628, 189.918132, 198.956200}, {1795.709656, 187.263932, 42}}, + {{1810.252868, 187.483745, 196.459153}, {1814.330171, 185.150281, 42}}, + {{1828.895140, 185.115507, 194.029409}, {1832.740867, 183.067406, 42}}, + {{1847.303803, 182.808258, 191.664567}, {1850.944764, 181.014179, 42}}, + {{1865.483558, 180.541544, 189.343897}, {1868.944774, 178.989529, 47}}, + {{1883.436259, 178.265014, 187.017591}, {1886.743703, 176.992444, 47}}, + {{1901.162989, 176.027487, 184.726412}, {1904.344260, 175.021966, 47}}, + {{1918.667719, 173.829195, 182.476347}, {1921.749060, 173.077184, 47}}, + {{1935.954308, 171.668762, 180.265982}, {1938.960629, 171.157236, 47}}, + {{1953.026695, 169.556717, 178.105244}, {1955.981408, 169.261300, 42}}, + {{1969.892237, 167.535963, 176.036400}, {1972.813760, 167.388599, 42}}, + {{1986.557245, 165.548477, 174.009020}, {1989.459971, 165.538390, 42}}, + {{2003.024880, 163.591898, 172.014067}, {2005.922255, 163.709966, 42}}, + {{2019.298208, 161.665414, 170.050694}, {2022.202756, 161.902656, 42}}, + {{2035.380192, 159.768248, 168.118092}, {2038.303554, 160.115817, 42}}, + {{2051.273739, 157.899723, 166.215548}, {2054.226664, 158.348836, 42}}, + {{2066.981667, 156.058799, 164.342006}, {2069.974044, 156.601130, 42}}, + {{2082.506730, 154.248230, 162.500016}, {2085.547592, 154.872138, 37}}, + {{2097.854705, 152.524817, 160.743928}, {2100.949153, 153.161327, 42}}, + {{2113.029474, 150.779544, 158.977494}, {2116.180520, 151.468184, 37}}, + {{2128.032661, 149.102631, 157.272102}, {2131.243434, 149.792220, 37}}, + {{2142.869254, 147.449022, 155.597295}, {2146.139590, 148.132964, 42}}, + {{2157.539574, 145.774056, 153.904527}, {2160.870637, 146.489965, 42}}, + {{2172.043305, 144.120512, 152.228433}, {2175.438177, 144.862792, 37}}, + {{2186.384445, 142.529927, 150.614286}, {2189.843773, 143.251029, 42}}, + {{2200.565807, 140.921126, 148.990906}, {2204.088946, 141.654277, 42}}, + {{2214.587137, 139.332351, 147.383014}, {2211.151838, 140.861409, 18}}, + {{2228.460605, 137.997251, 146.017108}, {2225.159142, 139.286457, 23}}, + {{2242.199592, 136.631669, 144.658159}, {2239.009657, 137.725582, 25}}, + {{2255.801635, 135.258500, 143.286187}, {2252.704773, 136.178435, 31}}, + {{2269.264653, 133.846198, 141.875936}, {2266.245846, 134.644680, 31}}, + {{2282.587000, 132.447932, 140.472672}, {2279.634199, 133.123991, 34}}, + {{2295.768818, 131.033137, 139.055589}, {2292.871123, 131.616055, 37}}, + {{2308.808947, 129.614030, 137.631334}, {2305.957877, 130.120570, 42}}, + {{2321.706517, 128.180216, 136.192035}, {2318.895692, 128.637242, 47}}, + {{2334.460496, 126.741962, 134.745625}, {2331.685771, 127.165789, 47}}, + {{2347.071384, 125.320791, 133.313972}, {2344.329285, 125.705937, 47}}, + {{2359.540933, 123.917076, 131.900803}, {2356.827383, 124.257422, 47}}, + {{2371.870911, 122.532933, 130.508066}, {2369.181185, 122.819986, 47}}, + {{2384.063455, 121.169173, 129.137048}, {2381.391786, 121.393382, 47}}, + {{2396.120351, 119.821636, 127.783700}, {2393.460258, 119.977367, 47}}, + {{2408.043202, 118.490093, 126.447308}, {2405.387648, 118.571709, 47}}, + {{2419.833600, 117.174202, 125.127524}, {2417.174980, 117.176180, 47}}, + {{2431.493089, 115.873506, 123.823868}, {2428.823256, 115.790559, 47}}, + {{2443.023168, 114.587765, 122.536085}, {2440.333455, 114.414633, 47}}, + {{2454.425336, 113.316794, 121.263998}, {2451.706538, 113.048192, 47}}, + {{2465.701065, 112.060635, 120.007620}, {2462.943442, 111.691036, 47}}, + {{2476.851826, 110.819059, 118.766717}, {2479.545467, 109.672277, 47}}, + {{2487.879076, 109.591889, 117.541105}, {2490.445900, 108.337477, 47}}, + {{2498.784216, 108.378379, 116.330073}, {2501.213286, 107.011294, 47}}, + {{2509.568649, 107.180215, 115.135168}, {2511.848475, 105.693546, 47}}, + {{2520.233980, 105.997142, 113.956499}, {2522.352304, 104.384059, 47}}, + {{2530.781586, 104.827191, 112.792035}, {2532.725590, 103.082661, 47}}, + {{2541.212773, 103.670184, 111.641381}, {2542.969134, 101.789185, 47}}, + {{2551.528835, 102.526054, 110.504453}, {2553.083718, 100.503469, 47}}, + {{2561.731051, 101.394638, 109.381080}, {2563.070113, 99.225354, 47}}, + {{2571.820687, 100.275905, 108.271212}, {2572.929068, 97.954683, 47}}, + {{2581.799011, 99.169689, 107.174677}, {2582.661323, 96.691305, 47}}, + {{2591.667262, 98.075732, 106.091218}, {2592.267597, 95.435071, 47}}, + {{2601.426644, 96.993572, 105.020417}, {2601.748599, 94.185836, 47}}, + {{2611.078326, 95.923005, 103.962069}, {2611.105022, 92.943457, 47}}, + {{2620.623468, 94.864078, 102.916190}, {2620.337543, 91.707796, 47}}, + {{2630.063231, 93.816695, 101.882658}, {2629.446828, 90.478716, 47}}, + {{2639.398778, 92.781107, 100.861679}, {2638.433528, 89.256083, 47}}, + {{2648.631364, 91.759096, 99.854888}, {2647.298281, 88.039768, 47}}, + {{2657.762287, 90.749088, 98.861187}, {2656.041713, 86.829641, 47}}, + {{2666.792738, 89.750941, 97.880047}, {2664.664437, 85.625578, 47}}, + {{2675.723890, 88.764378, 96.911207}, {2677.373503, 83.830583, 47}}, + {{2684.556894, 87.789249, 95.954520}, {2685.697053, 82.641144, 47}}, + {{2693.292887, 86.825310, 95.009744}, {2693.901943, 81.457348, 47}}, + {{2701.932973, 85.872327, 94.076650}, {2701.988730, 80.279079, 47}}, + {{2710.478255, 84.930573, 93.155466}, {2709.957962, 79.106226, 47}}, + {{2718.929851, 83.999718, 92.245867}, {2717.810174, 77.938676, 47}}, + {{2727.288851, 83.079948, 91.348002}, {2725.545892, 76.776320, 47}}, + {{2735.556376, 82.171467, 90.462034}, {2736.932163, 75.042295, 47}}, + {{2743.733553, 81.274200, 89.587876}, {2744.378870, 73.892463, 47}}, + {{2751.821505, 80.388253, 88.725607}, {2751.710836, 72.747460, 47}}, + {{2759.821363, 79.513656, 87.875236}, {2758.928539, 71.607186, 47}}, + {{2767.734266, 78.650322, 87.036667}, {2766.032447, 70.471541, 47}}, + {{2775.561341, 77.798472, 86.210081}, {2776.475942, 68.776543, 47}}, + {{2783.303745, 76.958077, 85.395494}, {2783.297345, 67.652051, 47}}, + {{2790.962609, 76.128962, 84.592727}, {2790.006513, 66.531856, 47}}, + {{2798.539064, 75.311053, 83.801630}, {2799.860756, 64.859421, 47}}, + {{2806.034223, 74.504368, 83.022192}, {2806.291180, 63.749571, 47}}, + {{2813.449217, 73.709026, 82.254497}, {2812.610819, 62.643704, 47}}, + {{2820.785182, 72.925059, 81.498548}, {2821.883415, 60.992183, 47}}, + {{2828.043258, 72.152491, 80.754339}, {2827.927796, 59.895899, 47}}, + {{2835.224595, 71.391639, 80.022127}, {2833.862733, 58.803302, 47}}, + {{2842.330363, 70.642388, 79.301781}, {2842.560728, 57.171140, 47}}, + {{2849.361726, 69.904847, 78.593371}, {2848.223633, 56.087406, 47}}, + {{2856.319851, 69.178811, 77.896668}, {2856.515218, 54.468166, 47}}, + {{2863.205877, 68.464245, 77.211616}, {2861.908246, 53.392803, 47}}, + {{2870.020975, 67.761568, 76.538559}, {2869.796559, 51.785766, 47}}, + {{2876.766336, 67.070971, 75.877632}, {2877.444344, 50.185714, 47}}, + {{2883.443189, 66.392787, 75.229093}, {2882.409750, 49.122774, 47}}, + {{2890.052785, 65.727272, 74.593138}, {2889.658923, 47.533827, 47}}, + {{2896.596398, 65.074554, 73.969843}, {2896.670233, 45.951231, 47}}, + {{2903.075312, 64.434784, 73.359308}, {2903.444615, 44.374755, 47}}, + {{2909.490825, 63.808020, 72.761550}, {2909.982971, 42.804176, 47}}, + {{2915.844240, 63.194294, 72.176558}, {2916.286168, 41.239278, 47}}, + {{2922.136863, 62.593651, 71.604335}, {2922.355043, 39.679847, 47}}, + {{2928.370015, 62.006143, 71.044886}, {2928.190400, 38.125677, 47}}, + {{2934.544986, 61.431720, 70.498126}, {2933.793013, 36.576565, 47}}, + {{2940.663101, 60.870369, 69.963999}, {2940.902399, 34.518613, 47}}, + {{2946.725663, 60.322181, 69.442539}, {2945.964786, 32.980549, 47}}, + {{2952.733995, 59.787173, 68.933715}, {2952.356386, 30.936647, 47}}, + {{2958.689421, 59.265601, 68.437703}, {2958.339957, 28.900189, 47}}, + {{2964.593285, 58.757435, 67.954427}, {2965.247826, 26.364468, 47}}, + {{2970.446935, 58.262832, 67.483975}, {2970.318504, 24.343303, 47}}, + {{2976.251733, 57.781954, 67.026439}, {2976.089420, 21.825516, 47}}, + {{2982.009053, 57.314879, 66.581839}, {2982.185347, 18.816013, 47}}, + {{2987.720285, 56.861770, 66.150265}, {2987.380236, 15.818434, 47}}, + {{2993.386824, 56.422499, 65.731562}, {2993.490561, 11.342404, 47}}, + {{2999.010034, 55.996487, 65.325199}, {2999.005544, 4.422944, 47}}, + {{3004.591225, 55.583640, 64.931072}, {3000.000000, 0.000000, 47}}, + {{3010.131737, 55.184535, 64.549579}, {3000.000000, 0.000000, 47}}, + {{3015.632951, 54.799137, 64.180643}, {3000.000000, 0.000000, 47}}, + {{3021.096243, 54.427742, 63.824472}, {3000.000000, 0.000000, 47}}, + {{3026.523022, 54.070479, 63.481133}, {3000.000000, 0.000000, 47}}, + {{3031.914697, 53.727203, 63.150462}, {3000.000000, 0.000000, 47}}, + {{3037.272672, 53.397992, 62.832485}, {3000.000000, 0.000000, 47}}, + {{3042.598343, 53.082623, 62.526974}, {3000.000000, 0.000000, 47}}, + {{3047.893077, 52.780683, 62.233541}, {3000.000000, 0.000000, 47}}, + {{3053.158198, 52.491706, 61.951753}, {3000.000000, 0.000000, 47}}, + {{3058.394977, 52.215147, 61.681112}, {3000.000000, 0.000000, 47}}, + {{3063.604629, 51.950342, 61.421022}, {3000.000000, 0.000000, 47}}, + {{3068.788298, 51.696594, 61.170857}, {3000.000000, 0.000000, 47}}, + {{3073.947064, 51.453372, 60.930135}, {3000.000000, 0.000000, 47}}, + {{3079.081940, 51.219723, 60.698017}, {3000.000000, 0.000000, 47}}, + {{3084.193853, 50.994958, 60.473889}, {3000.000000, 0.000000, 47}}, + {{3089.283652, 50.778258, 60.256915}, {3000.000000, 0.000000, 47}}, + {{3094.352109, 50.568747, 60.046347}, {3000.000000, 0.000000, 47}}, + {{3099.399910, 50.365716, 59.841651}, {3000.000000, 0.000000, 47}}, + {{3104.427659, 50.168278, 59.642050}, {3000.000000, 0.000000, 47}}, + {{3109.435873, 49.975287, 59.446551}, {3000.000000, 0.000000, 47}}, + {{3114.424970, 49.786171, 59.254645}, {3000.000000, 0.000000, 47}}, + {{3119.395287, 49.599982, 59.065508}, {3000.000000, 0.000000, 47}}, + {{3124.347085, 49.415770, 58.878314}, {3000.000000, 0.000000, 47}}, + +}; diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp index 940039cbb98f0f341d0e0fa80c36a39c46e05505..e02a89abcd248eaaca2b530046bad082a9c4bc04 100644 --- a/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp +++ b/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp @@ -1,323 +1,2659 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli * - * Permission is hereby grantedf, free of chargef, to any person obtaining a copy - * of this software and associated documentation files (the "Software")f, to deal - * in the Software without restrictionf, including without limitation the rights - * to usef, copyf, modifyf, mergef, publishf, distributef, sublicensef, and/or sell - * copies of the Softwaref, and to permit persons to whom the Software is - * furnished to do sof, subject to the following conditions: + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KINDf, EXPRESS OR - * IMPLIEDf, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIMf, DAMAGES OR OTHER - * LIABILITYf, WHETHER IN AN ACTION OF CONTRACTf, TORT OR OTHERWISEf, ARISING FROM, + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #include "test-ada-data.h" -const float SIMULATED_PRESSURE[DATA_SIZE] = { - 85431.242948f, 85431.195530f, 85430.531818f, 85428.511661f, 85424.541375f, 85418.586756f, 85410.484742f, 85400.380605f, 85387.951291f, 85373.412847f, 85356.530561f, 85337.302634f, 85315.693843f, 85291.654118f, 85265.156486f, 85236.179384f, 85204.699993f, 85170.715288f, 85134.214699f, 85095.204539f, 85053.713878f, 85009.692280f, 84963.342580f, 84914.537170f, 84863.342981f, 84809.942661f, 84754.307845f, 84696.606779f, 84636.840947f, 84575.108256f, 84511.411803f, 84445.886248f, 84378.531812f, 84309.408539f, 84238.575457f, 84166.083903f, 84091.993574f, 84016.326350f, 83939.283495f, 83860.838930f, 83781.113349f, 83700.192751f, 83618.160747f, 83535.097047f, 83451.069870f, 83366.139284f, 83280.348743f, 83193.731035f, 83106.324146f, 83018.168763f, -82929.332706f, 82839.907721f, 82749.980821f, 82659.659037f, 82569.047818f, 82478.250367f, 82387.372687f, 82296.514121f, 82205.765806f, 82115.204232f, 82024.866544f, 81934.773692f, 81844.948233f, 81755.416882f, 81666.211710f, 81577.355814f, 81488.856279f, 81400.744909f, 81313.024685f, 81225.725720f, 81138.858951f, 81052.432671f, 80966.442916f, 80880.879969f, 80795.733506f, 80710.993461f, 80626.652792f, 80542.709300f, 80459.154952f, 80376.001162f, 80293.249194f, 80210.902104f, 80128.967362f, 80047.445669f, 79966.333243f, 79885.651990f, 79805.357504f, 79725.473811f, 79645.974136f, 79566.865375f, 79488.136895f, 79409.786770f, 79331.813688f, 79254.217678f, 79177.018164f, 79100.171172f, 79023.724092f, 78947.664215f, 78871.990160f, 78796.711133f, -78721.810653f, 78647.294473f, 78573.155658f, 78499.389033f, 78425.989137f, 78352.953270f, 78280.274531f, 78207.954674f, 78135.994226f, 78064.391522f, 77993.148588f, 77922.267607f, 77851.747240f, 77781.592871f, 77711.801941f, 77642.368219f, 77573.321504f, 77504.601044f, 77436.249004f, 77368.249493f, 77300.601718f, 77233.292329f, 77166.332527f, 77099.709056f, 77033.429981f, 76967.488574f, 76901.887521f, 76836.627276f, 76771.707698f, 76707.131456f, 76642.893513f, 76578.999459f, 76515.448480f, 76452.236220f, 76389.362923f, 76326.825701f, 76264.622110f, 76202.749523f, 76141.205158f, 76079.986697f, 76019.092066f, 75958.519777f, 75898.268885f, 75838.338743f, 75778.729023f, 75719.439688f, 75660.470760f, 75601.822193f, 75543.497084f, 75485.490333f, -75427.807225f, 75370.444249f, 75313.396464f, 75256.688784f, 75200.282025f, 75144.172875f, 75088.392160f, 75032.925014f, 74977.764900f, 74922.913295f, 74868.368873f, 74814.122531f, 74760.186468f, 74706.552169f, 74653.219692f, 74600.189078f, 74547.460475f, 74495.032604f, 74442.907385f, 74391.081048f, 74339.553306f, 74288.330025f, 74237.404938f, 74186.779260f, 74136.451930f, 74086.421993f, 74036.688644f, 73987.249596f, 73938.088136f, 73889.284787f, 73840.731349f, 73792.440376f, 73744.447246f, 73696.757821f, 73649.353837f, 73602.227122f, 73555.394377f, 73508.847207f, 73462.582908f, 73416.597304f, 73370.890105f, 73325.473076f, 73280.337506f, 73235.480219f, 73190.905435f, 73146.613374f, 73102.596175f, 73058.860540f, 73015.406133f, 72972.227565f, -72929.331987f, 72886.711212f, 72844.367669f, 72802.299644f, 72760.507265f, 72718.988304f, 72677.743118f, 72636.769630f, 72596.068004f, 72555.636476f, 72515.476024f, 72475.578873f, 72435.952273f, 72396.594244f, 72357.501959f, 72318.674868f, 72280.113029f, 72241.815580f, 72203.782733f, 72166.013100f, 72128.489391f, 72091.283419f, 72054.315012f, 72017.574437f, 71981.104889f, 71944.909063f, 71908.971195f, 71873.295478f, 71837.881367f, 71802.722807f, 71767.823448f, 71733.185331f, 71698.801255f, 71664.673274f, 71630.804807f, 71597.189691f, 71563.827456f, 71530.721966f, 71497.870293f, 71465.268268f, 71432.919826f, 71400.825373f, 71368.977961f, 71337.381166f, 71306.038072f, 71274.941011f, 71244.091733f, 71213.494853f, 71183.144392f, 71153.038987f, -71123.184376f, 71093.579791f, 71064.218686f, 71035.105353f, 71006.241439f, 70977.618605f, 70949.241026f, 70921.113901f, 70893.229961f, 70865.588676f, 70838.193911f, 70811.040889f, 70784.126431f, 70757.456377f, 70731.031335f, 70704.843240f, 70678.896801f, 70653.195521f, 70627.730598f, 70602.504219f, 70577.521085f, 70552.773924f, 70528.261970f, 70503.991199f, 70479.959315f, 70456.159982f, 70432.598952f, 70409.278082f, 70386.187944f, 70363.333297f, 70340.714922f, 70318.292187f, 70296.120757f, 70274.282314f, 70252.696300f, 70231.274495f, 70210.039972f, 70189.015806f, 70168.225068f, 70147.690833f, 70127.426153f, 70107.387697f, 70087.570081f, 70067.976772f, 70048.611234f, 70029.476936f, 70010.577343f, 69991.908539f, 69973.465230f, 69955.251249f, -69937.270455f, 69919.526706f, 69902.023810f, 69884.743721f, 69867.675255f, 69850.826821f, 69834.206826f, 69817.823677f, 69801.685204f, 69785.766631f, 69770.057621f, 69754.567323f, 69739.304886f, 69724.279457f, 69709.498429f, 69694.931510f, 69680.572196f, 69666.430488f, 69652.516390f, 69638.839902f, 69625.407251f, 69612.182849f, 69599.164123f, 69586.362097f, 69573.787799f, 69561.452254f, 69549.359575f, 69537.469102f, 69525.782350f, 69514.311645f, 69503.069316f, 69492.067690f, 69481.307561f, 69470.743620f, 69460.381548f, 69450.235154f, 69440.318249f, 69430.644643f, 69421.209926f, 69411.964697f, 69402.919186f, 69394.089247f, 69385.490733f, 69377.139497f, 69368.999277f, 69360.982592f, 69353.151977f, 69345.576786f, 69338.326370f, 69331.470085f, -69324.939731f, 69318.528062f, 69312.244507f, 69306.103083f, 69300.117802f, 69294.302680f, 69288.671733f, 69283.238974f, 69278.018419f, 69273.024082f, 69268.269979f, 69263.770057f, 69259.496465f, 69255.430429f, 69251.590144f, 69247.993806f, 69244.659609f, 69241.603360f, 69238.702076f, 69235.921723f, 69233.323021f, 69230.966692f, 69228.913456f, 69227.212789f, 69225.658652f, 69224.199438f, 69222.896700f, 69221.811991f, 69221.006867f, 69220.521365f, 69220.165707f, 69219.980730f, 69220.043787f, 69220.291628f, 69220.700570f, 69221.255032f, 69221.948886f, 69222.770099f, 69223.718588f, 69224.789886f, 69225.981368f, 69227.290896f, 69228.716654f, 69230.257039f, 69231.910574f, 69233.675843f, 69235.549975f, 69237.540004f, 69239.632469f, 69241.824949f, -69244.129000f, 69246.532391f, 69249.039408f, 69251.642883f, 69254.345106f, 69257.140414f, 69260.029163f, 69263.007155f, 69266.072861f, 69269.223550f, 69272.450692f, 69275.766748f, 69279.168952f, 69282.635802f, 69286.165981f, 69289.758352f, 69293.417545f, 69297.149941f, 69300.944975f, 69304.791543f, 69308.685239f, 69312.638698f, 69316.644437f, 69320.693092f, 69324.778818f, 69328.912099f, 69333.088079f, 69337.299064f, 69341.538963f, 69345.815661f, 69350.126596f, 69354.465583f, 69358.826867f, 69363.213771f, 69367.627361f, 69372.065912f, 69376.527678f, 69381.009650f, 69385.509511f, 69390.025756f, 69394.556883f, 69399.101390f, 69403.657774f, 69408.224532f, 69412.800161f, 69417.386881f, 69421.988651f, 69426.603633f, 69431.229929f, 69435.865639f, -69440.508865f, 69445.157708f, 69449.810267f, 69454.466317f, 69459.130478f, 69463.802243f, 69468.480551f, 69473.164343f, 69477.852558f, 69482.544136f, 69487.238016f, 69491.933450f, 69496.632799f, 69501.336372f, 69506.043658f, 69510.754144f, 69515.467316f, 69520.182662f, 69524.899670f, 69529.617835f, 69534.337496f, 69539.058979f, 69543.782169f, 69548.506952f, 69553.233217f, 69557.960848f, 69562.689732f, 69567.419756f, 69572.150806f, 69576.882769f, 69581.615530f, 69586.348978f, 69591.082997f, 69595.817475f, 69600.552298f, 69605.287352f, 69610.022715f, 69614.759045f, 69619.496350f, 69624.234558f, 69628.973600f, 69633.713404f, 69638.453901f, 69643.195021f, 69647.936692f, 69652.678846f, 69657.421410f, 69662.164316f, 69666.907492f, 69671.650869f, -69676.394376f, 69681.137942f, 69685.881502f, 69690.625429f, 69695.369961f, 69700.115033f, 69704.860577f, 69709.606527f, 69714.352818f, 69719.099383f, 69723.846155f, 69728.593069f, 69733.340058f, 69738.087056f, 69742.833997f, 69747.580814f, 69752.327441f, 69757.073812f, 69761.819861f, 69766.565581f, 69771.311194f, 69776.056732f, 69780.802202f, 69785.547609f, 69790.292960f, 69795.038260f, 69799.783515f, 69804.528732f, 69809.273916f, 69814.019073f, 69818.764210f, 69823.509332f, 69828.254446f, 69832.999556f, 69837.744671f, 69842.489792f, 69847.234634f, 69851.979048f, 69856.723127f, 69861.466963f, 69866.210649f, 69870.954278f, 69875.697942f, 69880.441734f, 69885.185746f, 69889.930071f, 69894.674803f, 69899.420033f, 69904.165854f, 69908.912359f, -69913.659640f, 69918.407791f, 69923.156846f, 69927.906588f, 69932.656953f, 69937.407909f, 69942.159422f, 69946.911459f, 69951.663986f, 69956.416970f, 69961.170378f, 69965.924176f, 69970.678330f, 69975.432809f, 69980.187577f, 69984.942602f, 69989.697851f, 69994.453289f, 69999.208886f, 70003.965286f, 70008.722876f, 70013.481460f, 70018.240838f, 70023.000812f, 70027.761183f, 70032.521754f, 70037.282324f, 70042.042696f, 70046.802672f, 70051.562052f, 70056.320639f, 70061.078234f, 70065.834638f, 70070.589652f, 70075.343079f, 70080.094733f, 70084.844644f, 70089.593001f, 70094.340001f, 70099.085838f, 70103.830707f, 70108.574805f, 70113.318325f, 70118.061463f, 70122.804415f, 70127.547375f, 70132.290539f, 70137.034101f, 70141.778258f, 70146.523203f, -70151.269133f, 70156.016240f, 70160.763002f, 70165.508278f, 70170.252467f, 70174.995967f, 70179.739176f, 70184.482494f, 70189.226319f, 70193.971048f, 70198.717081f, 70203.464816f, 70208.214651f, 70212.966986f, 70217.722217f, 70222.480744f, 70227.242966f, 70232.009280f, 70236.780549f, 70241.559099f, 70246.344308f, 70251.135131f, 70255.930525f, 70260.729445f, 70265.530846f, 70270.333684f, 70275.136914f, 70279.939493f, 70284.740376f, 70289.538518f, 70294.332875f, 70299.122403f, 70303.906057f, 70308.682792f, 70313.451565f, 70318.210776f, 70322.961124f, 70327.704896f, 70332.444377f, 70337.181855f, 70341.919615f, 70346.659944f, 70351.403715f, 70356.146580f, 70360.888784f, 70365.631222f, 70370.374788f, 70375.120377f, 70379.868883f, 70384.621204f, -70389.377900f, 70394.138254f, 70398.901278f, 70403.665987f, 70408.431393f, 70413.196509f, 70417.960348f, 70422.722605f, 70427.484595f, 70432.246456f, 70437.008245f, 70441.770019f, 70446.531833f, 70451.293746f, 70456.055791f, 70460.817692f, 70465.579468f, 70470.341273f, 70475.103255f, 70479.865567f, 70484.628360f, 70489.391785f, 70494.155787f, 70498.920090f, 70503.684684f, 70508.449560f, 70513.214712f, 70517.980132f, 70522.745814f, 70527.511750f, 70532.277933f, 70537.044357f, 70541.811012f, 70546.577894f, 70551.344994f, 70556.112306f, 70560.879821f, 70565.647616f, 70570.415747f, 70575.184189f, 70579.952916f, 70584.721900f, 70589.491115f, 70594.260535f, 70599.030133f, 70603.799882f, 70608.569757f, 70613.339731f, 70618.109777f, 70622.879869f, -70627.649980f, 70632.420085f, 70637.190258f, 70641.960544f, 70646.730942f, 70651.501448f, 70656.272057f, 70661.042768f, 70665.813576f, 70670.584478f, 70675.355471f, 70680.126551f, 70684.897715f, 70689.668960f, 70694.440283f, 70699.211679f, 70703.983144f, 70708.754661f, 70713.526230f, 70718.297861f, 70723.069563f, 70727.841343f, 70732.613211f, 70737.385176f, 70742.157246f, 70746.929430f, 70751.701738f, 70756.474177f, 70761.246756f, 70766.019485f, 70770.792372f, 70775.565416f, 70780.338544f, 70785.111740f, 70789.885006f, 70794.658343f, 70799.431753f, 70804.205237f, 70808.978796f, 70813.752432f, 70818.526146f, 70823.299940f, 70828.073815f, 70832.847772f, 70837.621813f, 70842.395939f, 70847.170152f, 70851.944453f, 70856.718843f, 70861.493324f, -70866.267897f, 70871.042564f, 70875.817326f, 70880.592184f, 70885.367140f, 70890.142195f, 70894.917351f, 70899.692609f, 70904.467970f, 70909.243436f, 70914.019009f, 70918.794678f, 70923.570417f, 70928.346226f, 70933.122109f, 70937.898068f, 70942.674106f, 70947.450226f, 70952.226432f, 70957.002725f, 70961.779110f, 70966.555587f, 70971.332162f, 70976.108835f, 70980.885611f, 70985.662492f, 70990.439482f, 70995.216582f, 70999.993796f, 71004.771126f, 71009.548576f, 71014.326149f, 71019.103846f, 71023.881672f, 71028.659629f, 71033.437720f, 71038.215948f, 71042.994316f, 71047.772826f, 71052.551481f, 71057.330285f, 71062.109178f, 71066.888106f, 71071.667081f, 71076.446116f, 71081.225223f, 71086.004414f, 71090.783700f, 71095.563095f, 71100.342611f, -71105.122259f, 71109.902052f, 71114.682003f, 71119.462122f, 71124.242424f, 71129.022918f, 71133.803619f, 71138.584538f, 71143.365687f, 71148.147079f, 71152.928725f, 71157.710638f, 71162.492831f, 71167.275314f, 71172.058101f, 71176.841204f, 71181.624634f, 71186.408400f, 71191.192411f, 71195.976631f, 71200.761069f, 71205.545732f, 71210.330628f, 71215.115762f, 71219.901144f, 71224.686780f, 71229.472677f, 71234.258844f, 71239.045286f, 71243.832012f, 71248.619028f, 71253.406343f, 71258.193963f, 71262.981896f, 71267.770149f, 71272.558730f, 71277.347645f, 71282.136902f, 71286.926508f, 71291.716471f, 71296.506799f, 71301.297505f, 71306.088692f, 71310.880368f, 71315.672508f, 71320.465086f, 71325.258078f, 71330.051457f, 71334.845199f, 71339.639278f, -71344.433670f, 71349.228348f, 71354.023288f, 71358.818464f, 71363.613850f, 71368.409423f, 71373.205156f, 71378.001025f, 71382.797003f, 71387.593066f, 71392.389188f, 71397.185344f, 71401.981509f, 71406.777799f, 71411.574458f, 71416.371460f, 71421.168771f, 71425.966357f, 71430.764183f, 71435.562216f, 71440.360421f, 71445.158765f, 71449.957213f, 71454.755731f, 71459.554286f, 71464.352843f, 71469.151367f, 71473.949826f, 71478.748185f, 71483.546409f, 71488.344465f, 71493.142319f, 71497.939936f, 71502.737283f, 71507.534340f, 71512.331304f, 71517.128243f, 71521.925147f, 71526.722006f, 71531.518810f, 71536.315549f, 71541.112213f, 71545.908791f, 71550.705274f, 71555.501652f, 71560.297914f, 71565.094050f, 71569.890051f, 71574.685905f, 71579.481604f, -71584.277137f, 71589.072494f, 71593.867664f, 71598.662639f, 71603.457407f, 71608.251958f, 71613.046220f, 71617.840064f, 71622.633531f, 71627.426665f, 71632.219511f, 71637.012114f, 71641.804518f, 71646.596769f, 71651.388911f, 71656.180990f, 71660.973049f, 71665.765135f, 71670.557291f, 71675.349563f, 71680.141995f, 71684.934633f, 71689.727521f, 71694.520703f, 71699.314226f, 71704.108087f, 71708.901911f, 71713.695641f, 71718.489318f, 71723.282987f, 71728.076689f, 71732.870468f, 71737.664367f, 71742.458429f, 71747.252696f, 71752.047211f, 71756.842018f, 71761.637158f, 71766.432676f, 71771.228613f, 71776.025013f, 71780.821919f, 71785.619373f, 71790.417418f, 71795.216097f, 71800.015326f, 71804.815001f, 71809.615114f, 71814.415656f, 71819.216618f, -71824.017992f, 71828.819768f, 71833.621939f, 71838.424496f, 71843.227430f, 71848.030733f, 71852.834395f, 71857.638408f, 71862.442765f, 71867.247455f, 71872.052470f, 71876.857802f, 71881.663442f, 71886.469381f, 71891.275687f, 71896.082577f, 71900.890027f, 71905.697991f, 71910.506424f, 71915.315283f, 71920.124521f, 71924.934093f, 71929.743956f, 71934.554063f, 71939.364370f, 71944.174832f, 71948.985404f, 71953.796040f, 71958.606697f, 71963.417329f, 71968.227891f, 71973.038338f, 71977.848625f, 71982.658734f, 71987.469038f, 71992.279637f, 71997.090485f, 72001.901534f, 72006.712738f, 72011.524050f, 72016.335424f, 72021.146812f, 72025.958168f, 72030.769445f, 72035.580596f, 72040.391575f, 72045.202335f, 72050.012828f, 72054.823009f, 72059.632830f, -72064.442245f, 72069.251207f, 72074.059668f, 72078.867569f, 72083.674908f, 72088.481741f, 72093.288126f, 72098.094119f, 72102.899777f, 72107.705157f, 72112.510317f, 72117.315312f, 72122.120200f, 72126.925037f, 72131.729881f, 72136.534788f, 72141.339816f, 72146.145021f, 72150.950460f, 72155.756190f, 72160.562268f, 72165.368750f, 72170.175583f, 72174.982287f, 72179.788847f, 72184.595317f, 72189.401750f, 72194.208201f, 72199.014722f, 72203.821369f, 72208.628194f, 72213.435253f, 72218.242597f, 72223.050283f, 72227.858362f, 72232.666889f, 72237.475919f, 72242.285504f, 72247.095698f, 72251.906556f, 72256.718132f, 72261.530476f, 72266.343543f, 72271.157270f, 72275.971621f, 72280.786563f, 72285.602063f, 72290.418086f, 72295.234598f, 72300.051565f, -72304.868953f, 72309.686729f, 72314.504859f, 72319.323307f, 72324.142042f, 72328.961028f, 72333.780231f, 72338.599618f, 72343.419155f, 72348.238973f, 72353.059414f, 72357.880430f, 72362.701953f, 72367.523918f, 72372.346259f, 72377.168908f, 72381.991799f, 72386.814866f, 72391.638043f, 72396.461262f, 72401.284458f, 72406.107564f, 72410.930514f, 72415.753241f, 72420.575679f, 72425.397761f, 72430.219442f, 72435.040970f, 72439.862429f, 72444.683816f, 72449.505130f, 72454.326366f, 72459.147523f, 72463.968598f, 72468.789588f, 72473.610490f, 72478.431301f, 72483.252019f, 72488.072641f, 72492.893164f, 72497.713586f, 72502.533903f, 72507.354113f, 72512.174213f, 72516.994113f, 72521.813708f, 72526.633037f, 72531.452145f, 72536.271075f, 72541.089868f, -72545.908567f, 72550.727216f, 72555.545857f, 72560.364533f, 72565.183287f, 72570.002162f, 72574.821199f, 72579.640443f, 72584.459936f, 72589.279721f, 72594.099839f, 72598.920293f, 72603.740818f, 72608.561381f, 72613.382010f, 72618.202730f, 72623.023566f, 72627.844546f, 72632.665693f, 72637.487036f, 72642.308599f, 72647.130408f, 72651.952490f, 72656.774869f, 72661.597573f, 72666.420627f, 72671.244056f, 72676.067888f, 72680.892147f, 72685.716825f, 72690.541887f, 72695.367317f, 72700.193096f, 72705.019209f, 72709.845637f, 72714.672365f, 72719.499375f, 72724.326649f, 72729.154171f, 72733.981924f, 72738.809891f, 72743.638054f, 72748.466397f, 72753.294903f, 72758.123554f, 72762.952333f, 72767.781278f, 72772.610591f, 72777.440259f, 72782.270248f, -72787.100521f, 72791.931042f, 72796.761774f, 72801.592682f, 72806.423728f, 72811.254878f, 72816.086093f, 72820.917340f, 72825.748580f, 72830.579779f, 72835.410899f, 72840.241904f, 72845.072758f, 72849.903428f, 72854.734005f, 72859.564554f, 72864.395078f, 72869.225584f, 72874.056075f, 72878.886556f, 72883.717032f, 72888.547507f, 72893.377987f, 72898.208475f, 72903.038977f, 72907.869497f, 72912.700039f, 72917.530609f, 72922.361211f, 72927.191849f, 72932.022528f, 72936.853160f, 72941.683533f, 72946.513691f, 72951.343691f, 72956.173591f, 72961.003449f, 72965.833323f, 72970.663270f, 72975.493348f, 72980.323615f, 72985.154128f, 72989.984944f, 72994.816122f, 72999.647719f, 73004.479793f, 73009.312401f, 73014.145601f, 73018.979440f, 73023.813777f, -73028.648541f, 73033.483715f, 73038.319285f, 73043.155234f, 73047.991547f, 73052.828208f, 73057.665201f, 73062.502510f, 73067.340120f, 73072.178015f, 73077.016179f, 73081.854596f, 73086.693250f, 73091.532127f, 73096.371209f, 73101.210482f, 73106.050256f, 73110.890979f, 73115.732519f, 73120.574736f, 73125.417489f, 73130.260639f, 73135.104046f, 73139.947568f, 73144.791067f, 73149.634402f, 73154.477433f, 73159.320019f, 73164.162021f, 73169.003299f, 73173.843712f, 73178.683121f, 73183.521384f, 73188.358382f, 73193.194215f, 73198.029013f, 73202.862887f, 73207.695948f, 73212.528308f, 73217.360077f, 73222.191366f, 73227.022287f, 73231.852950f, 73236.683467f, 73241.513948f, 73246.344504f, 73251.175247f, 73256.006288f, 73260.837737f, 73265.669705f, -73270.502305f, 73275.334345f, 73280.164808f, 73284.994041f, 73289.822393f, 73294.650211f, 73299.477844f, 73304.305640f, 73309.133947f, 73313.963114f, 73318.793488f, 73323.625417f, 73328.459250f, 73333.295335f, 73338.134020f, 73342.975653f, 73347.820583f, 73352.669157f, 73357.522081f, 73362.380888f, 73367.245056f, 73372.113824f, 73376.986432f, 73381.862119f, 73386.740124f, 73391.619688f, 73396.500050f, 73401.380449f, 73406.260126f, 73411.138318f, 73416.014268f, 73420.887212f, 73425.756392f, 73430.621047f, 73435.480417f, 73440.333741f, 73445.180735f, 73450.022264f, 73454.859370f, 73459.693090f, 73464.524466f, 73469.354536f, 73474.184340f, 73479.014918f, 73483.846776f, 73488.676493f, 73493.504087f, 73498.330582f, 73503.156999f, 73507.984361f, -73512.813690f, 73517.646006f, 73522.482334f, 73527.323468f, 73532.168877f, 73537.017691f, 73541.869051f, 73546.722098f, 73551.575975f, 73556.429822f, 73561.282783f, 73566.133997f, 73570.984173f, 73575.834544f, 73580.684945f, 73585.535213f, 73590.385185f, 73595.234695f, 73600.083579f, 73604.931675f, 73609.778780f, 73614.624217f, 73619.468190f, 73624.311232f, 73629.153874f, 73633.996649f, 73638.840088f, 73643.684723f, 73648.531086f, 73653.379555f, 73658.229608f, 73663.080935f, 73667.933296f, 73672.786453f, 73677.640169f, 73682.494205f, 73687.348323f, 73692.202285f, 73697.056243f, 73701.910723f, 73706.765604f, 73711.620754f, 73716.476038f, 73721.331325f, 73726.186480f, 73731.041369f, 73735.895863f, 73740.749980f, 73745.603839f, 73750.457513f, -73755.311076f, 73760.164601f, 73765.018162f, 73769.871832f, 73774.725685f, 73779.579764f, 73784.433840f, 73789.287873f, 73794.141885f, 73798.995897f, 73803.849933f, 73808.704013f, 73813.558161f, 73818.412398f, 73823.266746f, 73828.121229f, 73832.975867f, 73837.830684f, 73842.685701f, 73847.540940f, 73852.396424f, 73857.252175f, 73862.108215f, 73866.964500f, 73871.820965f, 73876.677611f, 73881.534439f, 73886.391448f, 73891.248640f, 73896.106016f, 73900.963574f, 73905.821317f, 73910.679245f, 73915.537358f, 73920.395656f, 73925.254141f, 73930.112812f, 73934.971671f, 73939.830717f, 73944.689952f, 73949.549363f, 73954.408889f, 73959.268536f, 73964.128319f, 73968.988254f, 73973.848355f, 73978.708639f, 73983.569120f, 73988.429814f, 73993.290735f, -73998.151900f, 74003.013324f, 74007.875021f, 74012.737007f, 74017.599297f, 74022.461908f, 74027.324853f, 74032.188148f, 74037.051836f, 74041.915921f, 74046.780383f, 74051.645199f, 74056.510348f, 74061.375810f, 74066.241561f, 74071.107582f, 74075.973849f, 74080.840343f, 74085.707041f, 74090.573923f, 74095.440965f, 74100.308148f, 74105.175449f, 74110.042848f, 74114.910322f, 74119.777850f, 74124.645411f, 74129.512983f, 74134.380545f, 74139.248075f, 74144.115552f, 74148.982955f, 74153.850261f, 74158.717450f, 74163.584500f, 74168.451389f, 74173.318096f, 74178.184600f, 74183.050879f, 74187.916911f, 74192.782676f, 74197.648152f, 74202.513317f, 74207.378183f, 74212.242787f, 74217.107161f, 74221.971341f, 74226.835359f, 74231.699251f, 74236.563049f, -74241.426789f, 74246.290503f, 74251.154226f, 74256.017992f, 74260.881834f, 74265.745788f, 74270.609886f, 74275.474029f, 74280.337947f, 74285.201690f, 74290.065322f, 74294.928907f, 74299.792511f, 74304.656197f, 74309.520029f, 74314.384073f, 74319.248393f, 74324.113053f, 74328.978118f, 74333.843652f, 74338.709720f, 74343.576376f, 74348.443473f, 74353.310936f, 74358.178754f, 74363.046920f, 74367.915425f, 74372.784261f, 74377.653420f, 74382.522893f, 74387.392672f, 74392.262748f, 74397.133113f, 74402.003759f, 74406.874677f, 74411.745859f, 74416.617297f, 74421.488996f, 74426.360962f, 74431.233206f, 74436.105735f, 74440.978557f, 74445.851682f, 74450.725118f, 74455.598873f, 74460.472956f, 74465.347375f, 74470.222139f, 74475.097256f, 74479.972736f, -74484.848596f, 74489.725025f, 74494.602053f, 74499.479634f, 74504.357721f, 74509.236268f, 74514.115229f, 74518.994557f, 74523.874205f, 74528.754128f, 74533.634278f, 74538.514610f, 74543.395076f, 74548.275631f, 74553.156228f, 74558.036820f, 74562.917361f, 74567.797805f, 74572.678106f, 74577.558216f, 74582.438089f, 74587.317679f, 74592.196940f, 74597.075824f, 74601.954287f, 74606.832280f, 74611.709758f, 74616.586674f, 74621.462983f, 74626.338621f, 74631.213353f, 74636.087232f, 74640.960419f, 74645.833073f, 74650.705354f, 74655.577422f, 74660.449437f, 74665.321559f, 74670.193947f, 74675.066761f, 74679.940162f, 74684.813894f, 74689.687548f, 74694.561202f, 74699.434938f, 74704.308838f, 74709.182983f, 74714.057453f, 74718.932331f, 74723.807698f, -74728.683635f, 74733.560224f, 74738.437524f, 74743.315473f, 74748.194007f, 74753.073068f, 74757.952600f, 74762.832544f, 74767.712844f, 74772.593441f, 74777.474280f, 74782.355302f, 74787.236451f, 74792.117676f, 74796.999141f, 74801.880908f, 74806.762954f, 74811.645255f, 74816.527787f, 74821.410527f, 74826.293451f, 74831.176536f, 74836.059758f, 74840.943094f, 74845.826520f, 74850.710017f, 74855.593586f, 74860.477232f, 74865.360956f, 74870.244764f, 74875.128658f, 74880.012641f, 74884.896718f, 74889.780891f, 74894.665165f, 74899.549542f, 74904.434026f, 74909.318621f, 74914.203330f, 74919.088156f, 74923.973104f, 74928.858175f, 74933.743375f, 74938.628706f, 74943.514172f, 74948.399777f, 74953.285523f, 74958.171414f, 74963.057435f, 74967.943578f, -74972.829842f, 74977.716227f, 74982.602735f, 74987.489364f, 74992.376115f, 74997.262988f, 75002.149983f, 75007.037100f, 75011.924339f, 75016.811700f, 75021.699183f, 75026.586789f, 75031.474517f, 75036.362368f, 75041.250341f, 75046.138437f, 75051.026655f, 75055.914997f, 75060.803461f, 75065.692047f, 75070.580752f, 75075.469542f, 75080.358418f, 75085.247385f, 75090.136450f, 75095.025618f, 75099.914897f, 75104.804291f, 75109.693807f, 75114.583451f, 75119.473229f, 75124.363148f, 75129.253213f, 75134.143430f, 75139.033806f, 75143.924347f, 75148.815058f, 75153.705947f, 75158.597018f, 75163.488279f, 75168.379735f, 75173.271392f, 75178.163256f, 75183.055295f, 75187.947425f, 75192.839644f, 75197.731952f, 75202.624352f, 75207.516844f, 75212.409430f, -75217.302111f, 75222.194888f, 75227.087762f, 75231.980735f, 75236.873809f, 75241.766983f, 75246.660260f, 75251.553641f, 75256.447127f, 75261.340720f, 75266.234419f, 75271.128228f, 75276.022147f, 75280.916178f, 75285.810321f, 75290.704578f, 75295.598950f, 75300.493439f, 75305.388045f, 75310.282770f, 75315.177616f, 75320.072583f, 75324.967673f, 75329.862887f, 75334.758226f, 75339.653692f, 75344.549286f, 75349.445009f, 75354.340862f, 75359.236847f, 75364.132965f, 75369.029217f, 75373.925604f, 75378.822128f, 75383.718790f, 75388.615591f, 75393.512533f, 75398.409616f, 75403.306843f, 75408.204223f, 75413.101755f, 75417.999438f, 75422.897270f, 75427.795248f, 75432.693371f, 75437.591637f, 75442.490043f, 75447.388588f, 75452.287269f, 75457.186085f, -75462.085034f, 75466.984113f, 75471.883320f, 75476.782653f, 75481.682111f, 75486.581692f, 75491.481392f, 75496.381211f, 75501.281146f, 75506.181196f, 75511.081357f, 75515.981629f, 75520.882008f, 75525.782494f, 75530.683084f, 75535.583776f, 75540.484568f, 75545.385458f, 75550.286443f, 75555.187523f, 75560.088695f, 75564.989956f, 75569.891305f, 75574.792740f, 75579.694259f, 75584.595859f, 75589.497540f, 75594.399297f, 75599.301131f, 75604.203038f, 75609.105016f, 75614.007064f, 75618.909180f, 75623.811361f, 75628.713611f, 75633.615934f, 75638.518334f, 75643.420810f, 75648.323365f, 75653.225999f, 75658.128715f, 75663.031513f, 75667.934396f, 75672.837364f, 75677.740419f, 75682.643562f, 75687.546795f, 75692.450119f, 75697.353536f, 75702.257046f, -75707.160652f, 75712.064354f, 75716.968155f, 75721.872055f, 75726.776056f, 75731.680160f, 75736.584367f, 75741.488680f, 75746.393099f, 75751.297626f, 75756.202263f, 75761.107010f, 75766.011870f, 75770.916843f, 75775.821931f, 75780.727136f, 75785.632458f, 75790.537900f, 75795.443463f, 75800.349147f, 75805.254955f, 75810.160888f, 75815.066947f, 75819.973134f, 75824.879450f, 75829.785897f, 75834.692476f, 75839.599187f, 75844.506034f, 75849.413015f, 75854.320123f, 75859.227365f, 75864.134750f, 75869.042287f, 75873.949982f, 75878.857846f, 75883.765886f, 75888.674111f, 75893.582529f, 75898.491149f, 75903.399978f, 75908.309027f, 75913.218302f, 75918.127813f, 75923.037567f, 75927.947574f, 75932.857841f, 75937.768377f, 75942.679191f, 75947.590290f, -75952.501684f, 75957.413381f, 75962.325360f, 75967.237575f, 75972.150028f, 75977.062721f, 75981.975654f, 75986.888832f, 75991.802254f, 75996.715925f, 76001.629845f, 76006.544016f, 76011.458441f, 76016.373122f, 76021.288060f, 76026.203258f, 76031.118718f, 76036.034441f, 76040.950430f, 76045.866687f, 76050.783213f, 76055.700011f, 76060.617083f, 76065.534430f, 76070.452057f, 76075.370027f, 76080.288365f, 76085.207055f, 76090.126082f, 76095.045428f, 76099.965079f, 76104.885016f, 76109.805226f, 76114.725690f, 76119.646394f, 76124.567320f, 76129.488454f, 76134.409778f, 76139.331276f, 76144.252933f, 76149.174732f, 76154.096657f, 76159.018692f, 76163.940820f, 76168.863026f, 76173.785293f, 76178.707606f, 76183.629986f, 76188.552712f, 76193.475811f, -76198.399245f, 76203.322977f, 76208.246968f, 76213.171182f, 76218.095580f, 76223.020126f, 76227.944782f, 76232.869509f, 76237.794271f, 76242.719029f, 76247.643747f, 76252.568387f, 76257.492911f, 76262.417281f, 76267.341460f, 76272.265410f, 76277.189095f, 76282.112475f, 76287.035514f, 76291.958174f, 76296.880514f, 76301.802758f, 76306.724912f, 76311.646965f, 76316.568910f, 76321.490737f, 76326.412437f, 76331.334003f, 76336.255426f, 76341.176696f, 76346.097806f, 76351.018746f, 76355.939509f, 76360.860084f, 76365.780464f, 76370.700640f, 76375.620604f, 76380.540346f, 76385.459858f, 76390.379131f, 76395.298157f, 76400.216927f, 76405.135432f, 76410.053364f, 76414.970514f, 76419.886971f, 76424.802823f, 76429.718157f, 76434.633061f, 76439.547623f, -76444.461931f, 76449.376072f, 76454.290134f, 76459.204205f, 76464.118373f, 76469.032726f, 76473.947351f, 76478.862335f, 76483.777768f, 76488.693737f, 76493.610328f, 76498.527631f, 76503.445733f, 76508.364722f, 76513.284685f, 76518.205641f, 76523.126742f, 76528.047782f, 76532.968843f, 76537.890008f, 76542.811360f, 76547.732984f, 76552.654963f, 76557.577379f, 76562.500317f, 76567.423859f, 76572.348090f, 76577.273092f, 76582.198949f, 76587.125744f, 76592.053561f, 76596.982483f, 76601.912593f, 76606.843975f, 76611.776712f, 76616.710888f, 76621.646586f, 76626.583889f, 76631.523265f, 76636.466040f, 76641.411982f, 76646.360705f, 76651.311826f, 76656.264961f, 76661.219726f, 76666.175736f, 76671.132609f, 76676.089959f, 76681.047402f, 76686.004556f, -76690.961035f, 76695.916456f, 76700.870434f, 76705.822587f, 76710.772528f, 76715.719876f, 76720.664245f, 76725.605252f, 76730.542511f, 76735.475805f, 76740.405565f, 76745.332351f, 76750.256722f, 76755.179239f, 76760.100462f, 76765.020951f, 76769.941267f, 76774.861969f, 76779.783617f, 76784.704984f, 76789.624638f, 76794.543125f, 76799.460991f, 76804.378781f, 76809.297041f, 76814.216316f, 76819.137153f, 76824.060097f, 76828.985694f, 76833.914209f, 76838.845076f, 76843.777952f, 76848.712513f, 76853.648437f, 76858.585403f, 76863.523086f, 76868.461164f, 76873.399316f, 76878.337217f, 76883.274804f, 76888.213128f, 76893.152181f, 76898.091776f, 76903.031724f, 76907.971836f, 76912.911924f, 76917.851800f, 76922.791274f, 76927.730159f, 76932.668283f, -76937.605782f, 76942.542809f, 76947.479464f, 76952.415848f, 76957.352060f, 76962.288200f, 76967.224370f, 76972.160667f, 76977.097193f, 76982.034044f, 76986.970961f, 76991.907785f, 76996.844590f, 77001.781450f, 77006.718439f, 77011.655631f, 77016.593101f, 77021.530922f, 77026.469169f, 77031.407917f, 77036.347107f, 77041.286587f, 77046.226369f, 77051.166473f, 77056.106915f, 77061.047713f, 77065.988883f, 77070.930444f, 77075.872412f, 77080.814805f, 77085.757642f, 77090.700919f, 77095.644604f, 77100.588667f, 77105.533075f, 77110.477794f, 77115.422794f, 77120.368041f, 77125.313504f, 77130.259150f, 77135.204946f, 77140.150861f, 77145.096863f, 77150.042918f, 77154.988995f, 77159.935061f, 77164.881084f, 77169.827032f, 77174.772872f, 77179.718573f, -77184.664109f, 77189.609652f, 77194.555273f, 77199.500959f, 77204.446702f, 77209.392489f, 77214.338311f, 77219.284156f, 77224.230015f, 77229.175877f, 77234.121730f, 77239.067564f, 77244.013369f, 77248.959134f, 77253.904849f, 77258.850502f, 77263.796083f, 77268.741581f, 77273.686986f, 77278.632256f, 77283.577347f, 77288.522284f, 77293.467093f, 77298.411802f, 77303.356436f, 77308.301023f, 77313.245589f, 77318.190160f, 77323.134763f, 77328.079425f, 77333.024171f, 77337.969030f, 77342.914026f, 77347.859187f, 77352.804540f, 77357.750110f, 77362.695924f, 77367.641976f, 77372.588052f, 77377.534133f, 77382.480243f, 77387.426408f, 77392.372653f, 77397.319003f, 77402.265484f, 77407.212122f, 77412.158941f, 77417.105968f, 77422.053227f, 77427.000743f, -77431.948543f, 77436.896651f, 77441.845093f, 77446.793894f, 77451.743080f, 77456.692676f, 77461.642633f, 77466.592898f, 77471.543465f, 77476.494329f, 77481.445483f, 77486.396922f, 77491.348640f, 77496.300630f, 77501.252887f, 77506.205406f, 77511.158179f, 77516.111201f, 77521.064467f, 77526.017971f, 77530.971705f, 77535.925666f, 77540.879846f, 77545.834240f, 77550.788887f, 77555.743908f, 77560.699288f, 77565.655000f, 77570.611020f, 77575.567319f, 77580.523873f, 77585.480654f, 77590.437638f, 77595.394797f, 77600.352106f, 77605.309538f, 77610.267068f, 77615.224669f, 77620.182314f, 77625.139978f, 77630.097635f, 77635.055259f, 77640.012833f, 77644.970507f, 77649.928328f, 77654.886282f, 77659.844357f, 77664.802538f, 77669.760812f, 77674.719167f, -77679.677588f, 77684.636062f, 77689.594576f, 77694.553117f, 77699.511671f, 77704.470225f, 77709.428765f, 77714.387278f, 77719.345750f, 77724.304170f, 77729.262522f, 77734.220805f, 77739.179037f, 77744.137228f, 77749.095387f, 77754.053525f, 77759.011651f, 77763.969775f, 77768.927906f, 77773.886055f, 77778.844232f, 77783.802445f, 77788.760706f, 77793.719023f, 77798.677407f, 77803.635867f, 77808.594413f, 77813.553055f, 77818.511803f, 77823.470633f, 77828.429370f, 77833.388016f, 77838.346604f, 77843.305166f, 77848.263734f, 77853.222343f, 77858.181025f, 77863.139812f, 77868.098737f, 77873.057834f, 77878.017135f, 77882.976673f, 77887.936480f, 77892.896590f, 77897.857036f, 77902.817849f, 77907.779064f, 77912.740711f, 77917.702687f, 77922.664915f, -77927.627397f, 77932.590135f, 77937.553131f, 77942.516387f, 77947.479904f, 77952.443685f, 77957.407732f, 77962.372046f, 77967.336630f, 77972.301485f, 77977.266613f, 77982.232017f, 77987.197698f, 77992.163657f, 77997.129898f, 78002.096422f, 78007.063298f, 78012.030684f, 78016.998543f, 78021.966831f, 78026.935502f, 78031.904509f, 78036.873809f, 78041.843356f, 78046.813103f, 78051.783007f, 78056.753020f, 78061.723099f, 78066.693197f, 78071.663269f, 78076.633270f, 78081.603154f, 78086.572876f, 78091.542390f, 78096.511675f, 78101.480983f, 78106.450381f, 78111.419849f, 78116.389368f, 78121.358917f, 78126.328477f, 78131.298028f, 78136.267551f, 78141.237026f, 78146.206433f, 78151.175752f, 78156.144965f, 78161.114051f, 78166.082990f, 78171.051764f, -78176.020351f, 78180.988734f, 78185.956891f, 78190.924711f, 78195.892113f, 78200.859153f, 78205.825888f, 78210.792374f, 78215.758668f, 78220.724826f, 78225.690906f, 78230.656964f, 78235.623057f, 78240.589241f, 78245.555574f, 78250.522111f, 78255.488910f, 78260.456027f, 78265.423519f, 78270.391443f, 78275.359855f, 78280.328710f, 78285.297568f, 78290.266419f, 78295.235311f, 78300.204296f, 78305.173424f, 78310.142747f, 78315.112313f, 78320.082175f, 78325.052382f, 78330.022985f, 78334.994035f, 78339.965582f, 78344.937676f, 78349.910370f, 78354.883712f, 78359.857753f, 78364.832545f, 78369.808138f, 78374.784604f, 78379.761919f, 78384.740018f, 78389.718838f, 78394.698316f, 78399.678388f, 78404.658992f, 78409.640063f, 78414.621539f, 78419.603356f, -78424.585451f, 78429.567760f, 78434.550220f, 78439.532768f, 78444.515340f, 78449.497872f, 78454.480303f, 78459.462567f, 78464.444971f, 78469.428226f, 78474.412225f, 78479.396829f, 78484.381903f, 78489.367307f, 78494.352905f, 78499.338558f, 78504.324131f, 78509.309484f, 78514.294480f, 78519.278983f, 78524.262854f, 78529.245956f, 78534.228152f, 78539.209303f, 78544.189272f, 78549.167923f, 78554.145074f, 78559.120287f, 78564.093672f, 78569.065468f, 78574.035913f, 78579.005247f, 78583.973708f, 78588.941536f, 78593.908969f, 78598.876245f, 78603.843605f, 78608.811286f, 78613.779527f, 78618.748568f, 78623.718648f, 78628.690004f, 78633.662877f, 78638.637505f, 78643.614126f, 78648.592687f, 78653.572855f, 78658.554449f, 78663.537287f, 78668.521188f, -78673.505972f, 78678.491457f, 78683.477463f, 78688.463809f, 78693.450903f, 78698.439625f, 78703.429574f, 78708.420316f, 78713.411414f, 78718.402433f, 78723.392937f, 78728.382490f, 78733.370657f, 78738.357133f, 78743.342297f, 78748.326438f, 78753.309809f, 78758.292660f, 78763.275246f, 78768.257817f, 78773.240626f, 78778.223925f, 78783.207879f, 78788.191922f, 78793.175964f, 78798.160076f, 78803.144330f, 78808.128798f, 78813.113553f, 78818.098666f, 78823.084210f, 78828.070255f, 78833.056773f, 78838.043704f, 78843.031006f, 78848.018641f, 78853.006568f, 78857.994747f, 78862.983139f, 78867.971702f, 78872.960398f, 78877.949252f, 78882.938305f, 78887.927544f, 78892.916956f, 78897.906529f, 78902.896251f, 78907.886108f, 78912.876090f, 78917.866183f, -78922.856375f, 78927.846654f, 78932.837007f, 78937.827422f, 78942.817886f, 78947.808387f, 78952.798912f, 78957.789450f, 78962.779988f, 78967.770528f, 78972.761121f, 78977.751771f, 78982.742479f, 78987.733245f, 78992.724070f, 78997.714953f, 79002.705896f, 79007.696898f, 79012.687959f, 79017.679080f, 79022.670262f, 79027.661504f, 79032.652807f, 79037.644171f, 79042.635597f, 79047.627084f, 79052.618634f, 79057.610243f, 79062.601861f, 79067.593476f, 79072.585102f, 79077.576750f, 79082.568435f, 79087.560169f, 79092.551966f, 79097.543838f, 79102.535797f, 79107.527859f, 79112.520034f, 79117.512337f, 79122.504780f, 79127.497376f, 79132.490138f, 79137.483080f, 79142.476214f, 79147.469553f, 79152.463060f, 79157.456664f, 79162.450369f, 79167.444187f, -79172.438124f, 79177.432191f, 79182.426396f, 79187.420748f, 79192.415255f, 79197.409928f, 79202.404773f, 79207.399802f, 79212.395021f, 79217.390440f, 79222.386069f, 79227.381915f, 79232.377988f, 79237.374296f, 79242.370846f, 79247.367622f, 79252.364616f, 79257.361823f, 79262.359239f, 79267.356858f, 79272.354674f, 79277.352685f, 79282.350883f, 79287.349265f, 79292.347826f, 79297.346559f, 79302.345462f, 79307.344527f, 79312.343752f, 79317.343130f, 79322.342657f, 79327.342327f, 79332.342136f, 79337.342059f, 79342.342089f, 79347.342237f, 79352.342514f, 79357.342933f, 79362.343504f, 79367.344239f, 79372.345149f, 79377.346246f, 79382.347542f, 79387.349048f, 79392.350776f, 79397.352736f, 79402.354941f, 79407.357402f, 79412.360131f, 79417.363138f, -79422.366436f, 79427.370043f, 79432.373976f, 79437.378223f, 79442.382766f, 79447.387591f, 79452.392682f, 79457.398023f, 79462.403599f, 79467.409393f, 79472.415391f, 79477.421576f, 79482.427933f, 79487.434446f, 79492.441100f, 79497.447878f, 79502.454766f, 79507.461746f, 79512.468805f, 79517.475926f, 79522.483093f, 79527.490291f, 79532.497504f, 79537.504717f, 79542.511913f, 79547.519077f, 79552.526193f, 79557.533246f, 79562.540220f, 79567.547100f, 79572.553869f, 79577.560512f, 79582.567014f, 79587.573358f, 79592.579528f, 79597.585511f, 79602.591288f, 79607.596846f, 79612.602170f, 79617.607277f, 79622.612199f, 79627.616971f, 79632.621626f, 79637.626196f, 79642.630716f, 79647.635218f, 79652.639736f, 79657.644304f, 79662.648954f, 79667.653719f, -79672.658634f, 79677.663731f, 79682.669034f, 79687.674355f, 79692.679638f, 79697.684915f, 79702.690217f, 79707.695577f, 79712.701028f, 79717.706599f, 79722.712325f, 79727.718237f, 79732.724366f, 79737.730745f, 79742.737406f, 79747.744381f, 79752.751701f, 79757.759372f, 79762.767302f, 79767.775476f, 79772.783891f, 79777.792544f, 79782.801433f, 79787.810554f, 79792.819903f, 79797.829479f, 79802.839278f, 79807.849296f, 79812.859532f, 79817.869980f, 79822.880640f, 79827.891507f, 79832.902559f, 79837.913778f, 79842.925180f, 79847.936775f, 79852.948578f, 79857.960602f, 79862.972858f, 79867.985362f, 79872.998124f, 79878.011160f, 79883.024480f, 79888.038100f, 79893.052030f, 79898.066286f, 79903.080878f, 79908.095801f, 79913.111036f, 79918.126567f, -79923.142379f, 79928.158456f, 79933.174781f, 79938.191339f, 79943.208113f, 79948.225087f, 79953.242246f, 79958.259574f, 79963.277054f, 79968.294670f, 79973.312407f, 79978.330248f, 79983.348178f, 79988.366179f, 79993.384238f, 79998.402337f, 80003.420460f, 80008.438591f, 80013.456715f, 80018.474815f, 80023.492876f, 80028.510881f, 80033.528814f, 80038.546659f, 80043.564401f, 80048.582023f, 80053.599598f, 80058.617209f, 80063.634854f, 80068.652534f, 80073.670247f, 80078.687993f, 80083.705771f, 80088.723582f, 80093.741424f, 80098.759298f, 80103.777202f, 80108.795137f, 80113.813101f, 80118.831095f, 80123.849115f, 80128.867138f, 80133.885165f, 80138.903204f, 80143.921262f, 80148.939346f, 80153.957465f, 80158.975626f, 80163.993837f, 80169.012105f, -80174.030439f, 80179.048846f, 80184.067333f, 80189.085909f, 80194.104581f, 80199.123253f, 80204.141651f, 80209.159795f, 80214.177721f, 80219.195466f, 80224.213067f, 80229.230562f, 80234.247986f, 80239.265377f, 80244.282771f, 80249.300206f, 80254.317719f, 80259.335346f, 80264.353124f, 80269.371091f, 80274.389283f, 80279.407737f, 80284.426489f, 80289.445578f, 80294.465039f, 80299.484911f, 80304.505228f, 80309.526029f, 80314.547351f, 80319.569229f, 80324.591702f, 80329.614806f, 80334.638579f, 80339.663056f, 80344.688323f, 80349.714784f, 80354.742396f, 80359.771002f, 80364.800444f, 80369.830565f, 80374.861208f, 80379.892217f, 80384.923434f, 80389.954702f, 80394.985864f, 80400.016762f, 80405.047420f, 80410.078409f, 80415.109719f, 80420.141270f, -80425.172983f, 80430.204778f, 80435.236577f, 80440.268300f, 80445.299867f, 80450.331199f, 80455.362216f, 80460.392841f, 80465.423047f, 80470.452930f, 80475.482539f, 80480.511920f, 80485.541122f, 80490.570190f, 80495.599173f, 80500.628116f, 80505.657066f, 80510.686072f, 80515.715180f, 80520.744436f, 80525.773696f, 80530.802837f, 80535.831902f, 80540.860937f, 80545.889985f, 80550.919093f, 80555.948304f, 80560.977663f, 80566.007215f, 80571.037004f, 80576.067076f, 80581.097467f, 80586.128031f, 80591.158698f, 80596.189469f, 80601.220346f, 80606.251332f, 80611.282429f, 80616.313638f, 80621.344962f, 80626.376403f, 80631.407962f, 80636.439642f, 80641.471445f, 80646.503372f, 80651.535426f, 80656.567609f, 80661.599923f, 80666.632370f, 80671.664952f, -80676.697671f, 80681.730528f, 80686.763527f, 80691.796669f, 80696.829956f, 80701.863387f, 80706.896949f, 80711.930643f, 80716.964468f, 80721.998424f, 80727.032512f, 80732.066732f, 80737.101083f, 80742.135566f, 80747.170182f, 80752.204929f, 80757.239809f, 80762.274821f, 80767.309966f, 80772.345243f, 80777.380654f, 80782.416197f, 80787.451873f, 80792.487682f, 80797.523625f, 80802.559701f, 80807.595911f, 80812.632254f, 80817.668731f, 80822.705354f, 80827.742129f, 80832.779053f, 80837.816123f, 80842.853338f, 80847.890693f, 80852.928187f, 80857.965815f, 80863.003577f, 80868.041467f, 80873.079485f, 80878.117627f, 80883.155889f, 80888.194270f, 80893.232767f, 80898.271376f, 80903.310094f, 80908.348920f, 80913.387850f, 80918.426881f, 80923.466011f, -80928.505237f, 80933.544555f, 80938.583951f, 80943.623347f, 80948.662733f, 80953.702115f, 80958.741498f, 80963.780889f, 80968.820292f, 80973.859714f, 80978.899159f, 80983.938633f, 80988.978142f, 80994.017691f, 80999.057286f, 81004.096932f, 81009.136635f, 81014.176401f, 81019.216234f, 81024.256141f, 81029.296127f, 81034.336198f, 81039.376358f, 81044.416615f, 81049.456972f, 81054.497436f, 81059.538012f, 81064.578706f, 81069.619523f, 81074.660469f, 81079.701549f, 81084.742769f, 81089.784135f, 81094.825651f, 81099.867324f, 81104.909159f, 81109.951162f, 81114.993337f, 81120.035691f, 81125.078229f, 81130.120957f, 81135.163880f, 81140.207004f, 81145.250334f, 81150.293876f, 81155.337636f, 81160.381618f, 81165.425828f, 81170.470273f, 81175.514959f, -81180.559917f, 81185.605160f, 81190.650687f, 81195.696500f, 81200.742601f, 81205.788991f, 81210.835672f, 81215.882644f, 81220.929909f, 81225.977468f, 81231.025323f, 81236.073476f, 81241.121926f, 81246.170676f, 81251.219727f, 81256.269081f, 81261.318738f, 81266.368700f, 81271.418971f, 81276.469637f, 81281.520725f, 81286.572213f, 81291.624075f, 81296.676290f, 81301.728832f, 81306.781679f, 81311.834806f, 81316.888190f, 81321.941807f, 81326.995634f, 81332.049646f, 81337.103820f, 81342.158133f, 81347.212561f, 81352.267079f, 81357.321665f, 81362.376294f, 81367.430943f, 81372.485777f, 81377.540930f, 81382.596371f, 81387.652068f, 81392.707988f, 81397.764100f, 81402.820371f, 81407.876770f, 81412.933264f, 81417.989820f, 81423.046408f, 81428.102994f, -81433.159547f, 81438.216035f, 81443.272425f, 81448.328685f, 81453.384784f, 81458.440688f, 81463.496367f, 81468.551908f, 81473.607472f, 81478.663044f, 81483.718612f, 81488.774159f, 81493.829671f, 81498.885134f, 81503.940532f, 81508.995852f, 81514.051078f, 81519.106196f, 81524.161190f, 81529.216048f, 81534.270752f, 81539.325290f, 81544.379646f, 81549.433806f, 81554.487755f, 81559.541478f, 81564.594896f, 81569.647876f, 81574.700462f, 81579.752706f, 81584.804662f, 81589.856383f, 81594.907920f, 81599.959326f, 81605.010655f, 81610.061958f, 81615.113289f, 81620.164700f, 81625.216243f, 81630.267972f, 81635.319939f, 81640.372197f, 81645.424798f, 81650.477794f, 81655.531240f, 81660.585047f, 81665.638733f, 81670.692305f, 81675.745827f, 81680.799361f, -81685.852970f, 81690.906717f, 81695.960664f, 81701.014874f, 81706.069411f, 81711.124336f, 81716.179714f, 81721.235606f, 81726.292075f, 81731.349184f, 81736.406996f, 81741.465575f, 81746.524981f, 81751.585279f, 81756.646519f, 81761.708641f, 81766.771588f, 81771.835311f, 81776.899762f, 81781.964893f, 81787.030655f, 81792.097000f, 81797.163880f, 81802.231246f, 81807.299050f, 81812.367244f, 81817.435779f, 81822.504608f, 81827.573681f, 81832.642951f, 81837.712369f, 81842.781886f, 81847.851456f, 81852.921108f, 81857.991770f, 81863.063585f, 81868.136378f, 81873.209978f, 81878.284211f, 81883.358904f, 81888.433884f, 81893.508978f, 81898.584012f, 81903.658815f, 81908.733212f, 81913.807032f, 81918.880099f, 81923.952243f, 81929.023288f, 81934.093064f, -81939.161395f, 81944.228110f, 81949.293031f, 81954.355927f, 81959.416897f, 81964.476153f, 81969.533910f, 81974.590380f, 81979.645777f, 81984.700313f, 81989.754203f, 81994.807658f, 81999.860893f, 82004.914121f, 82009.967554f, 82015.021406f, 82020.075890f, 82025.131219f, 82030.187607f, 82035.245267f, 82040.304411f, 82045.365252f, 82050.427282f, 82055.490039f, 82060.553587f, 82065.617989f, 82070.683310f, 82075.749614f, 82080.816963f, 82085.885423f, 82090.955056f, 82096.026090f, 82101.099383f, 82106.174620f, 82111.251285f, 82116.328865f, 82121.406843f, 82126.484705f, 82131.561934f, 82136.638017f, 82141.712437f, 82146.785291f, 82151.857285f, 82156.928546f, 82161.999199f, 82167.069370f, 82172.139183f, 82177.208763f, 82182.278235f, 82187.347725f, -82192.417272f, 82197.486065f, 82202.554116f, 82207.621732f, 82212.689223f, 82217.756897f, 82222.825064f, 82227.894032f, 82232.964110f, 82238.035608f, 82243.108794f, 82248.183560f, 82253.259586f, 82258.336554f, 82263.414144f, 82268.492036f, 82273.569910f, 82278.647446f, 82283.724325f, 82288.800246f, 82293.875547f, 82298.950513f, 82304.025208f, 82309.099696f, 82314.174041f, 82319.248306f, 82324.322555f, 82329.396853f, 82334.471262f, 82339.545759f, 82344.620102f, 82349.694334f, 82354.768523f, 82359.842738f, 82364.917048f, 82369.991521f, 82375.066228f, 82380.141237f, 82385.216615f, 82390.292240f, 82395.367987f, 82400.443860f, 82405.519860f, 82410.595990f, 82415.672251f, 82420.748645f, 82425.825176f, 82430.901844f, 82435.978651f, 82441.055601f, -82446.132695f, 82451.209936f, 82456.287324f, 82461.364863f, 82466.442555f, 82471.520401f, 82476.598404f, 82481.676566f, 82486.754885f, 82491.833358f, 82496.911983f, 82501.990759f, 82507.069685f, 82512.148760f, 82517.227984f, 82522.307355f, 82527.386872f, 82532.466534f, 82537.546341f, 82542.626290f, 82547.706382f, 82552.786615f, 82557.866988f, 82562.947500f, 82568.028151f, 82573.108939f, 82578.189862f, 82583.270907f, 82588.352044f, 82593.433284f, 82598.514633f, 82603.596101f, 82608.677697f, 82613.759429f, 82618.841306f, 82623.923337f, 82629.005530f, 82634.087895f, 82639.170440f, 82644.253173f, 82649.336104f, 82654.419242f, 82659.502594f, 82664.586171f, 82669.669979f, 82674.754029f, 82679.838282f, 82684.922586f, 82690.006935f, 82695.091337f, -82700.175798f, 82705.260326f, 82710.344927f, 82715.429609f, 82720.514378f, 82725.599242f, 82730.684208f, 82735.769282f, 82740.854472f, 82745.939784f, 82751.025226f, 82756.110805f, 82761.196528f, 82766.282401f, 82771.368432f, 82776.454627f, 82781.540995f, 82786.627541f, 82791.714273f, 82796.801198f, 82801.888323f, 82806.975655f, 82812.063200f, 82817.150967f, 82822.238961f, 82827.327190f, 82832.415661f, 82837.504382f, 82842.593358f, 82847.682597f, 82852.772106f, 82857.861893f, 82862.951963f, 82868.042324f, 82873.132999f, 82878.224145f, 82883.315779f, 82888.407879f, 82893.500419f, 82898.593376f, 82903.686725f, 82908.780442f, 82913.874503f, 82918.968883f, 82924.063558f, 82929.158505f, 82934.253698f, 82939.349114f, 82944.444728f, 82949.540515f, -82954.636453f, 82959.732516f, 82964.828680f, 82969.924921f, 82975.021214f, 82980.117536f, 82985.213862f, 82990.310168f, 82995.406429f, 83000.502622f, 83005.598721f, 83010.694704f, 83015.790545f, 83020.886221f, 83025.981706f, 83031.076977f, 83036.172010f, 83041.266780f, 83046.361263f, 83051.455434f, 83056.549270f, 83061.642746f, 83066.735840f, 83071.828665f, 83076.921312f, 83082.013783f, 83087.106078f, 83092.198199f, 83097.290149f, 83102.381929f, 83107.473540f, 83112.564984f, 83117.656262f, 83122.747377f, 83127.838330f, 83132.929123f, 83138.019757f, 83143.110234f, 83148.200556f, 83153.290946f, 83158.382126f, 83163.473963f, 83168.566244f, 83173.658757f, 83178.751290f, 83183.843630f, 83188.935566f, 83194.026884f, 83199.117372f, 83204.206819f, -83209.295011f, 83214.381737f, 83219.466784f, 83224.549940f, 83229.630993f, 83234.709552f, 83239.778974f, 83244.842262f, 83249.908073f, 83254.985061f, 83259.991193f, 83264.515036f, 83268.412046f, 83271.635444f, 83274.201854f, 83276.170728f, 83277.575065f, 83278.577313f, 83279.204936f, 83279.514066f, 83279.641002f, 83279.635960f, 83279.513557f, 83279.349332f, 83279.170676f, 83279.006651f, 83278.887807f, 83278.844140f, 83278.868068f, 83278.954816f, 83279.160191f, 83279.462095f, 83279.870304f, 83280.345104f, 83280.914448f, 83281.604750f, 83282.363578f, 83283.181514f, 83284.089786f, 83285.049874f, 83286.042678f, 83287.091023f, 83288.167346f, 83289.250478f, 83290.356354f, 83291.467199f, 83292.564231f, 83293.657582f, 83294.738659f, 83295.794237f, -83296.828905f, 83297.841892f, 83298.827792f, 83299.786919f, 83300.722313f, 83301.635656f, 83302.526238f, 83303.396220f, 83304.250848f, 83305.090729f, 83305.915467f, 83306.731559f, 83307.541617f, 83308.343155f, 83309.141690f, 83309.940773f, 83310.737627f, 83311.535797f, 83312.338584f, 83313.144301f, 83313.954057f, 83314.769491f, 83315.590337f, 83316.416361f, 83317.247627f, 83318.084422f, 83318.926419f, 83319.773076f, 83320.624271f, 83321.479427f, 83322.337579f, 83323.197762f, 83324.059014f, 83324.920369f, 83325.782210f, 83326.646001f, 83327.510995f, 83328.376407f, 83329.241453f, 83330.105345f, 83330.967739f, 83331.829906f, 83332.691654f, 83333.552583f, 83334.412293f, 83335.270384f, 83336.126418f, 83336.979818f, 83337.831186f, 83338.681394f, -83339.531316f, 83340.381825f, 83341.233465f, 83342.085558f, 83342.937797f, 83343.789931f, 83344.641706f, 83345.492869f, 83346.343019f, 83347.192373f, 83348.041533f, 83348.891105f, 83349.741693f, 83350.593545f, 83351.446255f, 83352.299494f, 83353.152930f, 83354.006233f, 83354.859304f, 83355.712391f, 83356.565549f, 83357.418830f, 83358.272286f, 83359.126023f, 83359.980067f, 83360.834237f, 83361.688347f, 83362.542214f, 83363.395734f, 83364.249043f, 83365.102185f, 83365.955201f, 83366.808128f, 83367.661006f, 83368.513874f, 83369.366769f, 83370.219731f, 83371.072799f, 83371.925995f, 83372.779267f, 83373.632595f, 83374.485963f, 83375.339357f, 83376.192763f, 83377.046164f, 83377.899548f, 83378.752898f, 83379.606201f, 83380.459445f, 83381.312658f, -83382.165846f, 83383.019012f, 83383.872158f, 83384.725284f, 83385.578393f, 83386.431485f, 83387.284563f, 83388.137628f, 83388.990679f, 83389.843684f, 83390.696643f, 83391.549571f, 83392.402481f, 83393.255389f, 83394.108310f, 83394.961258f, 83395.814247f, 83396.667292f, 83397.520408f, 83398.373607f, 83399.226881f, 83400.080221f, 83400.933619f, 83401.787063f, 83402.640544f, 83403.494052f, 83404.347578f, 83405.201112f, 83406.054644f, 83406.908164f, 83407.761662f, 83408.615129f, 83409.468556f, 83410.321931f, 83411.175246f, 83412.028491f, 83412.881655f, 83413.734730f, 83414.587705f, 83415.440508f, 83416.293063f, 83417.145438f, 83417.997703f, 83418.849928f, 83419.702185f, 83420.554543f, 83421.407073f, 83422.259846f, 83423.112932f, 83423.966421f, -83424.820357f, 83425.674640f, 83426.529163f, 83427.383821f, 83428.238506f, 83429.093113f, 83429.947535f, 83430.801667f, 83431.655400f, 83432.508624f, 83433.361300f, 83434.213546f, 83435.065490f, 83435.917262f, 83436.768991f, 83437.620808f, 83438.472842f, 83439.325221f, 83440.178077f, 83441.031543f, 83441.885713f, 83442.740455f, 83443.595606f, 83444.451003f, 83445.306480f, 83446.161876f, 83447.017025f, 83447.871764f, 83448.725930f, 83449.579354f, 83450.431861f, 83451.283589f, 83452.134759f, 83452.985592f, 83453.836307f, 83454.687126f, 83455.538269f, 83456.389956f, 83457.242410f, 83458.095850f, 83458.950596f, 83459.806517f, 83460.663255f, 83461.520451f, 83462.377745f, 83463.234779f, 83464.091194f, 83464.946631f, 83465.800730f, 83466.653131f, -83467.503986f, 83468.353741f, 83469.202843f, 83470.051735f, 83470.900866f, 83471.750680f, 83472.601623f, 83473.454142f, 83474.308711f, 83475.165243f, 83476.023118f, 83476.881714f, 83477.740407f, 83478.598572f, 83479.455586f, 83480.310824f, 83481.163811f, 83482.014902f, 83482.864735f, 83483.713944f, 83484.563167f, 83485.413040f, 83486.264198f, 83487.117129f, 83487.971444f, 83488.826670f, 83489.682357f, 83490.538054f, 83491.393314f, 83492.247694f, 83493.101264f, 83493.954349f, 83494.807143f, 83495.659838f, 83496.512629f, 83497.365710f, 83498.219202f, 83499.072957f, 83499.926877f, 83500.780876f, 83501.634865f, 83502.488759f, 83503.342470f, 83504.195995f, 83505.049401f, 83505.902747f, 83506.756093f, 83507.609498f, 83508.463022f, 83509.316719f, -83510.170569f, 83511.024509f, 83511.878478f, 83512.732415f, 83513.586255f, 83514.439937f, 83515.293425f, 83516.146776f, 83517.000067f, 83517.853374f, 83518.706772f, 83519.560337f, 83520.414151f, 83521.268206f, 83522.122394f, 83522.976607f, 83523.830736f, 83524.684673f, 83525.538302f, 83526.391512f, 83527.244433f, 83528.097244f, 83528.950124f, 83529.803254f, 83530.656810f, 83531.511036f, 83532.365928f, 83533.221166f, 83534.076426f, 83534.931385f, 83535.785718f, 83536.639062f, 83537.491002f, 83538.342002f, 83539.192690f, 83540.043698f, 83540.895653f, 83541.749186f, 83542.605307f, 83543.463999f, 83544.323941f, 83545.183801f, 83546.042249f, 83546.897955f, 83547.749357f, 83548.594678f, 83549.436068f, 83550.276291f, 83551.118114f, 83551.964301f, -83552.817618f, 83553.682461f, 83554.557796f, 83555.437490f, 83556.315399f, 83557.185384f, 83558.041252f, 83558.876953f, 83559.698681f, 83560.516790f, 83561.341634f, 83562.183567f, 83563.054340f, 83563.950393f, 83564.855279f, 83565.752473f, 83566.625454f, 83567.457490f, 83568.250146f, 83569.027557f, 83569.814780f, 83570.636874f, 83571.520202f, 83572.433944f, 83573.318924f, 83574.184044f, 83575.054259f, 83575.938742f, 83576.819773f, 83577.681124f, 83578.533042f, 83579.394290f, 83580.265023f, 83581.127961f, 83581.974526f, 83582.817340f, 83583.668240f, 83584.522456f, 83585.372652f, 83586.218329f, 83587.063904f, 83587.911213f, 83588.758905f, 83589.606177f, 83590.453302f, 83591.301440f, 83592.150414f, 83592.999981f, 83593.850132f, 83594.700859f, -83595.552173f, 83596.404120f, 83597.256644f, 83598.109677f, 83598.963152f, 83599.817155f, 83600.671767f, 83601.526788f, 83602.382019f, 83603.237268f, 83604.092669f, 83604.948270f, 83605.803969f, 83606.659662f, 83607.515290f, 83608.371071f, 83609.226986f, 83610.082955f, 83610.938896f, 83611.794731f, 83612.650378f, 83613.505759f, 83614.360791f, 83615.215396f, 83616.069419f, 83616.922867f, 83617.775892f, 83618.628645f, 83619.481277f, 83620.333938f, 83621.186780f, 83622.039954f, 83622.893610f, 83623.747912f, 83624.602925f, 83625.458469f, 83626.314347f, 83627.170360f, 83628.026311f, 83628.882004f, 83629.737240f, 83630.591823f, 83631.445550f, 83632.298083f, 83633.149567f, 83634.000372f, 83634.850867f, 83635.701420f, 83636.552402f, 83637.404183f, -83638.257131f, 83639.111643f, 83639.967796f, 83640.825138f, 83641.683191f, 83642.541475f, 83643.399512f, 83644.256825f, 83645.112935f, 83645.967360f, 83646.819673f, 83647.670287f, 83648.519863f, 83649.369064f, 83650.218553f, 83651.068991f, 83651.921042f, 83652.775364f, 83653.632045f, 83654.490339f, 83655.349441f, 83656.208547f, 83657.066853f, 83657.923555f, 83658.777955f, 83659.630329f, 83660.481453f, 83661.332100f, 83662.183047f, 83663.035066f, 83663.888878f, 83664.744362f, 83665.600903f, 83666.457907f, 83667.314778f, 83668.170920f, 83669.025749f, 83669.879179f, 83670.731680f, 83671.583728f, 83672.435798f, 83673.288366f, 83674.141907f, 83674.996652f, 83675.852255f, 83676.708303f, 83677.564387f, 83678.420092f, 83679.275009f, 83680.128780f, -83680.981593f, 83681.833908f, 83682.686184f, 83683.538884f, 83684.392468f, 83685.247420f, 83686.103768f, 83686.960910f, 83687.818219f, 83688.675067f, 83689.530827f, 83690.384834f, 83691.236769f, 83692.087429f, 83692.937723f, 83693.788563f, 83694.640858f, 83695.495560f, 83696.353619f, 83697.213991f, 83698.075174f, 83698.935666f, 83699.793965f, 83700.648561f, 83701.497561f, 83702.342139f, 83703.184870f, 83704.028326f, 83704.875081f, 83705.727710f, 83706.589530f, 83707.459701f, 83708.333375f, 83709.205684f, 83710.071757f, 83710.926644f, 83711.767701f, 83712.600200f, 83713.430606f, 83714.265385f, 83715.111002f, 83715.974662f, 83716.856176f, 83717.745877f, 83718.633864f, 83719.510231f, 83720.364976f, 83721.190236f, 83721.996709f, 83722.799911f, -83723.615358f, 83724.458822f, 83725.343380f, 83726.236225f, 83727.109486f, 83727.977195f, 83728.848785f, 83729.722002f, 83730.588996f, 83731.448806f, 83732.307486f, 83733.167547f, 83734.026003f, 83734.880655f, 83735.732881f, 83736.584477f, 83737.435319f, 83738.285518f, 83739.135274f, 83739.984781f, 83740.833661f, 83741.681969f, 83742.530230f, 83743.378966f, 83744.228526f, 83745.078515f, 83745.928933f, 83746.779846f, 83747.631323f, 83748.483408f, 83749.336051f, 83750.189175f, 83751.042702f, 83751.896576f, 83752.750814f, 83753.605380f, 83754.460224f, 83755.315297f, 83756.170551f, 83757.025937f, 83757.881406f, 83758.736911f, 83759.592454f, 83760.448243f, 83761.304241f, 83762.160369f, 83763.016552f, 83763.872714f, 83764.728776f, 83765.584664f, -83766.440314f, 83767.295804f, 83768.151176f, 83769.006452f, 83769.861656f, 83770.716808f, 83771.571930f, 83772.427044f, 83773.282173f, 83774.137338f, 83774.992516f, 83775.847678f, 83776.702792f, 83777.557828f, 83778.412755f, 83779.267541f, 83780.122155f, 83780.976523f, 83781.830668f, 83782.684661f, 83783.538574f, 83784.392478f, 83785.246443f, 83786.100542f, 83786.954846f, 83787.809325f, 83788.663908f, 83789.518580f, 83790.373328f, 83791.228138f, 83792.082995f, 83792.937885f, 83793.792796f, 83794.647768f, 83795.502849f, 83796.358003f, 83797.213191f, 83798.068376f, 83798.923521f, 83799.778588f, 83800.633539f, 83801.488334f, 83802.342976f, 83803.197514f, 83804.051996f, 83804.906472f, 83805.760991f, 83806.615603f, 83807.470355f, 83808.325294f, -83809.180416f, 83810.035693f, 83810.891095f, 83811.746595f, 83812.602163f, 83813.457770f, 83814.313387f, 83815.168985f, 83816.024536f, 83816.880009f, 83817.735377f, 83818.590610f, 83819.445680f, 83820.300557f, 83821.155212f, 83822.009595f, 83822.863521f, 83823.717026f, 83824.570198f, 83825.423123f, 83826.275889f, 83827.128584f, 83827.981295f, 83828.834109f, 83829.687114f, 83830.540397f, 83831.394045f, 83832.248146f, 83833.102788f, 83833.958056f, 83834.814233f, 83835.671521f, 83836.529467f, 83837.387597f, 83838.245439f, 83839.102520f, 83839.958715f, 83840.814405f, 83841.669744f, 83842.524886f, 83843.379983f, 83844.235188f, 83845.090460f, 83845.945690f, 83846.800872f, 83847.656003f, 83848.511079f, 83849.366095f, 83850.221018f, 83851.075849f, -83851.930611f, 83852.785323f, 83853.640007f, 83854.494684f, 83855.349374f, 83856.204100f, 83857.058883f, 83857.913742f, 83858.768700f, 83859.623779f, 83860.479002f, 83861.334344f, 83862.189777f, 83863.045271f, 83863.900797f, 83864.756327f, 83865.611830f, 83866.467278f, 83867.322642f, 83868.177892f, 83869.033000f, 83869.887921f, 83870.742623f, 83871.597154f, 83872.451561f, 83873.305895f, 83874.160203f, 83875.014535f, 83875.868939f, 83876.723466f, 83877.578162f, 83878.433079f, 83879.288263f, 83880.143810f, 83880.999722f, 83881.855917f, 83882.712313f, 83883.568828f, 83884.425378f, 83885.281882f, 83886.138257f, 83886.994420f, 83887.850289f, 83888.705781f, 83889.560807f, 83890.415230f, 83891.269115f, 83892.122593f, 83892.975793f, 83893.828844f, -83894.681875f, 83895.535016f, 83896.388396f, 83897.242144f, 83898.096390f, 83898.951262f, 83899.806899f, 83900.663355f, 83901.520478f, 83902.378099f, 83903.236049f, 83904.094160f, 83904.952261f, 83905.810186f, 83906.667764f, 83907.524828f, 83908.381207f, 83909.236231f, 83910.089660f, 83910.942020f, 83911.793841f, 83912.645649f, 83913.497971f, 83914.351336f, 83915.206271f, 83916.063303f, 83916.922960f, 83917.786233f, 83918.655841f, 83919.528857f, 83920.401300f, 83921.269189f, 83922.129616f, 83922.984721f, 83923.836574f, 83924.687109f, 83925.538261f, 83926.390491f, 83927.242483f, 83928.094031f, 83928.944937f, 83929.794987f, 83930.643574f, 83931.491118f, 83932.338502f, 83933.186608f, 83934.036200f, 83934.886838f, 83935.738215f, 83936.590179f, -83937.442580f, 83938.295312f, 83939.148461f, 83940.002094f, 83940.856269f, 83941.711046f, 83942.566611f, 83943.422958f, 83944.279768f, 83945.136724f, 83945.993514f, 83946.850211f, 83947.706947f, 83948.563725f, 83949.420545f, 83950.277411f, 83951.134323f, 83951.991283f, 83952.848294f, 83953.705356f, 83954.562577f, 83955.420213f, 83956.278133f, 83957.136179f, 83957.994193f, 83958.852019f, 83959.709497f, 83960.566470f, 83961.422781f, 83962.278266f, 83963.132663f, 83963.986080f, 83964.838784f, 83965.691042f, 83966.543120f, 83967.395283f, 83968.247798f, 83969.100932f, 83969.954950f, 83970.810243f, 83971.667116f, 83972.525121f, 83973.383765f, 83974.242556f, 83975.101001f, 83975.958609f, 83976.814887f, 83977.669344f, 83978.521445f, 83979.369594f, -83980.214917f, 83981.059816f, 83981.906690f, 83982.757719f, 83983.612272f, 83984.468993f, 83985.326742f, 83986.184379f, 83987.041173f, 83987.897904f, 83988.754830f, 83989.612133f, 83990.469997f, 83991.328743f, 83992.188306f, 83993.048161f, 83993.907779f, 83994.766647f, 83995.624865f, 83996.482694f, 83997.340194f, 83998.197424f, 83999.054428f, 83999.911123f, 84000.767539f, 84001.623738f, 84002.479781f, 84003.335682f, 84004.191358f, 84005.046869f, 84005.902287f, 84006.757683f, 84007.613064f, 84008.468379f, 84009.323639f, 84010.178860f, 84011.034056f, 84011.889241f, 84012.744429f, 84013.599635f, 84014.454873f, 84015.310149f, 84016.165411f, 84017.020656f, 84017.875901f, 84018.731158f, 84019.586442f, 84020.441767f, 84021.297149f, 84022.152600f, -84023.008135f, 84023.863776f, 84024.719521f, 84025.575348f, 84026.431236f, 84027.287162f, 84028.143103f, 84028.999037f, 84029.854941f, 84030.710793f, 84031.566571f, 84032.422269f, 84033.277911f, 84034.133517f, 84034.989114f, 84035.844723f, 84036.700368f, 84037.556073f, 84038.411862f, 84039.267757f, 84040.123806f, 84040.980019f, 84041.836350f, 84042.692752f, 84043.549175f, 84044.405573f, 84045.261899f, 84046.118103f, 84046.974140f, 84047.829952f, 84048.685468f, 84049.540745f, 84050.395867f, 84051.250917f, 84052.105977f, 84052.961130f, 84053.816459f, 84054.672047f, 84055.527977f, 84056.384423f, 84057.241437f, 84058.098844f, 84058.956466f, 84059.814125f, 84060.671645f, 84061.528847f, 84062.385555f, 84063.241592f, 84064.096748f, 84064.950749f, -84065.803813f, 84066.656252f, 84067.508377f, 84068.360498f, 84069.212926f, 84070.065971f, 84070.919945f, 84071.775158f, 84072.632097f, 84073.490552f, 84074.349574f, 84075.208211f, 84076.065735f, 84076.922598f, 84077.779209f, 84078.635934f, 84079.493132f, 84080.350868f, 84081.208821f, 84082.066641f, 84082.923982f, 84083.780758f, 84084.637178f, 84085.493399f, 84086.349576f, 84087.205784f, 84088.061920f, 84088.917975f, 84089.773939f, 84090.629798f, 84091.485517f, 84092.341126f, 84093.196667f, 84094.052178f, 84094.907700f, 84095.763273f, 84096.618936f, 84097.474730f, 84098.330666f, 84099.186712f, 84100.042839f, 84100.899020f, 84101.755226f, 84102.611431f, 84103.467605f, 84104.323722f, 84105.179754f, 84106.035699f, 84106.891582f, 84107.747432f, -84108.603276f, 84109.459141f, 84110.315055f, 84111.171045f, 84112.027138f, 84112.883362f, 84113.739716f, 84114.596181f, 84115.452736f, 84116.309362f, 84117.166039f, 84118.022748f, 84118.879468f, 84119.736181f, 84120.592866f, 84121.449505f, 84122.306076f, 84123.162561f, 84124.018940f, 84124.875193f, 84125.731300f, 84126.587242f, 84127.442938f, 84128.298294f, 84129.153413f, 84130.008402f, 84130.863366f, 84131.718412f, 84132.573649f, 84133.429181f, 84134.285117f, 84135.141502f, 84135.998245f, 84136.855239f, 84137.712373f, 84138.569538f, 84139.426625f, 84140.283526f, 84141.140130f, 84141.996346f, 84142.852217f, 84143.707843f, 84144.563321f, 84145.418752f, 84146.274235f, 84147.129868f, 84147.985752f, 84148.841985f, 84149.698667f, 84150.555726f, -84151.413032f, 84152.270457f, 84153.127870f, 84153.985142f, 84154.842143f, 84155.698743f, 84156.554788f, 84157.410167f, 84158.265065f, 84159.119690f, 84159.974250f, 84160.828954f, 84161.684010f, 84162.539627f, 84163.396013f, 84164.253518f, 84165.112084f, 84165.971338f, 84166.830908f, 84167.690421f, 84168.549505f, 84169.407787f, 84170.264895f, 84171.120388f, 84171.973828f, 84172.825777f, 84173.676935f, 84174.528000f, 84175.379669f, 84176.232640f, 84177.087612f, 84177.945382f, 84178.805939f, 84179.668225f, 84180.531158f, 84181.393657f, 84182.254641f, 84183.113028f, 84183.967809f, 84184.819444f, 84185.669332f, 84186.518879f, 84187.369491f, 84188.222573f, 84189.079477f, 84189.940053f, 84190.802678f, 84191.665670f, 84192.527353f, 84193.386046f, -84194.240731f, 84195.092552f, 84195.943083f, 84196.793897f, 84197.646566f, 84198.502636f, 84199.362343f, 84200.224102f, 84201.086192f, 84201.946891f, 84202.804529f, 84203.658947f, 84204.511547f, 84205.363779f, 84206.217092f, 84207.072912f, 84207.931550f, 84208.791806f, 84209.652384f, 84210.511987f, 84211.369331f, 84212.224042f, 84213.077182f, 84213.929930f, 84214.783468f, 84215.638973f, 84216.496945f, 84217.356415f, 84218.216221f, 84219.075203f, 84219.932195f, 84220.786384f, 84221.638780f, 84222.490844f, 84223.344036f, 84224.199821f, 84225.059499f, 84225.921847f, 84226.784785f, 84227.646236f, 84228.504119f, 84229.356509f, 84230.204991f, 84231.052556f, 84231.902190f, 84232.756881f, 84233.619600f, 84234.488092f, 84235.357518f, 84236.223041f, -84237.079865f, 84237.927174f, 84238.770201f, 84239.614632f, 84240.466158f, 84241.329920f, 84242.202884f, 84243.077698f, 84243.946999f, 84244.803378f, 84245.644418f, 84246.478890f, 84247.316864f, 84248.168406f, 84249.037774f, 84249.915464f, 84250.790101f, 84251.650302f, 84252.490184f, 84253.319949f, 84254.152600f, 84255.001161f, 84255.875276f, 84256.764153f, 84257.651201f, 84258.519833f, 84259.358546f, 84260.179177f, 84261.000102f, 84261.839701f, 84262.713936f, 84263.611288f, 84264.506796f, 84265.375668f, 84266.209989f, 84267.030278f, 84267.859680f, 84268.720616f, 84269.611941f, 84270.506524f, 84271.376284f, 84272.212880f, 84273.038546f, 84273.877914f, 84274.749094f, 84275.636393f, 84276.513212f, 84277.363280f, 84278.201029f, 84279.044828f, -84279.905682f, 84280.773765f, 84281.636676f, 84282.486896f, 84283.331735f, 84284.180940f, 84285.040227f, 84285.903874f, 84286.764441f, 84287.617930f, 84288.468654f, 84289.321799f, 84290.179883f, 84291.040071f, 84291.899257f, 84292.756203f, 84293.612403f, 84294.469175f, 84295.327101f, 84296.185437f, 84297.043379f, 84297.900610f, 84298.757564f, 84299.614678f, 84300.472165f, 84301.329871f, 84302.187626f, 84303.045260f, 84303.902604f, 84304.759484f, 84305.615713f, 84306.471446f, 84307.326957f, 84308.182524f, 84309.038420f, 84309.894922f, 84310.752145f, 84311.609864f, 84312.467810f, 84313.325717f, 84314.183314f, 84315.040335f, 84315.896649f, 84316.752490f, 84317.608151f, 84318.463922f, 84319.320094f, 84320.176960f, 84321.034678f, 84321.892975f, -84322.751506f, 84323.609926f, 84324.467890f, 84325.325049f, 84326.181201f, 84327.036696f, 84327.891973f, 84328.747471f, 84329.603631f, 84330.460907f, 84331.319610f, 84332.179244f, 84333.039177f, 84333.898777f, 84334.757411f, 84335.614437f, 84336.469562f, 84337.323462f, 84338.176940f, 84339.030794f, 84339.885826f, 84340.742847f, 84341.602163f, 84342.462912f, 84343.324105f, 84344.184754f, 84345.043869f, 84345.900460f, 84346.754272f, 84347.606360f, 84348.457891f, 84349.310034f, 84350.163955f, 84351.020758f, 84351.880382f, 84352.741781f, 84353.603885f, 84354.465619f, 84355.325914f, 84356.183823f, 84357.039683f, 84357.894298f, 84358.748434f, 84359.602858f, 84360.458337f, 84361.315548f, 84362.174273f, 84363.033895f, 84363.893810f, 84364.753413f, -84365.612099f, 84366.469344f, 84367.325343f, 84368.180590f, 84369.035571f, 84369.890771f, 84370.746678f, 84371.603691f, 84372.461602f, 84373.320046f, 84374.178665f, 84375.037104f, 84375.895006f, 84376.752079f, 84377.608474f, 84378.464476f, 84379.320370f, 84380.176438f, 84381.032962f, 84381.890166f, 84382.747915f, 84383.605986f, 84384.464161f, 84385.322220f, 84386.179945f, 84387.037173f, 84387.894024f, 84388.750661f, 84389.607246f, 84390.463940f, 84391.320907f, 84392.178262f, 84393.035919f, 84393.893750f, 84394.751628f, 84395.609427f, 84396.467019f, 84397.324336f, 84398.181457f, 84399.038425f, 84399.895281f, 84400.752065f, 84401.608818f, 84402.465582f, 84403.322396f, 84404.179302f, 84405.036340f, 84405.893552f, 84406.750978f, 84407.608669f, -84408.466619f, 84409.324771f, 84410.183067f, 84411.041450f, 84411.899861f, 84412.758242f, 84413.616536f, 84414.474684f, 84415.332628f, 84416.190306f, 84417.047448f, 84417.904057f, 84418.760311f, 84419.616388f, 84420.472465f, 84421.328719f, 84422.185329f, 84423.042473f, 84423.900328f, 84424.759072f, 84425.619024f, 84426.481079f, 84427.344414f, 84428.207872f, 84429.070296f, 84429.930529f, 84430.788209f, 84431.644147f, 84432.499080f, 84433.353744f, 84434.208877f, 84435.064938f, 84435.921383f, 84436.777964f, 84437.634472f, 84438.490698f, 84439.346420f, 84440.201438f, 84441.056026f, 84441.910553f, 84442.765390f, 84443.620905f, 84444.477234f, 84445.334118f, 84446.191375f, 84447.048826f, 84447.906292f, 84448.763659f, 84449.621102f, 84450.478631f, -84451.336227f, 84452.193874f, 84453.051555f, 84453.909304f, 84454.767128f, 84455.625009f, 84456.482927f, 84457.340864f, 84458.198803f, 84459.056724f, 84459.914609f, 84460.772439f, 84461.630197f, 84462.487863f, 84463.345431f, 84464.202921f, 84465.060352f, 84465.917748f, 84466.775127f, 84467.632513f, 84468.489925f, 84469.347385f, 84470.204914f, 84471.062533f, 84471.920266f, 84472.778116f, 84473.636056f, 84474.494060f, 84475.352099f, 84476.210147f, 84477.068177f, 84477.926162f, 84478.784074f, 84479.641887f, 84480.499573f, 84481.357098f, 84482.214471f, 84483.071731f, 84483.928913f, 84484.786056f, 84485.643197f, 84486.500373f, 84487.357621f, 84488.214979f, 84489.072483f, 84489.930173f, 84490.788081f, 84491.646175f, 84492.504406f, 84493.362728f, -84494.221094f, 84495.079457f, 84495.937769f, 84496.795984f, 84497.654054f, 84498.511933f, 84499.369570f, 84500.226944f, 84501.084109f, 84501.941123f, 84502.798045f, 84503.654933f, 84504.511846f, 84505.368842f, 84506.225980f, 84507.083319f, 84507.940916f, 84508.798831f, 84509.657054f, 84510.515516f, 84511.374148f, 84512.232882f, 84513.091649f, 84513.950380f, 84514.809008f, 84515.667462f, 84516.525675f, 84517.383577f, 84518.241122f, 84519.098357f, 84519.955356f, 84520.812193f, 84521.668939f, 84522.525670f, 84523.382458f, 84524.239375f, 84525.096496f, 84525.953894f, 84526.811638f, 84527.669726f, 84528.528092f, 84529.386671f, 84530.245401f, 84531.104215f, 84531.963051f, 84532.821843f, 84533.680528f, 84534.539040f, 84535.397317f, 84536.255296f, -84537.112982f, 84537.970441f, 84538.827741f, 84539.684949f, 84540.542133f, 84541.399361f, 84542.256700f, 84543.114218f, 84543.971983f, 84544.830062f, 84545.688672f, 84546.547939f, 84547.407651f, 84548.267593f, 84549.127550f, 84549.987307f, 84550.846649f, 84551.705360f, 84552.563227f, 84553.420008f, 84554.275013f, 84555.128473f, 84555.980999f, 84556.833206f, 84557.685706f, 84558.539113f, 84559.394039f, 84560.251098f, 84561.111048f, 84561.973881f, 84562.838619f, 84563.704276f, 84564.569861f, 84565.434386f, 84566.296861f, 84567.156319f, 84568.012641f, 84568.866664f, 84569.719255f, 84570.571281f, 84571.423610f, 84572.277108f, 84573.132644f, 84573.990853f, 84574.851213f, 84575.712954f, 84576.575317f, 84577.437540f, 84578.298862f, 84579.158529f, -84580.016493f, 84580.873337f, 84581.729482f, 84582.585353f, 84583.441370f, 84584.297956f, 84585.155471f, 84586.013678f, 84586.872320f, 84587.731198f, 84588.590112f, 84589.448860f, 84590.307243f, 84591.165128f, 84592.022639f, 84592.879929f, 84593.737152f, 84594.594460f, 84595.452004f, 84596.309938f, 84597.168345f, 84598.027100f, 84598.886042f, 84599.745009f, 84600.603838f, 84601.462368f, 84602.320435f, 84603.178003f, 84604.035255f, 84604.892387f, 84605.749595f, 84606.607077f, 84607.465029f, 84608.324161f, 84609.184816f, 84610.046054f, 84610.906919f, 84611.766456f, 84612.623711f, 84613.477537 -}; +const float ADA_SIMULATED_PRESSURE[DATA_SIZE] = { + 85431.242948f, 85431.195530f, 85430.531818f, 85428.511661f, 85424.541375f, + 85418.586756f, 85410.484742f, 85400.380605f, 85387.951291f, 85373.412847f, + 85356.530561f, 85337.302634f, 85315.693843f, 85291.654118f, 85265.156486f, + 85236.179384f, 85204.699993f, 85170.715288f, 85134.214699f, 85095.204539f, + 85053.713878f, 85009.692280f, 84963.342580f, 84914.537170f, 84863.342981f, + 84809.942661f, 84754.307845f, 84696.606779f, 84636.840947f, 84575.108256f, + 84511.411803f, 84445.886248f, 84378.531812f, 84309.408539f, 84238.575457f, + 84166.083903f, 84091.993574f, 84016.326350f, 83939.283495f, 83860.838930f, + 83781.113349f, 83700.192751f, 83618.160747f, 83535.097047f, 83451.069870f, + 83366.139284f, 83280.348743f, 83193.731035f, 83106.324146f, 83018.168763f, + 82929.332706f, 82839.907721f, 82749.980821f, 82659.659037f, 82569.047818f, + 82478.250367f, 82387.372687f, 82296.514121f, 82205.765806f, 82115.204232f, + 82024.866544f, 81934.773692f, 81844.948233f, 81755.416882f, 81666.211710f, + 81577.355814f, 81488.856279f, 81400.744909f, 81313.024685f, 81225.725720f, + 81138.858951f, 81052.432671f, 80966.442916f, 80880.879969f, 80795.733506f, + 80710.993461f, 80626.652792f, 80542.709300f, 80459.154952f, 80376.001162f, + 80293.249194f, 80210.902104f, 80128.967362f, 80047.445669f, 79966.333243f, + 79885.651990f, 79805.357504f, 79725.473811f, 79645.974136f, 79566.865375f, + 79488.136895f, 79409.786770f, 79331.813688f, 79254.217678f, 79177.018164f, + 79100.171172f, 79023.724092f, 78947.664215f, 78871.990160f, 78796.711133f, + 78721.810653f, 78647.294473f, 78573.155658f, 78499.389033f, 78425.989137f, + 78352.953270f, 78280.274531f, 78207.954674f, 78135.994226f, 78064.391522f, + 77993.148588f, 77922.267607f, 77851.747240f, 77781.592871f, 77711.801941f, + 77642.368219f, 77573.321504f, 77504.601044f, 77436.249004f, 77368.249493f, + 77300.601718f, 77233.292329f, 77166.332527f, 77099.709056f, 77033.429981f, + 76967.488574f, 76901.887521f, 76836.627276f, 76771.707698f, 76707.131456f, + 76642.893513f, 76578.999459f, 76515.448480f, 76452.236220f, 76389.362923f, + 76326.825701f, 76264.622110f, 76202.749523f, 76141.205158f, 76079.986697f, + 76019.092066f, 75958.519777f, 75898.268885f, 75838.338743f, 75778.729023f, + 75719.439688f, 75660.470760f, 75601.822193f, 75543.497084f, 75485.490333f, + 75427.807225f, 75370.444249f, 75313.396464f, 75256.688784f, 75200.282025f, + 75144.172875f, 75088.392160f, 75032.925014f, 74977.764900f, 74922.913295f, + 74868.368873f, 74814.122531f, 74760.186468f, 74706.552169f, 74653.219692f, + 74600.189078f, 74547.460475f, 74495.032604f, 74442.907385f, 74391.081048f, + 74339.553306f, 74288.330025f, 74237.404938f, 74186.779260f, 74136.451930f, + 74086.421993f, 74036.688644f, 73987.249596f, 73938.088136f, 73889.284787f, + 73840.731349f, 73792.440376f, 73744.447246f, 73696.757821f, 73649.353837f, + 73602.227122f, 73555.394377f, 73508.847207f, 73462.582908f, 73416.597304f, + 73370.890105f, 73325.473076f, 73280.337506f, 73235.480219f, 73190.905435f, + 73146.613374f, 73102.596175f, 73058.860540f, 73015.406133f, 72972.227565f, + 72929.331987f, 72886.711212f, 72844.367669f, 72802.299644f, 72760.507265f, + 72718.988304f, 72677.743118f, 72636.769630f, 72596.068004f, 72555.636476f, + 72515.476024f, 72475.578873f, 72435.952273f, 72396.594244f, 72357.501959f, + 72318.674868f, 72280.113029f, 72241.815580f, 72203.782733f, 72166.013100f, + 72128.489391f, 72091.283419f, 72054.315012f, 72017.574437f, 71981.104889f, + 71944.909063f, 71908.971195f, 71873.295478f, 71837.881367f, 71802.722807f, + 71767.823448f, 71733.185331f, 71698.801255f, 71664.673274f, 71630.804807f, + 71597.189691f, 71563.827456f, 71530.721966f, 71497.870293f, 71465.268268f, + 71432.919826f, 71400.825373f, 71368.977961f, 71337.381166f, 71306.038072f, + 71274.941011f, 71244.091733f, 71213.494853f, 71183.144392f, 71153.038987f, + 71123.184376f, 71093.579791f, 71064.218686f, 71035.105353f, 71006.241439f, + 70977.618605f, 70949.241026f, 70921.113901f, 70893.229961f, 70865.588676f, + 70838.193911f, 70811.040889f, 70784.126431f, 70757.456377f, 70731.031335f, + 70704.843240f, 70678.896801f, 70653.195521f, 70627.730598f, 70602.504219f, + 70577.521085f, 70552.773924f, 70528.261970f, 70503.991199f, 70479.959315f, + 70456.159982f, 70432.598952f, 70409.278082f, 70386.187944f, 70363.333297f, + 70340.714922f, 70318.292187f, 70296.120757f, 70274.282314f, 70252.696300f, + 70231.274495f, 70210.039972f, 70189.015806f, 70168.225068f, 70147.690833f, + 70127.426153f, 70107.387697f, 70087.570081f, 70067.976772f, 70048.611234f, + 70029.476936f, 70010.577343f, 69991.908539f, 69973.465230f, 69955.251249f, + 69937.270455f, 69919.526706f, 69902.023810f, 69884.743721f, 69867.675255f, + 69850.826821f, 69834.206826f, 69817.823677f, 69801.685204f, 69785.766631f, + 69770.057621f, 69754.567323f, 69739.304886f, 69724.279457f, 69709.498429f, + 69694.931510f, 69680.572196f, 69666.430488f, 69652.516390f, 69638.839902f, + 69625.407251f, 69612.182849f, 69599.164123f, 69586.362097f, 69573.787799f, + 69561.452254f, 69549.359575f, 69537.469102f, 69525.782350f, 69514.311645f, + 69503.069316f, 69492.067690f, 69481.307561f, 69470.743620f, 69460.381548f, + 69450.235154f, 69440.318249f, 69430.644643f, 69421.209926f, 69411.964697f, + 69402.919186f, 69394.089247f, 69385.490733f, 69377.139497f, 69368.999277f, + 69360.982592f, 69353.151977f, 69345.576786f, 69338.326370f, 69331.470085f, + 69324.939731f, 69318.528062f, 69312.244507f, 69306.103083f, 69300.117802f, + 69294.302680f, 69288.671733f, 69283.238974f, 69278.018419f, 69273.024082f, + 69268.269979f, 69263.770057f, 69259.496465f, 69255.430429f, 69251.590144f, + 69247.993806f, 69244.659609f, 69241.603360f, 69238.702076f, 69235.921723f, + 69233.323021f, 69230.966692f, 69228.913456f, 69227.212789f, 69225.658652f, + 69224.199438f, 69222.896700f, 69221.811991f, 69221.006867f, 69220.521365f, + 69220.165707f, 69219.980730f, 69220.043787f, 69220.291628f, 69220.700570f, + 69221.255032f, 69221.948886f, 69222.770099f, 69223.718588f, 69224.789886f, + 69225.981368f, 69227.290896f, 69228.716654f, 69230.257039f, 69231.910574f, + 69233.675843f, 69235.549975f, 69237.540004f, 69239.632469f, 69241.824949f, + 69244.129000f, 69246.532391f, 69249.039408f, 69251.642883f, 69254.345106f, + 69257.140414f, 69260.029163f, 69263.007155f, 69266.072861f, 69269.223550f, + 69272.450692f, 69275.766748f, 69279.168952f, 69282.635802f, 69286.165981f, + 69289.758352f, 69293.417545f, 69297.149941f, 69300.944975f, 69304.791543f, + 69308.685239f, 69312.638698f, 69316.644437f, 69320.693092f, 69324.778818f, + 69328.912099f, 69333.088079f, 69337.299064f, 69341.538963f, 69345.815661f, + 69350.126596f, 69354.465583f, 69358.826867f, 69363.213771f, 69367.627361f, + 69372.065912f, 69376.527678f, 69381.009650f, 69385.509511f, 69390.025756f, + 69394.556883f, 69399.101390f, 69403.657774f, 69408.224532f, 69412.800161f, + 69417.386881f, 69421.988651f, 69426.603633f, 69431.229929f, 69435.865639f, + 69440.508865f, 69445.157708f, 69449.810267f, 69454.466317f, 69459.130478f, + 69463.802243f, 69468.480551f, 69473.164343f, 69477.852558f, 69482.544136f, + 69487.238016f, 69491.933450f, 69496.632799f, 69501.336372f, 69506.043658f, + 69510.754144f, 69515.467316f, 69520.182662f, 69524.899670f, 69529.617835f, + 69534.337496f, 69539.058979f, 69543.782169f, 69548.506952f, 69553.233217f, + 69557.960848f, 69562.689732f, 69567.419756f, 69572.150806f, 69576.882769f, + 69581.615530f, 69586.348978f, 69591.082997f, 69595.817475f, 69600.552298f, + 69605.287352f, 69610.022715f, 69614.759045f, 69619.496350f, 69624.234558f, + 69628.973600f, 69633.713404f, 69638.453901f, 69643.195021f, 69647.936692f, + 69652.678846f, 69657.421410f, 69662.164316f, 69666.907492f, 69671.650869f, + 69676.394376f, 69681.137942f, 69685.881502f, 69690.625429f, 69695.369961f, + 69700.115033f, 69704.860577f, 69709.606527f, 69714.352818f, 69719.099383f, + 69723.846155f, 69728.593069f, 69733.340058f, 69738.087056f, 69742.833997f, + 69747.580814f, 69752.327441f, 69757.073812f, 69761.819861f, 69766.565581f, + 69771.311194f, 69776.056732f, 69780.802202f, 69785.547609f, 69790.292960f, + 69795.038260f, 69799.783515f, 69804.528732f, 69809.273916f, 69814.019073f, + 69818.764210f, 69823.509332f, 69828.254446f, 69832.999556f, 69837.744671f, + 69842.489792f, 69847.234634f, 69851.979048f, 69856.723127f, 69861.466963f, + 69866.210649f, 69870.954278f, 69875.697942f, 69880.441734f, 69885.185746f, + 69889.930071f, 69894.674803f, 69899.420033f, 69904.165854f, 69908.912359f, + 69913.659640f, 69918.407791f, 69923.156846f, 69927.906588f, 69932.656953f, + 69937.407909f, 69942.159422f, 69946.911459f, 69951.663986f, 69956.416970f, + 69961.170378f, 69965.924176f, 69970.678330f, 69975.432809f, 69980.187577f, + 69984.942602f, 69989.697851f, 69994.453289f, 69999.208886f, 70003.965286f, + 70008.722876f, 70013.481460f, 70018.240838f, 70023.000812f, 70027.761183f, + 70032.521754f, 70037.282324f, 70042.042696f, 70046.802672f, 70051.562052f, + 70056.320639f, 70061.078234f, 70065.834638f, 70070.589652f, 70075.343079f, + 70080.094733f, 70084.844644f, 70089.593001f, 70094.340001f, 70099.085838f, + 70103.830707f, 70108.574805f, 70113.318325f, 70118.061463f, 70122.804415f, + 70127.547375f, 70132.290539f, 70137.034101f, 70141.778258f, 70146.523203f, + 70151.269133f, 70156.016240f, 70160.763002f, 70165.508278f, 70170.252467f, + 70174.995967f, 70179.739176f, 70184.482494f, 70189.226319f, 70193.971048f, + 70198.717081f, 70203.464816f, 70208.214651f, 70212.966986f, 70217.722217f, + 70222.480744f, 70227.242966f, 70232.009280f, 70236.780549f, 70241.559099f, + 70246.344308f, 70251.135131f, 70255.930525f, 70260.729445f, 70265.530846f, + 70270.333684f, 70275.136914f, 70279.939493f, 70284.740376f, 70289.538518f, + 70294.332875f, 70299.122403f, 70303.906057f, 70308.682792f, 70313.451565f, + 70318.210776f, 70322.961124f, 70327.704896f, 70332.444377f, 70337.181855f, + 70341.919615f, 70346.659944f, 70351.403715f, 70356.146580f, 70360.888784f, + 70365.631222f, 70370.374788f, 70375.120377f, 70379.868883f, 70384.621204f, + 70389.377900f, 70394.138254f, 70398.901278f, 70403.665987f, 70408.431393f, + 70413.196509f, 70417.960348f, 70422.722605f, 70427.484595f, 70432.246456f, + 70437.008245f, 70441.770019f, 70446.531833f, 70451.293746f, 70456.055791f, + 70460.817692f, 70465.579468f, 70470.341273f, 70475.103255f, 70479.865567f, + 70484.628360f, 70489.391785f, 70494.155787f, 70498.920090f, 70503.684684f, + 70508.449560f, 70513.214712f, 70517.980132f, 70522.745814f, 70527.511750f, + 70532.277933f, 70537.044357f, 70541.811012f, 70546.577894f, 70551.344994f, + 70556.112306f, 70560.879821f, 70565.647616f, 70570.415747f, 70575.184189f, + 70579.952916f, 70584.721900f, 70589.491115f, 70594.260535f, 70599.030133f, + 70603.799882f, 70608.569757f, 70613.339731f, 70618.109777f, 70622.879869f, + 70627.649980f, 70632.420085f, 70637.190258f, 70641.960544f, 70646.730942f, + 70651.501448f, 70656.272057f, 70661.042768f, 70665.813576f, 70670.584478f, + 70675.355471f, 70680.126551f, 70684.897715f, 70689.668960f, 70694.440283f, + 70699.211679f, 70703.983144f, 70708.754661f, 70713.526230f, 70718.297861f, + 70723.069563f, 70727.841343f, 70732.613211f, 70737.385176f, 70742.157246f, + 70746.929430f, 70751.701738f, 70756.474177f, 70761.246756f, 70766.019485f, + 70770.792372f, 70775.565416f, 70780.338544f, 70785.111740f, 70789.885006f, + 70794.658343f, 70799.431753f, 70804.205237f, 70808.978796f, 70813.752432f, + 70818.526146f, 70823.299940f, 70828.073815f, 70832.847772f, 70837.621813f, + 70842.395939f, 70847.170152f, 70851.944453f, 70856.718843f, 70861.493324f, + 70866.267897f, 70871.042564f, 70875.817326f, 70880.592184f, 70885.367140f, + 70890.142195f, 70894.917351f, 70899.692609f, 70904.467970f, 70909.243436f, + 70914.019009f, 70918.794678f, 70923.570417f, 70928.346226f, 70933.122109f, + 70937.898068f, 70942.674106f, 70947.450226f, 70952.226432f, 70957.002725f, + 70961.779110f, 70966.555587f, 70971.332162f, 70976.108835f, 70980.885611f, + 70985.662492f, 70990.439482f, 70995.216582f, 70999.993796f, 71004.771126f, + 71009.548576f, 71014.326149f, 71019.103846f, 71023.881672f, 71028.659629f, + 71033.437720f, 71038.215948f, 71042.994316f, 71047.772826f, 71052.551481f, + 71057.330285f, 71062.109178f, 71066.888106f, 71071.667081f, 71076.446116f, + 71081.225223f, 71086.004414f, 71090.783700f, 71095.563095f, 71100.342611f, + 71105.122259f, 71109.902052f, 71114.682003f, 71119.462122f, 71124.242424f, + 71129.022918f, 71133.803619f, 71138.584538f, 71143.365687f, 71148.147079f, + 71152.928725f, 71157.710638f, 71162.492831f, 71167.275314f, 71172.058101f, + 71176.841204f, 71181.624634f, 71186.408400f, 71191.192411f, 71195.976631f, + 71200.761069f, 71205.545732f, 71210.330628f, 71215.115762f, 71219.901144f, + 71224.686780f, 71229.472677f, 71234.258844f, 71239.045286f, 71243.832012f, + 71248.619028f, 71253.406343f, 71258.193963f, 71262.981896f, 71267.770149f, + 71272.558730f, 71277.347645f, 71282.136902f, 71286.926508f, 71291.716471f, + 71296.506799f, 71301.297505f, 71306.088692f, 71310.880368f, 71315.672508f, + 71320.465086f, 71325.258078f, 71330.051457f, 71334.845199f, 71339.639278f, + 71344.433670f, 71349.228348f, 71354.023288f, 71358.818464f, 71363.613850f, + 71368.409423f, 71373.205156f, 71378.001025f, 71382.797003f, 71387.593066f, + 71392.389188f, 71397.185344f, 71401.981509f, 71406.777799f, 71411.574458f, + 71416.371460f, 71421.168771f, 71425.966357f, 71430.764183f, 71435.562216f, + 71440.360421f, 71445.158765f, 71449.957213f, 71454.755731f, 71459.554286f, + 71464.352843f, 71469.151367f, 71473.949826f, 71478.748185f, 71483.546409f, + 71488.344465f, 71493.142319f, 71497.939936f, 71502.737283f, 71507.534340f, + 71512.331304f, 71517.128243f, 71521.925147f, 71526.722006f, 71531.518810f, + 71536.315549f, 71541.112213f, 71545.908791f, 71550.705274f, 71555.501652f, + 71560.297914f, 71565.094050f, 71569.890051f, 71574.685905f, 71579.481604f, + 71584.277137f, 71589.072494f, 71593.867664f, 71598.662639f, 71603.457407f, + 71608.251958f, 71613.046220f, 71617.840064f, 71622.633531f, 71627.426665f, + 71632.219511f, 71637.012114f, 71641.804518f, 71646.596769f, 71651.388911f, + 71656.180990f, 71660.973049f, 71665.765135f, 71670.557291f, 71675.349563f, + 71680.141995f, 71684.934633f, 71689.727521f, 71694.520703f, 71699.314226f, + 71704.108087f, 71708.901911f, 71713.695641f, 71718.489318f, 71723.282987f, + 71728.076689f, 71732.870468f, 71737.664367f, 71742.458429f, 71747.252696f, + 71752.047211f, 71756.842018f, 71761.637158f, 71766.432676f, 71771.228613f, + 71776.025013f, 71780.821919f, 71785.619373f, 71790.417418f, 71795.216097f, + 71800.015326f, 71804.815001f, 71809.615114f, 71814.415656f, 71819.216618f, + 71824.017992f, 71828.819768f, 71833.621939f, 71838.424496f, 71843.227430f, + 71848.030733f, 71852.834395f, 71857.638408f, 71862.442765f, 71867.247455f, + 71872.052470f, 71876.857802f, 71881.663442f, 71886.469381f, 71891.275687f, + 71896.082577f, 71900.890027f, 71905.697991f, 71910.506424f, 71915.315283f, + 71920.124521f, 71924.934093f, 71929.743956f, 71934.554063f, 71939.364370f, + 71944.174832f, 71948.985404f, 71953.796040f, 71958.606697f, 71963.417329f, + 71968.227891f, 71973.038338f, 71977.848625f, 71982.658734f, 71987.469038f, + 71992.279637f, 71997.090485f, 72001.901534f, 72006.712738f, 72011.524050f, + 72016.335424f, 72021.146812f, 72025.958168f, 72030.769445f, 72035.580596f, + 72040.391575f, 72045.202335f, 72050.012828f, 72054.823009f, 72059.632830f, + 72064.442245f, 72069.251207f, 72074.059668f, 72078.867569f, 72083.674908f, + 72088.481741f, 72093.288126f, 72098.094119f, 72102.899777f, 72107.705157f, + 72112.510317f, 72117.315312f, 72122.120200f, 72126.925037f, 72131.729881f, + 72136.534788f, 72141.339816f, 72146.145021f, 72150.950460f, 72155.756190f, + 72160.562268f, 72165.368750f, 72170.175583f, 72174.982287f, 72179.788847f, + 72184.595317f, 72189.401750f, 72194.208201f, 72199.014722f, 72203.821369f, + 72208.628194f, 72213.435253f, 72218.242597f, 72223.050283f, 72227.858362f, + 72232.666889f, 72237.475919f, 72242.285504f, 72247.095698f, 72251.906556f, + 72256.718132f, 72261.530476f, 72266.343543f, 72271.157270f, 72275.971621f, + 72280.786563f, 72285.602063f, 72290.418086f, 72295.234598f, 72300.051565f, + 72304.868953f, 72309.686729f, 72314.504859f, 72319.323307f, 72324.142042f, + 72328.961028f, 72333.780231f, 72338.599618f, 72343.419155f, 72348.238973f, + 72353.059414f, 72357.880430f, 72362.701953f, 72367.523918f, 72372.346259f, + 72377.168908f, 72381.991799f, 72386.814866f, 72391.638043f, 72396.461262f, + 72401.284458f, 72406.107564f, 72410.930514f, 72415.753241f, 72420.575679f, + 72425.397761f, 72430.219442f, 72435.040970f, 72439.862429f, 72444.683816f, + 72449.505130f, 72454.326366f, 72459.147523f, 72463.968598f, 72468.789588f, + 72473.610490f, 72478.431301f, 72483.252019f, 72488.072641f, 72492.893164f, + 72497.713586f, 72502.533903f, 72507.354113f, 72512.174213f, 72516.994113f, + 72521.813708f, 72526.633037f, 72531.452145f, 72536.271075f, 72541.089868f, + 72545.908567f, 72550.727216f, 72555.545857f, 72560.364533f, 72565.183287f, + 72570.002162f, 72574.821199f, 72579.640443f, 72584.459936f, 72589.279721f, + 72594.099839f, 72598.920293f, 72603.740818f, 72608.561381f, 72613.382010f, + 72618.202730f, 72623.023566f, 72627.844546f, 72632.665693f, 72637.487036f, + 72642.308599f, 72647.130408f, 72651.952490f, 72656.774869f, 72661.597573f, + 72666.420627f, 72671.244056f, 72676.067888f, 72680.892147f, 72685.716825f, + 72690.541887f, 72695.367317f, 72700.193096f, 72705.019209f, 72709.845637f, + 72714.672365f, 72719.499375f, 72724.326649f, 72729.154171f, 72733.981924f, + 72738.809891f, 72743.638054f, 72748.466397f, 72753.294903f, 72758.123554f, + 72762.952333f, 72767.781278f, 72772.610591f, 72777.440259f, 72782.270248f, + 72787.100521f, 72791.931042f, 72796.761774f, 72801.592682f, 72806.423728f, + 72811.254878f, 72816.086093f, 72820.917340f, 72825.748580f, 72830.579779f, + 72835.410899f, 72840.241904f, 72845.072758f, 72849.903428f, 72854.734005f, + 72859.564554f, 72864.395078f, 72869.225584f, 72874.056075f, 72878.886556f, + 72883.717032f, 72888.547507f, 72893.377987f, 72898.208475f, 72903.038977f, + 72907.869497f, 72912.700039f, 72917.530609f, 72922.361211f, 72927.191849f, + 72932.022528f, 72936.853160f, 72941.683533f, 72946.513691f, 72951.343691f, + 72956.173591f, 72961.003449f, 72965.833323f, 72970.663270f, 72975.493348f, + 72980.323615f, 72985.154128f, 72989.984944f, 72994.816122f, 72999.647719f, + 73004.479793f, 73009.312401f, 73014.145601f, 73018.979440f, 73023.813777f, + 73028.648541f, 73033.483715f, 73038.319285f, 73043.155234f, 73047.991547f, + 73052.828208f, 73057.665201f, 73062.502510f, 73067.340120f, 73072.178015f, + 73077.016179f, 73081.854596f, 73086.693250f, 73091.532127f, 73096.371209f, + 73101.210482f, 73106.050256f, 73110.890979f, 73115.732519f, 73120.574736f, + 73125.417489f, 73130.260639f, 73135.104046f, 73139.947568f, 73144.791067f, + 73149.634402f, 73154.477433f, 73159.320019f, 73164.162021f, 73169.003299f, + 73173.843712f, 73178.683121f, 73183.521384f, 73188.358382f, 73193.194215f, + 73198.029013f, 73202.862887f, 73207.695948f, 73212.528308f, 73217.360077f, + 73222.191366f, 73227.022287f, 73231.852950f, 73236.683467f, 73241.513948f, + 73246.344504f, 73251.175247f, 73256.006288f, 73260.837737f, 73265.669705f, + 73270.502305f, 73275.334345f, 73280.164808f, 73284.994041f, 73289.822393f, + 73294.650211f, 73299.477844f, 73304.305640f, 73309.133947f, 73313.963114f, + 73318.793488f, 73323.625417f, 73328.459250f, 73333.295335f, 73338.134020f, + 73342.975653f, 73347.820583f, 73352.669157f, 73357.522081f, 73362.380888f, + 73367.245056f, 73372.113824f, 73376.986432f, 73381.862119f, 73386.740124f, + 73391.619688f, 73396.500050f, 73401.380449f, 73406.260126f, 73411.138318f, + 73416.014268f, 73420.887212f, 73425.756392f, 73430.621047f, 73435.480417f, + 73440.333741f, 73445.180735f, 73450.022264f, 73454.859370f, 73459.693090f, + 73464.524466f, 73469.354536f, 73474.184340f, 73479.014918f, 73483.846776f, + 73488.676493f, 73493.504087f, 73498.330582f, 73503.156999f, 73507.984361f, + 73512.813690f, 73517.646006f, 73522.482334f, 73527.323468f, 73532.168877f, + 73537.017691f, 73541.869051f, 73546.722098f, 73551.575975f, 73556.429822f, + 73561.282783f, 73566.133997f, 73570.984173f, 73575.834544f, 73580.684945f, + 73585.535213f, 73590.385185f, 73595.234695f, 73600.083579f, 73604.931675f, + 73609.778780f, 73614.624217f, 73619.468190f, 73624.311232f, 73629.153874f, + 73633.996649f, 73638.840088f, 73643.684723f, 73648.531086f, 73653.379555f, + 73658.229608f, 73663.080935f, 73667.933296f, 73672.786453f, 73677.640169f, + 73682.494205f, 73687.348323f, 73692.202285f, 73697.056243f, 73701.910723f, + 73706.765604f, 73711.620754f, 73716.476038f, 73721.331325f, 73726.186480f, + 73731.041369f, 73735.895863f, 73740.749980f, 73745.603839f, 73750.457513f, + 73755.311076f, 73760.164601f, 73765.018162f, 73769.871832f, 73774.725685f, + 73779.579764f, 73784.433840f, 73789.287873f, 73794.141885f, 73798.995897f, + 73803.849933f, 73808.704013f, 73813.558161f, 73818.412398f, 73823.266746f, + 73828.121229f, 73832.975867f, 73837.830684f, 73842.685701f, 73847.540940f, + 73852.396424f, 73857.252175f, 73862.108215f, 73866.964500f, 73871.820965f, + 73876.677611f, 73881.534439f, 73886.391448f, 73891.248640f, 73896.106016f, + 73900.963574f, 73905.821317f, 73910.679245f, 73915.537358f, 73920.395656f, + 73925.254141f, 73930.112812f, 73934.971671f, 73939.830717f, 73944.689952f, + 73949.549363f, 73954.408889f, 73959.268536f, 73964.128319f, 73968.988254f, + 73973.848355f, 73978.708639f, 73983.569120f, 73988.429814f, 73993.290735f, + 73998.151900f, 74003.013324f, 74007.875021f, 74012.737007f, 74017.599297f, + 74022.461908f, 74027.324853f, 74032.188148f, 74037.051836f, 74041.915921f, + 74046.780383f, 74051.645199f, 74056.510348f, 74061.375810f, 74066.241561f, + 74071.107582f, 74075.973849f, 74080.840343f, 74085.707041f, 74090.573923f, + 74095.440965f, 74100.308148f, 74105.175449f, 74110.042848f, 74114.910322f, + 74119.777850f, 74124.645411f, 74129.512983f, 74134.380545f, 74139.248075f, + 74144.115552f, 74148.982955f, 74153.850261f, 74158.717450f, 74163.584500f, + 74168.451389f, 74173.318096f, 74178.184600f, 74183.050879f, 74187.916911f, + 74192.782676f, 74197.648152f, 74202.513317f, 74207.378183f, 74212.242787f, + 74217.107161f, 74221.971341f, 74226.835359f, 74231.699251f, 74236.563049f, + 74241.426789f, 74246.290503f, 74251.154226f, 74256.017992f, 74260.881834f, + 74265.745788f, 74270.609886f, 74275.474029f, 74280.337947f, 74285.201690f, + 74290.065322f, 74294.928907f, 74299.792511f, 74304.656197f, 74309.520029f, + 74314.384073f, 74319.248393f, 74324.113053f, 74328.978118f, 74333.843652f, + 74338.709720f, 74343.576376f, 74348.443473f, 74353.310936f, 74358.178754f, + 74363.046920f, 74367.915425f, 74372.784261f, 74377.653420f, 74382.522893f, + 74387.392672f, 74392.262748f, 74397.133113f, 74402.003759f, 74406.874677f, + 74411.745859f, 74416.617297f, 74421.488996f, 74426.360962f, 74431.233206f, + 74436.105735f, 74440.978557f, 74445.851682f, 74450.725118f, 74455.598873f, + 74460.472956f, 74465.347375f, 74470.222139f, 74475.097256f, 74479.972736f, + 74484.848596f, 74489.725025f, 74494.602053f, 74499.479634f, 74504.357721f, + 74509.236268f, 74514.115229f, 74518.994557f, 74523.874205f, 74528.754128f, + 74533.634278f, 74538.514610f, 74543.395076f, 74548.275631f, 74553.156228f, + 74558.036820f, 74562.917361f, 74567.797805f, 74572.678106f, 74577.558216f, + 74582.438089f, 74587.317679f, 74592.196940f, 74597.075824f, 74601.954287f, + 74606.832280f, 74611.709758f, 74616.586674f, 74621.462983f, 74626.338621f, + 74631.213353f, 74636.087232f, 74640.960419f, 74645.833073f, 74650.705354f, + 74655.577422f, 74660.449437f, 74665.321559f, 74670.193947f, 74675.066761f, + 74679.940162f, 74684.813894f, 74689.687548f, 74694.561202f, 74699.434938f, + 74704.308838f, 74709.182983f, 74714.057453f, 74718.932331f, 74723.807698f, + 74728.683635f, 74733.560224f, 74738.437524f, 74743.315473f, 74748.194007f, + 74753.073068f, 74757.952600f, 74762.832544f, 74767.712844f, 74772.593441f, + 74777.474280f, 74782.355302f, 74787.236451f, 74792.117676f, 74796.999141f, + 74801.880908f, 74806.762954f, 74811.645255f, 74816.527787f, 74821.410527f, + 74826.293451f, 74831.176536f, 74836.059758f, 74840.943094f, 74845.826520f, + 74850.710017f, 74855.593586f, 74860.477232f, 74865.360956f, 74870.244764f, + 74875.128658f, 74880.012641f, 74884.896718f, 74889.780891f, 74894.665165f, + 74899.549542f, 74904.434026f, 74909.318621f, 74914.203330f, 74919.088156f, + 74923.973104f, 74928.858175f, 74933.743375f, 74938.628706f, 74943.514172f, + 74948.399777f, 74953.285523f, 74958.171414f, 74963.057435f, 74967.943578f, + 74972.829842f, 74977.716227f, 74982.602735f, 74987.489364f, 74992.376115f, + 74997.262988f, 75002.149983f, 75007.037100f, 75011.924339f, 75016.811700f, + 75021.699183f, 75026.586789f, 75031.474517f, 75036.362368f, 75041.250341f, + 75046.138437f, 75051.026655f, 75055.914997f, 75060.803461f, 75065.692047f, + 75070.580752f, 75075.469542f, 75080.358418f, 75085.247385f, 75090.136450f, + 75095.025618f, 75099.914897f, 75104.804291f, 75109.693807f, 75114.583451f, + 75119.473229f, 75124.363148f, 75129.253213f, 75134.143430f, 75139.033806f, + 75143.924347f, 75148.815058f, 75153.705947f, 75158.597018f, 75163.488279f, + 75168.379735f, 75173.271392f, 75178.163256f, 75183.055295f, 75187.947425f, + 75192.839644f, 75197.731952f, 75202.624352f, 75207.516844f, 75212.409430f, + 75217.302111f, 75222.194888f, 75227.087762f, 75231.980735f, 75236.873809f, + 75241.766983f, 75246.660260f, 75251.553641f, 75256.447127f, 75261.340720f, + 75266.234419f, 75271.128228f, 75276.022147f, 75280.916178f, 75285.810321f, + 75290.704578f, 75295.598950f, 75300.493439f, 75305.388045f, 75310.282770f, + 75315.177616f, 75320.072583f, 75324.967673f, 75329.862887f, 75334.758226f, + 75339.653692f, 75344.549286f, 75349.445009f, 75354.340862f, 75359.236847f, + 75364.132965f, 75369.029217f, 75373.925604f, 75378.822128f, 75383.718790f, + 75388.615591f, 75393.512533f, 75398.409616f, 75403.306843f, 75408.204223f, + 75413.101755f, 75417.999438f, 75422.897270f, 75427.795248f, 75432.693371f, + 75437.591637f, 75442.490043f, 75447.388588f, 75452.287269f, 75457.186085f, + 75462.085034f, 75466.984113f, 75471.883320f, 75476.782653f, 75481.682111f, + 75486.581692f, 75491.481392f, 75496.381211f, 75501.281146f, 75506.181196f, + 75511.081357f, 75515.981629f, 75520.882008f, 75525.782494f, 75530.683084f, + 75535.583776f, 75540.484568f, 75545.385458f, 75550.286443f, 75555.187523f, + 75560.088695f, 75564.989956f, 75569.891305f, 75574.792740f, 75579.694259f, + 75584.595859f, 75589.497540f, 75594.399297f, 75599.301131f, 75604.203038f, + 75609.105016f, 75614.007064f, 75618.909180f, 75623.811361f, 75628.713611f, + 75633.615934f, 75638.518334f, 75643.420810f, 75648.323365f, 75653.225999f, + 75658.128715f, 75663.031513f, 75667.934396f, 75672.837364f, 75677.740419f, + 75682.643562f, 75687.546795f, 75692.450119f, 75697.353536f, 75702.257046f, + 75707.160652f, 75712.064354f, 75716.968155f, 75721.872055f, 75726.776056f, + 75731.680160f, 75736.584367f, 75741.488680f, 75746.393099f, 75751.297626f, + 75756.202263f, 75761.107010f, 75766.011870f, 75770.916843f, 75775.821931f, + 75780.727136f, 75785.632458f, 75790.537900f, 75795.443463f, 75800.349147f, + 75805.254955f, 75810.160888f, 75815.066947f, 75819.973134f, 75824.879450f, + 75829.785897f, 75834.692476f, 75839.599187f, 75844.506034f, 75849.413015f, + 75854.320123f, 75859.227365f, 75864.134750f, 75869.042287f, 75873.949982f, + 75878.857846f, 75883.765886f, 75888.674111f, 75893.582529f, 75898.491149f, + 75903.399978f, 75908.309027f, 75913.218302f, 75918.127813f, 75923.037567f, + 75927.947574f, 75932.857841f, 75937.768377f, 75942.679191f, 75947.590290f, + 75952.501684f, 75957.413381f, 75962.325360f, 75967.237575f, 75972.150028f, + 75977.062721f, 75981.975654f, 75986.888832f, 75991.802254f, 75996.715925f, + 76001.629845f, 76006.544016f, 76011.458441f, 76016.373122f, 76021.288060f, + 76026.203258f, 76031.118718f, 76036.034441f, 76040.950430f, 76045.866687f, + 76050.783213f, 76055.700011f, 76060.617083f, 76065.534430f, 76070.452057f, + 76075.370027f, 76080.288365f, 76085.207055f, 76090.126082f, 76095.045428f, + 76099.965079f, 76104.885016f, 76109.805226f, 76114.725690f, 76119.646394f, + 76124.567320f, 76129.488454f, 76134.409778f, 76139.331276f, 76144.252933f, + 76149.174732f, 76154.096657f, 76159.018692f, 76163.940820f, 76168.863026f, + 76173.785293f, 76178.707606f, 76183.629986f, 76188.552712f, 76193.475811f, + 76198.399245f, 76203.322977f, 76208.246968f, 76213.171182f, 76218.095580f, + 76223.020126f, 76227.944782f, 76232.869509f, 76237.794271f, 76242.719029f, + 76247.643747f, 76252.568387f, 76257.492911f, 76262.417281f, 76267.341460f, + 76272.265410f, 76277.189095f, 76282.112475f, 76287.035514f, 76291.958174f, + 76296.880514f, 76301.802758f, 76306.724912f, 76311.646965f, 76316.568910f, + 76321.490737f, 76326.412437f, 76331.334003f, 76336.255426f, 76341.176696f, + 76346.097806f, 76351.018746f, 76355.939509f, 76360.860084f, 76365.780464f, + 76370.700640f, 76375.620604f, 76380.540346f, 76385.459858f, 76390.379131f, + 76395.298157f, 76400.216927f, 76405.135432f, 76410.053364f, 76414.970514f, + 76419.886971f, 76424.802823f, 76429.718157f, 76434.633061f, 76439.547623f, + 76444.461931f, 76449.376072f, 76454.290134f, 76459.204205f, 76464.118373f, + 76469.032726f, 76473.947351f, 76478.862335f, 76483.777768f, 76488.693737f, + 76493.610328f, 76498.527631f, 76503.445733f, 76508.364722f, 76513.284685f, + 76518.205641f, 76523.126742f, 76528.047782f, 76532.968843f, 76537.890008f, + 76542.811360f, 76547.732984f, 76552.654963f, 76557.577379f, 76562.500317f, + 76567.423859f, 76572.348090f, 76577.273092f, 76582.198949f, 76587.125744f, + 76592.053561f, 76596.982483f, 76601.912593f, 76606.843975f, 76611.776712f, + 76616.710888f, 76621.646586f, 76626.583889f, 76631.523265f, 76636.466040f, + 76641.411982f, 76646.360705f, 76651.311826f, 76656.264961f, 76661.219726f, + 76666.175736f, 76671.132609f, 76676.089959f, 76681.047402f, 76686.004556f, + 76690.961035f, 76695.916456f, 76700.870434f, 76705.822587f, 76710.772528f, + 76715.719876f, 76720.664245f, 76725.605252f, 76730.542511f, 76735.475805f, + 76740.405565f, 76745.332351f, 76750.256722f, 76755.179239f, 76760.100462f, + 76765.020951f, 76769.941267f, 76774.861969f, 76779.783617f, 76784.704984f, + 76789.624638f, 76794.543125f, 76799.460991f, 76804.378781f, 76809.297041f, + 76814.216316f, 76819.137153f, 76824.060097f, 76828.985694f, 76833.914209f, + 76838.845076f, 76843.777952f, 76848.712513f, 76853.648437f, 76858.585403f, + 76863.523086f, 76868.461164f, 76873.399316f, 76878.337217f, 76883.274804f, + 76888.213128f, 76893.152181f, 76898.091776f, 76903.031724f, 76907.971836f, + 76912.911924f, 76917.851800f, 76922.791274f, 76927.730159f, 76932.668283f, + 76937.605782f, 76942.542809f, 76947.479464f, 76952.415848f, 76957.352060f, + 76962.288200f, 76967.224370f, 76972.160667f, 76977.097193f, 76982.034044f, + 76986.970961f, 76991.907785f, 76996.844590f, 77001.781450f, 77006.718439f, + 77011.655631f, 77016.593101f, 77021.530922f, 77026.469169f, 77031.407917f, + 77036.347107f, 77041.286587f, 77046.226369f, 77051.166473f, 77056.106915f, + 77061.047713f, 77065.988883f, 77070.930444f, 77075.872412f, 77080.814805f, + 77085.757642f, 77090.700919f, 77095.644604f, 77100.588667f, 77105.533075f, + 77110.477794f, 77115.422794f, 77120.368041f, 77125.313504f, 77130.259150f, + 77135.204946f, 77140.150861f, 77145.096863f, 77150.042918f, 77154.988995f, + 77159.935061f, 77164.881084f, 77169.827032f, 77174.772872f, 77179.718573f, + 77184.664109f, 77189.609652f, 77194.555273f, 77199.500959f, 77204.446702f, + 77209.392489f, 77214.338311f, 77219.284156f, 77224.230015f, 77229.175877f, + 77234.121730f, 77239.067564f, 77244.013369f, 77248.959134f, 77253.904849f, + 77258.850502f, 77263.796083f, 77268.741581f, 77273.686986f, 77278.632256f, + 77283.577347f, 77288.522284f, 77293.467093f, 77298.411802f, 77303.356436f, + 77308.301023f, 77313.245589f, 77318.190160f, 77323.134763f, 77328.079425f, + 77333.024171f, 77337.969030f, 77342.914026f, 77347.859187f, 77352.804540f, + 77357.750110f, 77362.695924f, 77367.641976f, 77372.588052f, 77377.534133f, + 77382.480243f, 77387.426408f, 77392.372653f, 77397.319003f, 77402.265484f, + 77407.212122f, 77412.158941f, 77417.105968f, 77422.053227f, 77427.000743f, + 77431.948543f, 77436.896651f, 77441.845093f, 77446.793894f, 77451.743080f, + 77456.692676f, 77461.642633f, 77466.592898f, 77471.543465f, 77476.494329f, + 77481.445483f, 77486.396922f, 77491.348640f, 77496.300630f, 77501.252887f, + 77506.205406f, 77511.158179f, 77516.111201f, 77521.064467f, 77526.017971f, + 77530.971705f, 77535.925666f, 77540.879846f, 77545.834240f, 77550.788887f, + 77555.743908f, 77560.699288f, 77565.655000f, 77570.611020f, 77575.567319f, + 77580.523873f, 77585.480654f, 77590.437638f, 77595.394797f, 77600.352106f, + 77605.309538f, 77610.267068f, 77615.224669f, 77620.182314f, 77625.139978f, + 77630.097635f, 77635.055259f, 77640.012833f, 77644.970507f, 77649.928328f, + 77654.886282f, 77659.844357f, 77664.802538f, 77669.760812f, 77674.719167f, + 77679.677588f, 77684.636062f, 77689.594576f, 77694.553117f, 77699.511671f, + 77704.470225f, 77709.428765f, 77714.387278f, 77719.345750f, 77724.304170f, + 77729.262522f, 77734.220805f, 77739.179037f, 77744.137228f, 77749.095387f, + 77754.053525f, 77759.011651f, 77763.969775f, 77768.927906f, 77773.886055f, + 77778.844232f, 77783.802445f, 77788.760706f, 77793.719023f, 77798.677407f, + 77803.635867f, 77808.594413f, 77813.553055f, 77818.511803f, 77823.470633f, + 77828.429370f, 77833.388016f, 77838.346604f, 77843.305166f, 77848.263734f, + 77853.222343f, 77858.181025f, 77863.139812f, 77868.098737f, 77873.057834f, + 77878.017135f, 77882.976673f, 77887.936480f, 77892.896590f, 77897.857036f, + 77902.817849f, 77907.779064f, 77912.740711f, 77917.702687f, 77922.664915f, + 77927.627397f, 77932.590135f, 77937.553131f, 77942.516387f, 77947.479904f, + 77952.443685f, 77957.407732f, 77962.372046f, 77967.336630f, 77972.301485f, + 77977.266613f, 77982.232017f, 77987.197698f, 77992.163657f, 77997.129898f, + 78002.096422f, 78007.063298f, 78012.030684f, 78016.998543f, 78021.966831f, + 78026.935502f, 78031.904509f, 78036.873809f, 78041.843356f, 78046.813103f, + 78051.783007f, 78056.753020f, 78061.723099f, 78066.693197f, 78071.663269f, + 78076.633270f, 78081.603154f, 78086.572876f, 78091.542390f, 78096.511675f, + 78101.480983f, 78106.450381f, 78111.419849f, 78116.389368f, 78121.358917f, + 78126.328477f, 78131.298028f, 78136.267551f, 78141.237026f, 78146.206433f, + 78151.175752f, 78156.144965f, 78161.114051f, 78166.082990f, 78171.051764f, + 78176.020351f, 78180.988734f, 78185.956891f, 78190.924711f, 78195.892113f, + 78200.859153f, 78205.825888f, 78210.792374f, 78215.758668f, 78220.724826f, + 78225.690906f, 78230.656964f, 78235.623057f, 78240.589241f, 78245.555574f, + 78250.522111f, 78255.488910f, 78260.456027f, 78265.423519f, 78270.391443f, + 78275.359855f, 78280.328710f, 78285.297568f, 78290.266419f, 78295.235311f, + 78300.204296f, 78305.173424f, 78310.142747f, 78315.112313f, 78320.082175f, + 78325.052382f, 78330.022985f, 78334.994035f, 78339.965582f, 78344.937676f, + 78349.910370f, 78354.883712f, 78359.857753f, 78364.832545f, 78369.808138f, + 78374.784604f, 78379.761919f, 78384.740018f, 78389.718838f, 78394.698316f, + 78399.678388f, 78404.658992f, 78409.640063f, 78414.621539f, 78419.603356f, + 78424.585451f, 78429.567760f, 78434.550220f, 78439.532768f, 78444.515340f, + 78449.497872f, 78454.480303f, 78459.462567f, 78464.444971f, 78469.428226f, + 78474.412225f, 78479.396829f, 78484.381903f, 78489.367307f, 78494.352905f, + 78499.338558f, 78504.324131f, 78509.309484f, 78514.294480f, 78519.278983f, + 78524.262854f, 78529.245956f, 78534.228152f, 78539.209303f, 78544.189272f, + 78549.167923f, 78554.145074f, 78559.120287f, 78564.093672f, 78569.065468f, + 78574.035913f, 78579.005247f, 78583.973708f, 78588.941536f, 78593.908969f, + 78598.876245f, 78603.843605f, 78608.811286f, 78613.779527f, 78618.748568f, + 78623.718648f, 78628.690004f, 78633.662877f, 78638.637505f, 78643.614126f, + 78648.592687f, 78653.572855f, 78658.554449f, 78663.537287f, 78668.521188f, + 78673.505972f, 78678.491457f, 78683.477463f, 78688.463809f, 78693.450903f, + 78698.439625f, 78703.429574f, 78708.420316f, 78713.411414f, 78718.402433f, + 78723.392937f, 78728.382490f, 78733.370657f, 78738.357133f, 78743.342297f, + 78748.326438f, 78753.309809f, 78758.292660f, 78763.275246f, 78768.257817f, + 78773.240626f, 78778.223925f, 78783.207879f, 78788.191922f, 78793.175964f, + 78798.160076f, 78803.144330f, 78808.128798f, 78813.113553f, 78818.098666f, + 78823.084210f, 78828.070255f, 78833.056773f, 78838.043704f, 78843.031006f, + 78848.018641f, 78853.006568f, 78857.994747f, 78862.983139f, 78867.971702f, + 78872.960398f, 78877.949252f, 78882.938305f, 78887.927544f, 78892.916956f, + 78897.906529f, 78902.896251f, 78907.886108f, 78912.876090f, 78917.866183f, + 78922.856375f, 78927.846654f, 78932.837007f, 78937.827422f, 78942.817886f, + 78947.808387f, 78952.798912f, 78957.789450f, 78962.779988f, 78967.770528f, + 78972.761121f, 78977.751771f, 78982.742479f, 78987.733245f, 78992.724070f, + 78997.714953f, 79002.705896f, 79007.696898f, 79012.687959f, 79017.679080f, + 79022.670262f, 79027.661504f, 79032.652807f, 79037.644171f, 79042.635597f, + 79047.627084f, 79052.618634f, 79057.610243f, 79062.601861f, 79067.593476f, + 79072.585102f, 79077.576750f, 79082.568435f, 79087.560169f, 79092.551966f, + 79097.543838f, 79102.535797f, 79107.527859f, 79112.520034f, 79117.512337f, + 79122.504780f, 79127.497376f, 79132.490138f, 79137.483080f, 79142.476214f, + 79147.469553f, 79152.463060f, 79157.456664f, 79162.450369f, 79167.444187f, + 79172.438124f, 79177.432191f, 79182.426396f, 79187.420748f, 79192.415255f, + 79197.409928f, 79202.404773f, 79207.399802f, 79212.395021f, 79217.390440f, + 79222.386069f, 79227.381915f, 79232.377988f, 79237.374296f, 79242.370846f, + 79247.367622f, 79252.364616f, 79257.361823f, 79262.359239f, 79267.356858f, + 79272.354674f, 79277.352685f, 79282.350883f, 79287.349265f, 79292.347826f, + 79297.346559f, 79302.345462f, 79307.344527f, 79312.343752f, 79317.343130f, + 79322.342657f, 79327.342327f, 79332.342136f, 79337.342059f, 79342.342089f, + 79347.342237f, 79352.342514f, 79357.342933f, 79362.343504f, 79367.344239f, + 79372.345149f, 79377.346246f, 79382.347542f, 79387.349048f, 79392.350776f, + 79397.352736f, 79402.354941f, 79407.357402f, 79412.360131f, 79417.363138f, + 79422.366436f, 79427.370043f, 79432.373976f, 79437.378223f, 79442.382766f, + 79447.387591f, 79452.392682f, 79457.398023f, 79462.403599f, 79467.409393f, + 79472.415391f, 79477.421576f, 79482.427933f, 79487.434446f, 79492.441100f, + 79497.447878f, 79502.454766f, 79507.461746f, 79512.468805f, 79517.475926f, + 79522.483093f, 79527.490291f, 79532.497504f, 79537.504717f, 79542.511913f, + 79547.519077f, 79552.526193f, 79557.533246f, 79562.540220f, 79567.547100f, + 79572.553869f, 79577.560512f, 79582.567014f, 79587.573358f, 79592.579528f, + 79597.585511f, 79602.591288f, 79607.596846f, 79612.602170f, 79617.607277f, + 79622.612199f, 79627.616971f, 79632.621626f, 79637.626196f, 79642.630716f, + 79647.635218f, 79652.639736f, 79657.644304f, 79662.648954f, 79667.653719f, + 79672.658634f, 79677.663731f, 79682.669034f, 79687.674355f, 79692.679638f, + 79697.684915f, 79702.690217f, 79707.695577f, 79712.701028f, 79717.706599f, + 79722.712325f, 79727.718237f, 79732.724366f, 79737.730745f, 79742.737406f, + 79747.744381f, 79752.751701f, 79757.759372f, 79762.767302f, 79767.775476f, + 79772.783891f, 79777.792544f, 79782.801433f, 79787.810554f, 79792.819903f, + 79797.829479f, 79802.839278f, 79807.849296f, 79812.859532f, 79817.869980f, + 79822.880640f, 79827.891507f, 79832.902559f, 79837.913778f, 79842.925180f, + 79847.936775f, 79852.948578f, 79857.960602f, 79862.972858f, 79867.985362f, + 79872.998124f, 79878.011160f, 79883.024480f, 79888.038100f, 79893.052030f, + 79898.066286f, 79903.080878f, 79908.095801f, 79913.111036f, 79918.126567f, + 79923.142379f, 79928.158456f, 79933.174781f, 79938.191339f, 79943.208113f, + 79948.225087f, 79953.242246f, 79958.259574f, 79963.277054f, 79968.294670f, + 79973.312407f, 79978.330248f, 79983.348178f, 79988.366179f, 79993.384238f, + 79998.402337f, 80003.420460f, 80008.438591f, 80013.456715f, 80018.474815f, + 80023.492876f, 80028.510881f, 80033.528814f, 80038.546659f, 80043.564401f, + 80048.582023f, 80053.599598f, 80058.617209f, 80063.634854f, 80068.652534f, + 80073.670247f, 80078.687993f, 80083.705771f, 80088.723582f, 80093.741424f, + 80098.759298f, 80103.777202f, 80108.795137f, 80113.813101f, 80118.831095f, + 80123.849115f, 80128.867138f, 80133.885165f, 80138.903204f, 80143.921262f, + 80148.939346f, 80153.957465f, 80158.975626f, 80163.993837f, 80169.012105f, + 80174.030439f, 80179.048846f, 80184.067333f, 80189.085909f, 80194.104581f, + 80199.123253f, 80204.141651f, 80209.159795f, 80214.177721f, 80219.195466f, + 80224.213067f, 80229.230562f, 80234.247986f, 80239.265377f, 80244.282771f, + 80249.300206f, 80254.317719f, 80259.335346f, 80264.353124f, 80269.371091f, + 80274.389283f, 80279.407737f, 80284.426489f, 80289.445578f, 80294.465039f, + 80299.484911f, 80304.505228f, 80309.526029f, 80314.547351f, 80319.569229f, + 80324.591702f, 80329.614806f, 80334.638579f, 80339.663056f, 80344.688323f, + 80349.714784f, 80354.742396f, 80359.771002f, 80364.800444f, 80369.830565f, + 80374.861208f, 80379.892217f, 80384.923434f, 80389.954702f, 80394.985864f, + 80400.016762f, 80405.047420f, 80410.078409f, 80415.109719f, 80420.141270f, + 80425.172983f, 80430.204778f, 80435.236577f, 80440.268300f, 80445.299867f, + 80450.331199f, 80455.362216f, 80460.392841f, 80465.423047f, 80470.452930f, + 80475.482539f, 80480.511920f, 80485.541122f, 80490.570190f, 80495.599173f, + 80500.628116f, 80505.657066f, 80510.686072f, 80515.715180f, 80520.744436f, + 80525.773696f, 80530.802837f, 80535.831902f, 80540.860937f, 80545.889985f, + 80550.919093f, 80555.948304f, 80560.977663f, 80566.007215f, 80571.037004f, + 80576.067076f, 80581.097467f, 80586.128031f, 80591.158698f, 80596.189469f, + 80601.220346f, 80606.251332f, 80611.282429f, 80616.313638f, 80621.344962f, + 80626.376403f, 80631.407962f, 80636.439642f, 80641.471445f, 80646.503372f, + 80651.535426f, 80656.567609f, 80661.599923f, 80666.632370f, 80671.664952f, + 80676.697671f, 80681.730528f, 80686.763527f, 80691.796669f, 80696.829956f, + 80701.863387f, 80706.896949f, 80711.930643f, 80716.964468f, 80721.998424f, + 80727.032512f, 80732.066732f, 80737.101083f, 80742.135566f, 80747.170182f, + 80752.204929f, 80757.239809f, 80762.274821f, 80767.309966f, 80772.345243f, + 80777.380654f, 80782.416197f, 80787.451873f, 80792.487682f, 80797.523625f, + 80802.559701f, 80807.595911f, 80812.632254f, 80817.668731f, 80822.705354f, + 80827.742129f, 80832.779053f, 80837.816123f, 80842.853338f, 80847.890693f, + 80852.928187f, 80857.965815f, 80863.003577f, 80868.041467f, 80873.079485f, + 80878.117627f, 80883.155889f, 80888.194270f, 80893.232767f, 80898.271376f, + 80903.310094f, 80908.348920f, 80913.387850f, 80918.426881f, 80923.466011f, + 80928.505237f, 80933.544555f, 80938.583951f, 80943.623347f, 80948.662733f, + 80953.702115f, 80958.741498f, 80963.780889f, 80968.820292f, 80973.859714f, + 80978.899159f, 80983.938633f, 80988.978142f, 80994.017691f, 80999.057286f, + 81004.096932f, 81009.136635f, 81014.176401f, 81019.216234f, 81024.256141f, + 81029.296127f, 81034.336198f, 81039.376358f, 81044.416615f, 81049.456972f, + 81054.497436f, 81059.538012f, 81064.578706f, 81069.619523f, 81074.660469f, + 81079.701549f, 81084.742769f, 81089.784135f, 81094.825651f, 81099.867324f, + 81104.909159f, 81109.951162f, 81114.993337f, 81120.035691f, 81125.078229f, + 81130.120957f, 81135.163880f, 81140.207004f, 81145.250334f, 81150.293876f, + 81155.337636f, 81160.381618f, 81165.425828f, 81170.470273f, 81175.514959f, + 81180.559917f, 81185.605160f, 81190.650687f, 81195.696500f, 81200.742601f, + 81205.788991f, 81210.835672f, 81215.882644f, 81220.929909f, 81225.977468f, + 81231.025323f, 81236.073476f, 81241.121926f, 81246.170676f, 81251.219727f, + 81256.269081f, 81261.318738f, 81266.368700f, 81271.418971f, 81276.469637f, + 81281.520725f, 81286.572213f, 81291.624075f, 81296.676290f, 81301.728832f, + 81306.781679f, 81311.834806f, 81316.888190f, 81321.941807f, 81326.995634f, + 81332.049646f, 81337.103820f, 81342.158133f, 81347.212561f, 81352.267079f, + 81357.321665f, 81362.376294f, 81367.430943f, 81372.485777f, 81377.540930f, + 81382.596371f, 81387.652068f, 81392.707988f, 81397.764100f, 81402.820371f, + 81407.876770f, 81412.933264f, 81417.989820f, 81423.046408f, 81428.102994f, + 81433.159547f, 81438.216035f, 81443.272425f, 81448.328685f, 81453.384784f, + 81458.440688f, 81463.496367f, 81468.551908f, 81473.607472f, 81478.663044f, + 81483.718612f, 81488.774159f, 81493.829671f, 81498.885134f, 81503.940532f, + 81508.995852f, 81514.051078f, 81519.106196f, 81524.161190f, 81529.216048f, + 81534.270752f, 81539.325290f, 81544.379646f, 81549.433806f, 81554.487755f, + 81559.541478f, 81564.594896f, 81569.647876f, 81574.700462f, 81579.752706f, + 81584.804662f, 81589.856383f, 81594.907920f, 81599.959326f, 81605.010655f, + 81610.061958f, 81615.113289f, 81620.164700f, 81625.216243f, 81630.267972f, + 81635.319939f, 81640.372197f, 81645.424798f, 81650.477794f, 81655.531240f, + 81660.585047f, 81665.638733f, 81670.692305f, 81675.745827f, 81680.799361f, + 81685.852970f, 81690.906717f, 81695.960664f, 81701.014874f, 81706.069411f, + 81711.124336f, 81716.179714f, 81721.235606f, 81726.292075f, 81731.349184f, + 81736.406996f, 81741.465575f, 81746.524981f, 81751.585279f, 81756.646519f, + 81761.708641f, 81766.771588f, 81771.835311f, 81776.899762f, 81781.964893f, + 81787.030655f, 81792.097000f, 81797.163880f, 81802.231246f, 81807.299050f, + 81812.367244f, 81817.435779f, 81822.504608f, 81827.573681f, 81832.642951f, + 81837.712369f, 81842.781886f, 81847.851456f, 81852.921108f, 81857.991770f, + 81863.063585f, 81868.136378f, 81873.209978f, 81878.284211f, 81883.358904f, + 81888.433884f, 81893.508978f, 81898.584012f, 81903.658815f, 81908.733212f, + 81913.807032f, 81918.880099f, 81923.952243f, 81929.023288f, 81934.093064f, + 81939.161395f, 81944.228110f, 81949.293031f, 81954.355927f, 81959.416897f, + 81964.476153f, 81969.533910f, 81974.590380f, 81979.645777f, 81984.700313f, + 81989.754203f, 81994.807658f, 81999.860893f, 82004.914121f, 82009.967554f, + 82015.021406f, 82020.075890f, 82025.131219f, 82030.187607f, 82035.245267f, + 82040.304411f, 82045.365252f, 82050.427282f, 82055.490039f, 82060.553587f, + 82065.617989f, 82070.683310f, 82075.749614f, 82080.816963f, 82085.885423f, + 82090.955056f, 82096.026090f, 82101.099383f, 82106.174620f, 82111.251285f, + 82116.328865f, 82121.406843f, 82126.484705f, 82131.561934f, 82136.638017f, + 82141.712437f, 82146.785291f, 82151.857285f, 82156.928546f, 82161.999199f, + 82167.069370f, 82172.139183f, 82177.208763f, 82182.278235f, 82187.347725f, + 82192.417272f, 82197.486065f, 82202.554116f, 82207.621732f, 82212.689223f, + 82217.756897f, 82222.825064f, 82227.894032f, 82232.964110f, 82238.035608f, + 82243.108794f, 82248.183560f, 82253.259586f, 82258.336554f, 82263.414144f, + 82268.492036f, 82273.569910f, 82278.647446f, 82283.724325f, 82288.800246f, + 82293.875547f, 82298.950513f, 82304.025208f, 82309.099696f, 82314.174041f, + 82319.248306f, 82324.322555f, 82329.396853f, 82334.471262f, 82339.545759f, + 82344.620102f, 82349.694334f, 82354.768523f, 82359.842738f, 82364.917048f, + 82369.991521f, 82375.066228f, 82380.141237f, 82385.216615f, 82390.292240f, + 82395.367987f, 82400.443860f, 82405.519860f, 82410.595990f, 82415.672251f, + 82420.748645f, 82425.825176f, 82430.901844f, 82435.978651f, 82441.055601f, + 82446.132695f, 82451.209936f, 82456.287324f, 82461.364863f, 82466.442555f, + 82471.520401f, 82476.598404f, 82481.676566f, 82486.754885f, 82491.833358f, + 82496.911983f, 82501.990759f, 82507.069685f, 82512.148760f, 82517.227984f, + 82522.307355f, 82527.386872f, 82532.466534f, 82537.546341f, 82542.626290f, + 82547.706382f, 82552.786615f, 82557.866988f, 82562.947500f, 82568.028151f, + 82573.108939f, 82578.189862f, 82583.270907f, 82588.352044f, 82593.433284f, + 82598.514633f, 82603.596101f, 82608.677697f, 82613.759429f, 82618.841306f, + 82623.923337f, 82629.005530f, 82634.087895f, 82639.170440f, 82644.253173f, + 82649.336104f, 82654.419242f, 82659.502594f, 82664.586171f, 82669.669979f, + 82674.754029f, 82679.838282f, 82684.922586f, 82690.006935f, 82695.091337f, + 82700.175798f, 82705.260326f, 82710.344927f, 82715.429609f, 82720.514378f, + 82725.599242f, 82730.684208f, 82735.769282f, 82740.854472f, 82745.939784f, + 82751.025226f, 82756.110805f, 82761.196528f, 82766.282401f, 82771.368432f, + 82776.454627f, 82781.540995f, 82786.627541f, 82791.714273f, 82796.801198f, + 82801.888323f, 82806.975655f, 82812.063200f, 82817.150967f, 82822.238961f, + 82827.327190f, 82832.415661f, 82837.504382f, 82842.593358f, 82847.682597f, + 82852.772106f, 82857.861893f, 82862.951963f, 82868.042324f, 82873.132999f, + 82878.224145f, 82883.315779f, 82888.407879f, 82893.500419f, 82898.593376f, + 82903.686725f, 82908.780442f, 82913.874503f, 82918.968883f, 82924.063558f, + 82929.158505f, 82934.253698f, 82939.349114f, 82944.444728f, 82949.540515f, + 82954.636453f, 82959.732516f, 82964.828680f, 82969.924921f, 82975.021214f, + 82980.117536f, 82985.213862f, 82990.310168f, 82995.406429f, 83000.502622f, + 83005.598721f, 83010.694704f, 83015.790545f, 83020.886221f, 83025.981706f, + 83031.076977f, 83036.172010f, 83041.266780f, 83046.361263f, 83051.455434f, + 83056.549270f, 83061.642746f, 83066.735840f, 83071.828665f, 83076.921312f, + 83082.013783f, 83087.106078f, 83092.198199f, 83097.290149f, 83102.381929f, + 83107.473540f, 83112.564984f, 83117.656262f, 83122.747377f, 83127.838330f, + 83132.929123f, 83138.019757f, 83143.110234f, 83148.200556f, 83153.290946f, + 83158.382126f, 83163.473963f, 83168.566244f, 83173.658757f, 83178.751290f, + 83183.843630f, 83188.935566f, 83194.026884f, 83199.117372f, 83204.206819f, + 83209.295011f, 83214.381737f, 83219.466784f, 83224.549940f, 83229.630993f, + 83234.709552f, 83239.778974f, 83244.842262f, 83249.908073f, 83254.985061f, + 83259.991193f, 83264.515036f, 83268.412046f, 83271.635444f, 83274.201854f, + 83276.170728f, 83277.575065f, 83278.577313f, 83279.204936f, 83279.514066f, + 83279.641002f, 83279.635960f, 83279.513557f, 83279.349332f, 83279.170676f, + 83279.006651f, 83278.887807f, 83278.844140f, 83278.868068f, 83278.954816f, + 83279.160191f, 83279.462095f, 83279.870304f, 83280.345104f, 83280.914448f, + 83281.604750f, 83282.363578f, 83283.181514f, 83284.089786f, 83285.049874f, + 83286.042678f, 83287.091023f, 83288.167346f, 83289.250478f, 83290.356354f, + 83291.467199f, 83292.564231f, 83293.657582f, 83294.738659f, 83295.794237f, + 83296.828905f, 83297.841892f, 83298.827792f, 83299.786919f, 83300.722313f, + 83301.635656f, 83302.526238f, 83303.396220f, 83304.250848f, 83305.090729f, + 83305.915467f, 83306.731559f, 83307.541617f, 83308.343155f, 83309.141690f, + 83309.940773f, 83310.737627f, 83311.535797f, 83312.338584f, 83313.144301f, + 83313.954057f, 83314.769491f, 83315.590337f, 83316.416361f, 83317.247627f, + 83318.084422f, 83318.926419f, 83319.773076f, 83320.624271f, 83321.479427f, + 83322.337579f, 83323.197762f, 83324.059014f, 83324.920369f, 83325.782210f, + 83326.646001f, 83327.510995f, 83328.376407f, 83329.241453f, 83330.105345f, + 83330.967739f, 83331.829906f, 83332.691654f, 83333.552583f, 83334.412293f, + 83335.270384f, 83336.126418f, 83336.979818f, 83337.831186f, 83338.681394f, + 83339.531316f, 83340.381825f, 83341.233465f, 83342.085558f, 83342.937797f, + 83343.789931f, 83344.641706f, 83345.492869f, 83346.343019f, 83347.192373f, + 83348.041533f, 83348.891105f, 83349.741693f, 83350.593545f, 83351.446255f, + 83352.299494f, 83353.152930f, 83354.006233f, 83354.859304f, 83355.712391f, + 83356.565549f, 83357.418830f, 83358.272286f, 83359.126023f, 83359.980067f, + 83360.834237f, 83361.688347f, 83362.542214f, 83363.395734f, 83364.249043f, + 83365.102185f, 83365.955201f, 83366.808128f, 83367.661006f, 83368.513874f, + 83369.366769f, 83370.219731f, 83371.072799f, 83371.925995f, 83372.779267f, + 83373.632595f, 83374.485963f, 83375.339357f, 83376.192763f, 83377.046164f, + 83377.899548f, 83378.752898f, 83379.606201f, 83380.459445f, 83381.312658f, + 83382.165846f, 83383.019012f, 83383.872158f, 83384.725284f, 83385.578393f, + 83386.431485f, 83387.284563f, 83388.137628f, 83388.990679f, 83389.843684f, + 83390.696643f, 83391.549571f, 83392.402481f, 83393.255389f, 83394.108310f, + 83394.961258f, 83395.814247f, 83396.667292f, 83397.520408f, 83398.373607f, + 83399.226881f, 83400.080221f, 83400.933619f, 83401.787063f, 83402.640544f, + 83403.494052f, 83404.347578f, 83405.201112f, 83406.054644f, 83406.908164f, + 83407.761662f, 83408.615129f, 83409.468556f, 83410.321931f, 83411.175246f, + 83412.028491f, 83412.881655f, 83413.734730f, 83414.587705f, 83415.440508f, + 83416.293063f, 83417.145438f, 83417.997703f, 83418.849928f, 83419.702185f, + 83420.554543f, 83421.407073f, 83422.259846f, 83423.112932f, 83423.966421f, + 83424.820357f, 83425.674640f, 83426.529163f, 83427.383821f, 83428.238506f, + 83429.093113f, 83429.947535f, 83430.801667f, 83431.655400f, 83432.508624f, + 83433.361300f, 83434.213546f, 83435.065490f, 83435.917262f, 83436.768991f, + 83437.620808f, 83438.472842f, 83439.325221f, 83440.178077f, 83441.031543f, + 83441.885713f, 83442.740455f, 83443.595606f, 83444.451003f, 83445.306480f, + 83446.161876f, 83447.017025f, 83447.871764f, 83448.725930f, 83449.579354f, + 83450.431861f, 83451.283589f, 83452.134759f, 83452.985592f, 83453.836307f, + 83454.687126f, 83455.538269f, 83456.389956f, 83457.242410f, 83458.095850f, + 83458.950596f, 83459.806517f, 83460.663255f, 83461.520451f, 83462.377745f, + 83463.234779f, 83464.091194f, 83464.946631f, 83465.800730f, 83466.653131f, + 83467.503986f, 83468.353741f, 83469.202843f, 83470.051735f, 83470.900866f, + 83471.750680f, 83472.601623f, 83473.454142f, 83474.308711f, 83475.165243f, + 83476.023118f, 83476.881714f, 83477.740407f, 83478.598572f, 83479.455586f, + 83480.310824f, 83481.163811f, 83482.014902f, 83482.864735f, 83483.713944f, + 83484.563167f, 83485.413040f, 83486.264198f, 83487.117129f, 83487.971444f, + 83488.826670f, 83489.682357f, 83490.538054f, 83491.393314f, 83492.247694f, + 83493.101264f, 83493.954349f, 83494.807143f, 83495.659838f, 83496.512629f, + 83497.365710f, 83498.219202f, 83499.072957f, 83499.926877f, 83500.780876f, + 83501.634865f, 83502.488759f, 83503.342470f, 83504.195995f, 83505.049401f, + 83505.902747f, 83506.756093f, 83507.609498f, 83508.463022f, 83509.316719f, + 83510.170569f, 83511.024509f, 83511.878478f, 83512.732415f, 83513.586255f, + 83514.439937f, 83515.293425f, 83516.146776f, 83517.000067f, 83517.853374f, + 83518.706772f, 83519.560337f, 83520.414151f, 83521.268206f, 83522.122394f, + 83522.976607f, 83523.830736f, 83524.684673f, 83525.538302f, 83526.391512f, + 83527.244433f, 83528.097244f, 83528.950124f, 83529.803254f, 83530.656810f, + 83531.511036f, 83532.365928f, 83533.221166f, 83534.076426f, 83534.931385f, + 83535.785718f, 83536.639062f, 83537.491002f, 83538.342002f, 83539.192690f, + 83540.043698f, 83540.895653f, 83541.749186f, 83542.605307f, 83543.463999f, + 83544.323941f, 83545.183801f, 83546.042249f, 83546.897955f, 83547.749357f, + 83548.594678f, 83549.436068f, 83550.276291f, 83551.118114f, 83551.964301f, + 83552.817618f, 83553.682461f, 83554.557796f, 83555.437490f, 83556.315399f, + 83557.185384f, 83558.041252f, 83558.876953f, 83559.698681f, 83560.516790f, + 83561.341634f, 83562.183567f, 83563.054340f, 83563.950393f, 83564.855279f, + 83565.752473f, 83566.625454f, 83567.457490f, 83568.250146f, 83569.027557f, + 83569.814780f, 83570.636874f, 83571.520202f, 83572.433944f, 83573.318924f, + 83574.184044f, 83575.054259f, 83575.938742f, 83576.819773f, 83577.681124f, + 83578.533042f, 83579.394290f, 83580.265023f, 83581.127961f, 83581.974526f, + 83582.817340f, 83583.668240f, 83584.522456f, 83585.372652f, 83586.218329f, + 83587.063904f, 83587.911213f, 83588.758905f, 83589.606177f, 83590.453302f, + 83591.301440f, 83592.150414f, 83592.999981f, 83593.850132f, 83594.700859f, + 83595.552173f, 83596.404120f, 83597.256644f, 83598.109677f, 83598.963152f, + 83599.817155f, 83600.671767f, 83601.526788f, 83602.382019f, 83603.237268f, + 83604.092669f, 83604.948270f, 83605.803969f, 83606.659662f, 83607.515290f, + 83608.371071f, 83609.226986f, 83610.082955f, 83610.938896f, 83611.794731f, + 83612.650378f, 83613.505759f, 83614.360791f, 83615.215396f, 83616.069419f, + 83616.922867f, 83617.775892f, 83618.628645f, 83619.481277f, 83620.333938f, + 83621.186780f, 83622.039954f, 83622.893610f, 83623.747912f, 83624.602925f, + 83625.458469f, 83626.314347f, 83627.170360f, 83628.026311f, 83628.882004f, + 83629.737240f, 83630.591823f, 83631.445550f, 83632.298083f, 83633.149567f, + 83634.000372f, 83634.850867f, 83635.701420f, 83636.552402f, 83637.404183f, + 83638.257131f, 83639.111643f, 83639.967796f, 83640.825138f, 83641.683191f, + 83642.541475f, 83643.399512f, 83644.256825f, 83645.112935f, 83645.967360f, + 83646.819673f, 83647.670287f, 83648.519863f, 83649.369064f, 83650.218553f, + 83651.068991f, 83651.921042f, 83652.775364f, 83653.632045f, 83654.490339f, + 83655.349441f, 83656.208547f, 83657.066853f, 83657.923555f, 83658.777955f, + 83659.630329f, 83660.481453f, 83661.332100f, 83662.183047f, 83663.035066f, + 83663.888878f, 83664.744362f, 83665.600903f, 83666.457907f, 83667.314778f, + 83668.170920f, 83669.025749f, 83669.879179f, 83670.731680f, 83671.583728f, + 83672.435798f, 83673.288366f, 83674.141907f, 83674.996652f, 83675.852255f, + 83676.708303f, 83677.564387f, 83678.420092f, 83679.275009f, 83680.128780f, + 83680.981593f, 83681.833908f, 83682.686184f, 83683.538884f, 83684.392468f, + 83685.247420f, 83686.103768f, 83686.960910f, 83687.818219f, 83688.675067f, + 83689.530827f, 83690.384834f, 83691.236769f, 83692.087429f, 83692.937723f, + 83693.788563f, 83694.640858f, 83695.495560f, 83696.353619f, 83697.213991f, + 83698.075174f, 83698.935666f, 83699.793965f, 83700.648561f, 83701.497561f, + 83702.342139f, 83703.184870f, 83704.028326f, 83704.875081f, 83705.727710f, + 83706.589530f, 83707.459701f, 83708.333375f, 83709.205684f, 83710.071757f, + 83710.926644f, 83711.767701f, 83712.600200f, 83713.430606f, 83714.265385f, + 83715.111002f, 83715.974662f, 83716.856176f, 83717.745877f, 83718.633864f, + 83719.510231f, 83720.364976f, 83721.190236f, 83721.996709f, 83722.799911f, + 83723.615358f, 83724.458822f, 83725.343380f, 83726.236225f, 83727.109486f, + 83727.977195f, 83728.848785f, 83729.722002f, 83730.588996f, 83731.448806f, + 83732.307486f, 83733.167547f, 83734.026003f, 83734.880655f, 83735.732881f, + 83736.584477f, 83737.435319f, 83738.285518f, 83739.135274f, 83739.984781f, + 83740.833661f, 83741.681969f, 83742.530230f, 83743.378966f, 83744.228526f, + 83745.078515f, 83745.928933f, 83746.779846f, 83747.631323f, 83748.483408f, + 83749.336051f, 83750.189175f, 83751.042702f, 83751.896576f, 83752.750814f, + 83753.605380f, 83754.460224f, 83755.315297f, 83756.170551f, 83757.025937f, + 83757.881406f, 83758.736911f, 83759.592454f, 83760.448243f, 83761.304241f, + 83762.160369f, 83763.016552f, 83763.872714f, 83764.728776f, 83765.584664f, + 83766.440314f, 83767.295804f, 83768.151176f, 83769.006452f, 83769.861656f, + 83770.716808f, 83771.571930f, 83772.427044f, 83773.282173f, 83774.137338f, + 83774.992516f, 83775.847678f, 83776.702792f, 83777.557828f, 83778.412755f, + 83779.267541f, 83780.122155f, 83780.976523f, 83781.830668f, 83782.684661f, + 83783.538574f, 83784.392478f, 83785.246443f, 83786.100542f, 83786.954846f, + 83787.809325f, 83788.663908f, 83789.518580f, 83790.373328f, 83791.228138f, + 83792.082995f, 83792.937885f, 83793.792796f, 83794.647768f, 83795.502849f, + 83796.358003f, 83797.213191f, 83798.068376f, 83798.923521f, 83799.778588f, + 83800.633539f, 83801.488334f, 83802.342976f, 83803.197514f, 83804.051996f, + 83804.906472f, 83805.760991f, 83806.615603f, 83807.470355f, 83808.325294f, + 83809.180416f, 83810.035693f, 83810.891095f, 83811.746595f, 83812.602163f, + 83813.457770f, 83814.313387f, 83815.168985f, 83816.024536f, 83816.880009f, + 83817.735377f, 83818.590610f, 83819.445680f, 83820.300557f, 83821.155212f, + 83822.009595f, 83822.863521f, 83823.717026f, 83824.570198f, 83825.423123f, + 83826.275889f, 83827.128584f, 83827.981295f, 83828.834109f, 83829.687114f, + 83830.540397f, 83831.394045f, 83832.248146f, 83833.102788f, 83833.958056f, + 83834.814233f, 83835.671521f, 83836.529467f, 83837.387597f, 83838.245439f, + 83839.102520f, 83839.958715f, 83840.814405f, 83841.669744f, 83842.524886f, + 83843.379983f, 83844.235188f, 83845.090460f, 83845.945690f, 83846.800872f, + 83847.656003f, 83848.511079f, 83849.366095f, 83850.221018f, 83851.075849f, + 83851.930611f, 83852.785323f, 83853.640007f, 83854.494684f, 83855.349374f, + 83856.204100f, 83857.058883f, 83857.913742f, 83858.768700f, 83859.623779f, + 83860.479002f, 83861.334344f, 83862.189777f, 83863.045271f, 83863.900797f, + 83864.756327f, 83865.611830f, 83866.467278f, 83867.322642f, 83868.177892f, + 83869.033000f, 83869.887921f, 83870.742623f, 83871.597154f, 83872.451561f, + 83873.305895f, 83874.160203f, 83875.014535f, 83875.868939f, 83876.723466f, + 83877.578162f, 83878.433079f, 83879.288263f, 83880.143810f, 83880.999722f, + 83881.855917f, 83882.712313f, 83883.568828f, 83884.425378f, 83885.281882f, + 83886.138257f, 83886.994420f, 83887.850289f, 83888.705781f, 83889.560807f, + 83890.415230f, 83891.269115f, 83892.122593f, 83892.975793f, 83893.828844f, + 83894.681875f, 83895.535016f, 83896.388396f, 83897.242144f, 83898.096390f, + 83898.951262f, 83899.806899f, 83900.663355f, 83901.520478f, 83902.378099f, + 83903.236049f, 83904.094160f, 83904.952261f, 83905.810186f, 83906.667764f, + 83907.524828f, 83908.381207f, 83909.236231f, 83910.089660f, 83910.942020f, + 83911.793841f, 83912.645649f, 83913.497971f, 83914.351336f, 83915.206271f, + 83916.063303f, 83916.922960f, 83917.786233f, 83918.655841f, 83919.528857f, + 83920.401300f, 83921.269189f, 83922.129616f, 83922.984721f, 83923.836574f, + 83924.687109f, 83925.538261f, 83926.390491f, 83927.242483f, 83928.094031f, + 83928.944937f, 83929.794987f, 83930.643574f, 83931.491118f, 83932.338502f, + 83933.186608f, 83934.036200f, 83934.886838f, 83935.738215f, 83936.590179f, + 83937.442580f, 83938.295312f, 83939.148461f, 83940.002094f, 83940.856269f, + 83941.711046f, 83942.566611f, 83943.422958f, 83944.279768f, 83945.136724f, + 83945.993514f, 83946.850211f, 83947.706947f, 83948.563725f, 83949.420545f, + 83950.277411f, 83951.134323f, 83951.991283f, 83952.848294f, 83953.705356f, + 83954.562577f, 83955.420213f, 83956.278133f, 83957.136179f, 83957.994193f, + 83958.852019f, 83959.709497f, 83960.566470f, 83961.422781f, 83962.278266f, + 83963.132663f, 83963.986080f, 83964.838784f, 83965.691042f, 83966.543120f, + 83967.395283f, 83968.247798f, 83969.100932f, 83969.954950f, 83970.810243f, + 83971.667116f, 83972.525121f, 83973.383765f, 83974.242556f, 83975.101001f, + 83975.958609f, 83976.814887f, 83977.669344f, 83978.521445f, 83979.369594f, + 83980.214917f, 83981.059816f, 83981.906690f, 83982.757719f, 83983.612272f, + 83984.468993f, 83985.326742f, 83986.184379f, 83987.041173f, 83987.897904f, + 83988.754830f, 83989.612133f, 83990.469997f, 83991.328743f, 83992.188306f, + 83993.048161f, 83993.907779f, 83994.766647f, 83995.624865f, 83996.482694f, + 83997.340194f, 83998.197424f, 83999.054428f, 83999.911123f, 84000.767539f, + 84001.623738f, 84002.479781f, 84003.335682f, 84004.191358f, 84005.046869f, + 84005.902287f, 84006.757683f, 84007.613064f, 84008.468379f, 84009.323639f, + 84010.178860f, 84011.034056f, 84011.889241f, 84012.744429f, 84013.599635f, + 84014.454873f, 84015.310149f, 84016.165411f, 84017.020656f, 84017.875901f, + 84018.731158f, 84019.586442f, 84020.441767f, 84021.297149f, 84022.152600f, + 84023.008135f, 84023.863776f, 84024.719521f, 84025.575348f, 84026.431236f, + 84027.287162f, 84028.143103f, 84028.999037f, 84029.854941f, 84030.710793f, + 84031.566571f, 84032.422269f, 84033.277911f, 84034.133517f, 84034.989114f, + 84035.844723f, 84036.700368f, 84037.556073f, 84038.411862f, 84039.267757f, + 84040.123806f, 84040.980019f, 84041.836350f, 84042.692752f, 84043.549175f, + 84044.405573f, 84045.261899f, 84046.118103f, 84046.974140f, 84047.829952f, + 84048.685468f, 84049.540745f, 84050.395867f, 84051.250917f, 84052.105977f, + 84052.961130f, 84053.816459f, 84054.672047f, 84055.527977f, 84056.384423f, + 84057.241437f, 84058.098844f, 84058.956466f, 84059.814125f, 84060.671645f, + 84061.528847f, 84062.385555f, 84063.241592f, 84064.096748f, 84064.950749f, + 84065.803813f, 84066.656252f, 84067.508377f, 84068.360498f, 84069.212926f, + 84070.065971f, 84070.919945f, 84071.775158f, 84072.632097f, 84073.490552f, + 84074.349574f, 84075.208211f, 84076.065735f, 84076.922598f, 84077.779209f, + 84078.635934f, 84079.493132f, 84080.350868f, 84081.208821f, 84082.066641f, + 84082.923982f, 84083.780758f, 84084.637178f, 84085.493399f, 84086.349576f, + 84087.205784f, 84088.061920f, 84088.917975f, 84089.773939f, 84090.629798f, + 84091.485517f, 84092.341126f, 84093.196667f, 84094.052178f, 84094.907700f, + 84095.763273f, 84096.618936f, 84097.474730f, 84098.330666f, 84099.186712f, + 84100.042839f, 84100.899020f, 84101.755226f, 84102.611431f, 84103.467605f, + 84104.323722f, 84105.179754f, 84106.035699f, 84106.891582f, 84107.747432f, + 84108.603276f, 84109.459141f, 84110.315055f, 84111.171045f, 84112.027138f, + 84112.883362f, 84113.739716f, 84114.596181f, 84115.452736f, 84116.309362f, + 84117.166039f, 84118.022748f, 84118.879468f, 84119.736181f, 84120.592866f, + 84121.449505f, 84122.306076f, 84123.162561f, 84124.018940f, 84124.875193f, + 84125.731300f, 84126.587242f, 84127.442938f, 84128.298294f, 84129.153413f, + 84130.008402f, 84130.863366f, 84131.718412f, 84132.573649f, 84133.429181f, + 84134.285117f, 84135.141502f, 84135.998245f, 84136.855239f, 84137.712373f, + 84138.569538f, 84139.426625f, 84140.283526f, 84141.140130f, 84141.996346f, + 84142.852217f, 84143.707843f, 84144.563321f, 84145.418752f, 84146.274235f, + 84147.129868f, 84147.985752f, 84148.841985f, 84149.698667f, 84150.555726f, + 84151.413032f, 84152.270457f, 84153.127870f, 84153.985142f, 84154.842143f, + 84155.698743f, 84156.554788f, 84157.410167f, 84158.265065f, 84159.119690f, + 84159.974250f, 84160.828954f, 84161.684010f, 84162.539627f, 84163.396013f, + 84164.253518f, 84165.112084f, 84165.971338f, 84166.830908f, 84167.690421f, + 84168.549505f, 84169.407787f, 84170.264895f, 84171.120388f, 84171.973828f, + 84172.825777f, 84173.676935f, 84174.528000f, 84175.379669f, 84176.232640f, + 84177.087612f, 84177.945382f, 84178.805939f, 84179.668225f, 84180.531158f, + 84181.393657f, 84182.254641f, 84183.113028f, 84183.967809f, 84184.819444f, + 84185.669332f, 84186.518879f, 84187.369491f, 84188.222573f, 84189.079477f, + 84189.940053f, 84190.802678f, 84191.665670f, 84192.527353f, 84193.386046f, + 84194.240731f, 84195.092552f, 84195.943083f, 84196.793897f, 84197.646566f, + 84198.502636f, 84199.362343f, 84200.224102f, 84201.086192f, 84201.946891f, + 84202.804529f, 84203.658947f, 84204.511547f, 84205.363779f, 84206.217092f, + 84207.072912f, 84207.931550f, 84208.791806f, 84209.652384f, 84210.511987f, + 84211.369331f, 84212.224042f, 84213.077182f, 84213.929930f, 84214.783468f, + 84215.638973f, 84216.496945f, 84217.356415f, 84218.216221f, 84219.075203f, + 84219.932195f, 84220.786384f, 84221.638780f, 84222.490844f, 84223.344036f, + 84224.199821f, 84225.059499f, 84225.921847f, 84226.784785f, 84227.646236f, + 84228.504119f, 84229.356509f, 84230.204991f, 84231.052556f, 84231.902190f, + 84232.756881f, 84233.619600f, 84234.488092f, 84235.357518f, 84236.223041f, + 84237.079865f, 84237.927174f, 84238.770201f, 84239.614632f, 84240.466158f, + 84241.329920f, 84242.202884f, 84243.077698f, 84243.946999f, 84244.803378f, + 84245.644418f, 84246.478890f, 84247.316864f, 84248.168406f, 84249.037774f, + 84249.915464f, 84250.790101f, 84251.650302f, 84252.490184f, 84253.319949f, + 84254.152600f, 84255.001161f, 84255.875276f, 84256.764153f, 84257.651201f, + 84258.519833f, 84259.358546f, 84260.179177f, 84261.000102f, 84261.839701f, + 84262.713936f, 84263.611288f, 84264.506796f, 84265.375668f, 84266.209989f, + 84267.030278f, 84267.859680f, 84268.720616f, 84269.611941f, 84270.506524f, + 84271.376284f, 84272.212880f, 84273.038546f, 84273.877914f, 84274.749094f, + 84275.636393f, 84276.513212f, 84277.363280f, 84278.201029f, 84279.044828f, + 84279.905682f, 84280.773765f, 84281.636676f, 84282.486896f, 84283.331735f, + 84284.180940f, 84285.040227f, 84285.903874f, 84286.764441f, 84287.617930f, + 84288.468654f, 84289.321799f, 84290.179883f, 84291.040071f, 84291.899257f, + 84292.756203f, 84293.612403f, 84294.469175f, 84295.327101f, 84296.185437f, + 84297.043379f, 84297.900610f, 84298.757564f, 84299.614678f, 84300.472165f, + 84301.329871f, 84302.187626f, 84303.045260f, 84303.902604f, 84304.759484f, + 84305.615713f, 84306.471446f, 84307.326957f, 84308.182524f, 84309.038420f, + 84309.894922f, 84310.752145f, 84311.609864f, 84312.467810f, 84313.325717f, + 84314.183314f, 84315.040335f, 84315.896649f, 84316.752490f, 84317.608151f, + 84318.463922f, 84319.320094f, 84320.176960f, 84321.034678f, 84321.892975f, + 84322.751506f, 84323.609926f, 84324.467890f, 84325.325049f, 84326.181201f, + 84327.036696f, 84327.891973f, 84328.747471f, 84329.603631f, 84330.460907f, + 84331.319610f, 84332.179244f, 84333.039177f, 84333.898777f, 84334.757411f, + 84335.614437f, 84336.469562f, 84337.323462f, 84338.176940f, 84339.030794f, + 84339.885826f, 84340.742847f, 84341.602163f, 84342.462912f, 84343.324105f, + 84344.184754f, 84345.043869f, 84345.900460f, 84346.754272f, 84347.606360f, + 84348.457891f, 84349.310034f, 84350.163955f, 84351.020758f, 84351.880382f, + 84352.741781f, 84353.603885f, 84354.465619f, 84355.325914f, 84356.183823f, + 84357.039683f, 84357.894298f, 84358.748434f, 84359.602858f, 84360.458337f, + 84361.315548f, 84362.174273f, 84363.033895f, 84363.893810f, 84364.753413f, + 84365.612099f, 84366.469344f, 84367.325343f, 84368.180590f, 84369.035571f, + 84369.890771f, 84370.746678f, 84371.603691f, 84372.461602f, 84373.320046f, + 84374.178665f, 84375.037104f, 84375.895006f, 84376.752079f, 84377.608474f, + 84378.464476f, 84379.320370f, 84380.176438f, 84381.032962f, 84381.890166f, + 84382.747915f, 84383.605986f, 84384.464161f, 84385.322220f, 84386.179945f, + 84387.037173f, 84387.894024f, 84388.750661f, 84389.607246f, 84390.463940f, + 84391.320907f, 84392.178262f, 84393.035919f, 84393.893750f, 84394.751628f, + 84395.609427f, 84396.467019f, 84397.324336f, 84398.181457f, 84399.038425f, + 84399.895281f, 84400.752065f, 84401.608818f, 84402.465582f, 84403.322396f, + 84404.179302f, 84405.036340f, 84405.893552f, 84406.750978f, 84407.608669f, + 84408.466619f, 84409.324771f, 84410.183067f, 84411.041450f, 84411.899861f, + 84412.758242f, 84413.616536f, 84414.474684f, 84415.332628f, 84416.190306f, + 84417.047448f, 84417.904057f, 84418.760311f, 84419.616388f, 84420.472465f, + 84421.328719f, 84422.185329f, 84423.042473f, 84423.900328f, 84424.759072f, + 84425.619024f, 84426.481079f, 84427.344414f, 84428.207872f, 84429.070296f, + 84429.930529f, 84430.788209f, 84431.644147f, 84432.499080f, 84433.353744f, + 84434.208877f, 84435.064938f, 84435.921383f, 84436.777964f, 84437.634472f, + 84438.490698f, 84439.346420f, 84440.201438f, 84441.056026f, 84441.910553f, + 84442.765390f, 84443.620905f, 84444.477234f, 84445.334118f, 84446.191375f, + 84447.048826f, 84447.906292f, 84448.763659f, 84449.621102f, 84450.478631f, + 84451.336227f, 84452.193874f, 84453.051555f, 84453.909304f, 84454.767128f, + 84455.625009f, 84456.482927f, 84457.340864f, 84458.198803f, 84459.056724f, + 84459.914609f, 84460.772439f, 84461.630197f, 84462.487863f, 84463.345431f, + 84464.202921f, 84465.060352f, 84465.917748f, 84466.775127f, 84467.632513f, + 84468.489925f, 84469.347385f, 84470.204914f, 84471.062533f, 84471.920266f, + 84472.778116f, 84473.636056f, 84474.494060f, 84475.352099f, 84476.210147f, + 84477.068177f, 84477.926162f, 84478.784074f, 84479.641887f, 84480.499573f, + 84481.357098f, 84482.214471f, 84483.071731f, 84483.928913f, 84484.786056f, + 84485.643197f, 84486.500373f, 84487.357621f, 84488.214979f, 84489.072483f, + 84489.930173f, 84490.788081f, 84491.646175f, 84492.504406f, 84493.362728f, + 84494.221094f, 84495.079457f, 84495.937769f, 84496.795984f, 84497.654054f, + 84498.511933f, 84499.369570f, 84500.226944f, 84501.084109f, 84501.941123f, + 84502.798045f, 84503.654933f, 84504.511846f, 84505.368842f, 84506.225980f, + 84507.083319f, 84507.940916f, 84508.798831f, 84509.657054f, 84510.515516f, + 84511.374148f, 84512.232882f, 84513.091649f, 84513.950380f, 84514.809008f, + 84515.667462f, 84516.525675f, 84517.383577f, 84518.241122f, 84519.098357f, + 84519.955356f, 84520.812193f, 84521.668939f, 84522.525670f, 84523.382458f, + 84524.239375f, 84525.096496f, 84525.953894f, 84526.811638f, 84527.669726f, + 84528.528092f, 84529.386671f, 84530.245401f, 84531.104215f, 84531.963051f, + 84532.821843f, 84533.680528f, 84534.539040f, 84535.397317f, 84536.255296f, + 84537.112982f, 84537.970441f, 84538.827741f, 84539.684949f, 84540.542133f, + 84541.399361f, 84542.256700f, 84543.114218f, 84543.971983f, 84544.830062f, + 84545.688672f, 84546.547939f, 84547.407651f, 84548.267593f, 84549.127550f, + 84549.987307f, 84550.846649f, 84551.705360f, 84552.563227f, 84553.420008f, + 84554.275013f, 84555.128473f, 84555.980999f, 84556.833206f, 84557.685706f, + 84558.539113f, 84559.394039f, 84560.251098f, 84561.111048f, 84561.973881f, + 84562.838619f, 84563.704276f, 84564.569861f, 84565.434386f, 84566.296861f, + 84567.156319f, 84568.012641f, 84568.866664f, 84569.719255f, 84570.571281f, + 84571.423610f, 84572.277108f, 84573.132644f, 84573.990853f, 84574.851213f, + 84575.712954f, 84576.575317f, 84577.437540f, 84578.298862f, 84579.158529f, + 84580.016493f, 84580.873337f, 84581.729482f, 84582.585353f, 84583.441370f, + 84584.297956f, 84585.155471f, 84586.013678f, 84586.872320f, 84587.731198f, + 84588.590112f, 84589.448860f, 84590.307243f, 84591.165128f, 84592.022639f, + 84592.879929f, 84593.737152f, 84594.594460f, 84595.452004f, 84596.309938f, + 84597.168345f, 84598.027100f, 84598.886042f, 84599.745009f, 84600.603838f, + 84601.462368f, 84602.320435f, 84603.178003f, 84604.035255f, 84604.892387f, + 84605.749595f, 84606.607077f, 84607.465029f, 84608.324161f, 84609.184816f, + 84610.046054f, 84610.906919f, 84611.766456f, 84612.623711f, 84613.477537}; -const float SIMULATED_PRESSURE_SPEED[] = { --0.948363f, -0.948363f, -13.274254f, -40.403141f, -79.405710f, -119.092389f, -162.040265f, -202.082736f, -248.586283f, -290.768883f, -337.645715f, -384.558541f, -432.175827f, -480.794502f, -529.952635f, -579.542043f, -629.587822f, -679.694102f, -730.011778f, -780.203192f, -829.813236f, -880.431945f, -926.994001f, -976.108202f, -1023.883786f, -1068.006391f, -1112.696323f, -1154.021320f, -1195.316651f, -1234.653809f, -1273.929067f, -1310.511099f, -1347.088722f, -1382.465466f, -1416.661632f, -1449.831084f, -1481.806571f, -1513.344488f, -1540.857087f, -1568.891304f, -1594.511632f, -1618.411951f, -1640.640080f, -1661.274003f, -1680.543531f, -1698.611728f, -1715.810816f, -1732.354168f, -1748.137780f, -1763.107652f, --1776.721145f, -1788.499697f, -1798.538005f, -1806.435685f, -1812.224381f, -1815.949022f, -1817.553582f, -1817.171319f, -1814.966301f, -1811.231493f, -1806.753756f, -1801.857044f, -1796.509169f, -1790.627019f, -1784.103445f, -1777.117924f, -1769.990709f, -1762.227380f, -1754.404480f, -1745.979311f, -1737.335373f, -1728.525613f, -1719.795091f, -1711.258934f, -1702.929259f, -1694.800914f, -1686.813373f, -1678.869836f, -1671.086967f, -1663.075796f, -1655.039355f, -1646.941808f, -1638.694844f, -1630.433859f, -1622.248517f, -1613.625054f, -1605.889735f, -1597.673857f, -1589.993489f, -1582.175234f, -1574.569597f, -1567.002497f, -1559.461641f, -1551.920197f, -1543.990283f, -1536.939837f, -1528.941596f, -1521.197540f, -1513.481103f, -1505.580544f, --1498.009587f, -1490.323611f, -1482.776293f, -1475.332504f, -1467.997921f, -1460.717334f, -1453.574783f, -1446.397141f, -1439.208953f, -1432.054093f, -1424.858679f, -1417.619623f, -1410.407338f, -1403.087371f, -1395.818595f, -1388.674458f, -1380.934283f, -1374.409215f, -1367.040802f, -1359.990206f, -1352.955497f, -1346.187796f, -1339.196030f, -1332.469424f, -1325.581498f, -1318.828149f, -1312.021043f, -1305.204915f, -1298.391547f, -1291.524853f, -1284.758862f, -1277.881065f, -1271.019585f, -1264.245204f, -1257.465943f, -1250.744429f, -1244.071819f, -1237.451756f, -1230.887296f, -1224.369212f, -1217.892631f, -1211.445773f, -1205.017845f, -1198.602846f, -1192.194381f, -1185.786708f, -1179.378571f, -1172.971336f, -1166.502184f, -1160.135006f, --1153.662157f, -1147.259526f, -1140.955712f, -1134.153584f, -1128.135193f, -1122.182990f, -1115.614306f, -1109.342914f, -1103.202287f, -1097.032092f, -1090.888438f, -1084.926840f, -1078.721265f, -1072.685980f, -1066.649538f, -1060.612288f, -1054.572058f, -1048.557423f, -1042.504375f, -1036.526748f, -1030.554832f, -1024.465625f, -1018.501725f, -1012.513563f, -1006.546597f, -1000.598756f, -994.666982f, -988.780946f, -983.229207f, -976.066977f, -971.068754f, -965.819476f, -959.862593f, -953.788488f, -948.079682f, -942.534301f, -936.654908f, -930.943397f, -925.285974f, -919.712081f, -914.143991f, -908.340575f, -902.711404f, -897.145734f, -891.495687f, -885.841221f, -880.343976f, -874.712701f, -869.088132f, -863.571373f, --857.911559f, -852.415494f, -846.870857f, -841.360507f, -835.847587f, -830.379206f, -824.903727f, -819.469763f, -814.032513f, -808.630569f, -803.209037f, -797.943025f, -792.531988f, -787.160589f, -781.845686f, -776.541833f, -771.236783f, -765.948975f, -760.656941f, -755.392663f, -750.474171f, -744.119438f, -739.368150f, -734.811494f, -729.390960f, -723.916522f, -718.757363f, -713.514328f, -708.282230f, -703.171187f, -697.987194f, -692.762326f, -687.681524f, -682.559624f, -677.369340f, -672.302322f, -667.244692f, -662.109798f, -657.033467f, -652.040492f, -646.968855f, -641.889054f, -636.948245f, -631.935895f, -626.861884f, -621.941216f, -616.985557f, -611.937612f, -607.009210f, -602.108093f, --597.092236f, -592.091690f, -587.222108f, -582.266663f, -577.278266f, -572.456691f, -567.551570f, -562.542502f, -557.678809f, -552.825696f, -547.895296f, -543.060442f, -538.289163f, -533.401076f, -528.500840f, -523.761905f, -518.928770f, -514.025605f, -509.298459f, -504.527584f, -499.662678f, -494.943221f, -490.239079f, -485.415426f, -480.637675f, -475.986668f, -471.220600f, -466.417382f, -461.802760f, -457.092949f, -452.367494f, -448.454713f, -443.428587f, -436.768857f, -431.720285f, -428.436103f, -424.690453f, -420.483337f, -415.814754f, -410.684704f, -405.293589f, -400.769121f, -396.352320f, -391.866193f, -387.310742f, -382.685966f, -377.991866f, -373.376069f, -368.866194f, -364.279610f, --359.615876f, -354.874993f, -350.057910f, -345.601793f, -341.369310f, -336.968681f, -332.399905f, -327.662983f, -322.769446f, -318.371461f, -314.180197f, -309.805962f, -305.248756f, -300.508579f, -295.620549f, -291.338380f, -287.186287f, -282.834150f, -278.281970f, -273.529748f, -268.653028f, -264.488035f, -260.374530f, -256.040505f, -251.485963f, -246.710901f, -241.853583f, -237.809455f, -233.735053f, -229.414096f, -224.846583f, -220.032514f, -215.202584f, -211.278819f, -207.241447f, -202.927874f, -198.338100f, -193.472125f, -188.694335f, -184.904575f, -180.910215f, -176.598786f, -171.970288f, -167.024721f, -162.804402f, -160.333689f, -156.612294f, -151.503833f, -145.008306f, -137.125714f, --130.607074f, -128.233382f, -125.671089f, -122.828499f, -119.705613f, -116.302430f, -112.618951f, -108.655176f, -104.411103f, -99.886735f, -95.082070f, -89.998441f, -85.471831f, -81.320716f, -76.805696f, -71.926771f, -66.683942f, -61.124979f, -58.025683f, -55.607063f, -51.974028f, -47.126579f, -41.064716f, -34.013352f, -31.082730f, -29.184286f, -26.054765f, -21.694166f, -16.102490f, -9.710043f, -7.113163f, -3.699538f, 1.261139f, 4.956837f, 8.178839f, 11.089240f, 13.877067f, 16.424262f, 18.969779f, 21.425959f, 23.829639f, 26.190556f, 28.515170f, 30.807700f, 33.070689f, 35.305395f, 37.482641f, 39.800572f, 41.849296f, 43.849612f, -46.081010f, 48.067826f, 50.140332f, 52.069499f, 54.044469f, 55.906150f, 57.774978f, 59.559850f, 61.314128f, 63.013778f, 64.542838f, 66.321117f, 68.044085f, 69.336995f, 70.603586f, 71.847410f, 73.183871f, 74.647901f, 75.900694f, 76.931359f, 77.873919f, 79.069182f, 80.114781f, 80.973104f, 81.714515f, 82.665612f, 83.519602f, 84.219700f, 84.797987f, 85.533954f, 86.218707f, 86.779738f, 87.225675f, 87.738076f, 88.271803f, 88.771024f, 89.235311f, 89.639452f, 89.997206f, 90.324905f, 90.622550f, 90.890140f, 91.127676f, 91.335156f, 91.512583f, 91.734407f, 92.035394f, 92.299645f, 92.525915f, 92.714206f, -92.864516f, 92.976846f, 93.051195f, 93.120986f, 93.283226f, 93.435300f, 93.566170f, 93.675835f, 93.764296f, 93.831552f, 93.877603f, 93.908696f, 93.986962f, 94.071469f, 94.145719f, 94.209712f, 94.263448f, 94.306928f, 94.340151f, 94.363302f, 94.393229f, 94.429648f, 94.463797f, 94.495675f, 94.525282f, 94.552619f, 94.577684f, 94.600479f, 94.621003f, 94.639255f, 94.655237f, 94.668949f, 94.680389f, 94.689558f, 94.696457f, 94.701085f, 94.707253f, 94.726611f, 94.746091f, 94.764164f, 94.780830f, 94.796090f, 94.809944f, 94.822391f, 94.833431f, 94.843064f, 94.851291f, 94.858112f, 94.863526f, 94.867533f, -94.870134f, 94.871328f, 94.871191f, 94.878544f, 94.890648f, 94.901428f, 94.910882f, 94.919011f, 94.925815f, 94.931294f, 94.935448f, 94.938277f, 94.939781f, 94.939959f, 94.938813f, 94.936341f, 94.932545f, 94.927423f, 94.920976f, 94.914409f, 94.912257f, 94.910765f, 94.909394f, 94.908142f, 94.907011f, 94.905999f, 94.905106f, 94.904334f, 94.903681f, 94.903149f, 94.902736f, 94.902443f, 94.902269f, 94.902216f, 94.902282f, 94.902434f, 94.896842f, 94.888282f, 94.881575f, 94.876721f, 94.873720f, 94.872572f, 94.873278f, 94.875836f, 94.880247f, 94.886511f, 94.894628f, 94.904599f, 94.916422f, 94.930098f, -94.945628f, 94.963010f, 94.981112f, 94.994828f, 95.007306f, 95.019117f, 95.030260f, 95.040735f, 95.050543f, 95.059683f, 95.068155f, 95.075960f, 95.083097f, 95.089566f, 95.095368f, 95.100502f, 95.104968f, 95.108767f, 95.111943f, 95.127988f, 95.151816f, 95.171674f, 95.187562f, 95.199479f, 95.207426f, 95.211403f, 95.211410f, 95.207447f, 95.199513f, 95.187609f, 95.171735f, 95.151891f, 95.128076f, 95.100291f, 95.068536f, 95.033090f, 94.998207f, 94.967148f, 94.939993f, 94.916741f, 94.897392f, 94.881947f, 94.870406f, 94.862768f, 94.859033f, 94.859202f, 94.863274f, 94.871249f, 94.883128f, 94.898911f, -94.918596f, 94.942137f, 94.935235f, 94.905522f, 94.883777f, 94.870001f, 94.864195f, 94.866358f, 94.876489f, 94.894590f, 94.920659f, 94.954698f, 94.996706f, 95.046683f, 95.104629f, 95.170544f, 95.244427f, 95.326280f, 95.425384f, 95.571000f, 95.704180f, 95.816472f, 95.907876f, 95.978392f, 96.028020f, 96.056760f, 96.064612f, 96.051577f, 96.017653f, 95.962841f, 95.887141f, 95.790554f, 95.673078f, 95.534715f, 95.375456f, 95.184213f, 95.006962f, 94.875436f, 94.789634f, 94.749556f, 94.755203f, 94.806573f, 94.875420f, 94.857304f, 94.844080f, 94.848751f, 94.871317f, 94.911778f, 94.970134f, 95.046420f, -95.133913f, 95.207073f, 95.260493f, 95.294175f, 95.308118f, 95.302323f, 95.276789f, 95.245125f, 95.239799f, 95.237223f, 95.235781f, 95.235472f, 95.236297f, 95.238255f, 95.240892f, 95.238018f, 95.235537f, 95.236081f, 95.239649f, 95.246242f, 95.255860f, 95.268502f, 95.280038f, 95.286065f, 95.291866f, 95.297523f, 95.303038f, 95.308409f, 95.313637f, 95.318723f, 95.323665f, 95.328463f, 95.333119f, 95.337632f, 95.342001f, 95.346228f, 95.350311f, 95.355889f, 95.362632f, 95.368844f, 95.374527f, 95.379680f, 95.384303f, 95.388397f, 95.391960f, 95.394995f, 95.397499f, 95.399474f, 95.400919f, 95.401834f, -95.402220f, 95.402110f, 95.403448f, 95.405735f, 95.407955f, 95.410108f, 95.412193f, 95.414211f, 95.416161f, 95.418043f, 95.419858f, 95.421606f, 95.423286f, 95.424898f, 95.426444f, 95.427921f, 95.429309f, 95.430336f, 95.431390f, 95.432619f, 95.434025f, 95.435606f, 95.437362f, 95.439295f, 95.441403f, 95.443687f, 95.446147f, 95.448782f, 95.451593f, 95.454580f, 95.457742f, 95.460880f, 95.462555f, 95.463923f, 95.465319f, 95.466743f, 95.468195f, 95.469675f, 95.471183f, 95.472719f, 95.474283f, 95.475876f, 95.477496f, 95.479144f, 95.480820f, 95.482524f, 95.484256f, 95.486016f, 95.487804f, 95.489620f, -95.491465f, 95.493337f, 95.495237f, 95.497165f, 95.499121f, 95.501105f, 95.503117f, 95.505157f, 95.507226f, 95.509322f, 95.511446f, 95.513383f, 95.514773f, 95.516185f, 95.517655f, 95.519182f, 95.520767f, 95.522410f, 95.524110f, 95.525868f, 95.527683f, 95.529556f, 95.531486f, 95.533474f, 95.535520f, 95.537623f, 95.539783f, 95.542002f, 95.544277f, 95.546611f, 95.549002f, 95.551450f, 95.553956f, 95.556520f, 95.559141f, 95.561819f, 95.564556f, 95.567349f, 95.570201f, 95.573110f, 95.576076f, 95.577858f, 95.578561f, 95.579508f, 95.580699f, 95.582133f, 95.583812f, 95.585735f, 95.587901f, 95.590312f, -95.592966f, 95.595864f, 95.599007f, 95.602393f, 95.606023f, 95.609897f, 95.614015f, 95.618377f, 95.622983f, 95.627833f, 95.632926f, 95.638264f, 95.643846f, 95.649671f, 95.655741f, 95.662054f, 95.668611f, 95.675320f, 95.680205f, 95.684411f, 95.688763f, 95.693261f, 95.697905f, 95.702696f, 95.707634f, 95.712717f, 95.717948f, 95.723324f, 95.728847f, 95.734517f, 95.740333f, 95.746295f, 95.752403f, 95.758658f, 95.765060f, 95.771608f, 95.778302f, 95.785143f, 95.792130f, 95.799263f, 95.806543f, 95.814132f, 95.823742f, 95.833521f, 95.842796f, 95.851565f, 95.859828f, 95.867586f, 95.874839f, 95.881586f, -95.887827f, 95.893563f, 95.898794f, 95.903519f, 95.907739f, 95.911453f, 95.914662f, 95.917366f, 95.919564f, 95.921256f, 95.922443f, 95.923125f, 95.923301f, 95.925791f, 95.933186f, 95.940043f, 95.946219f, 95.951713f, 95.956526f, 95.960657f, 95.964107f, 95.966875f, 95.968962f, 95.970368f, 95.971091f, 95.971134f, 95.970495f, 95.969174f, 95.967172f, 95.964488f, 95.961123f, 95.957076f, 95.952348f, 95.946938f, 95.941124f, 95.939281f, 95.938784f, 95.938085f, 95.937185f, 95.936083f, 95.934779f, 95.933274f, 95.931568f, 95.929659f, 95.927549f, 95.925238f, 95.922725f, 95.920010f, 95.917094f, 95.913976f, -95.910656f, 95.907135f, 95.903413f, 95.899488f, 95.895362f, 95.891035f, 95.885223f, 95.876891f, 95.869337f, 95.862679f, 95.856918f, 95.852055f, 95.848088f, 95.845018f, 95.842846f, 95.841570f, 95.841191f, 95.841710f, 95.843125f, 95.845437f, 95.848646f, 95.852753f, 95.857756f, 95.863656f, 95.870453f, 95.877222f, 95.876480f, 95.874586f, 95.873550f, 95.873371f, 95.874050f, 95.875586f, 95.877980f, 95.881231f, 95.885340f, 95.890306f, 95.896130f, 95.902811f, 95.910350f, 95.918746f, 95.928000f, 95.938111f, 95.949079f, 95.960906f, 95.973586f, 95.984581f, 95.993505f, 96.002256f, 96.010835f, 96.019240f, -96.027473f, 96.035534f, 96.043422f, 96.051137f, 96.058679f, 96.066049f, 96.073246f, 96.080270f, 96.087122f, 96.093801f, 96.100307f, 96.106641f, 96.112802f, 96.118790f, 96.126106f, 96.137803f, 96.148992f, 96.159283f, 96.168674f, 96.177166f, 96.184759f, 96.191453f, 96.197248f, 96.202144f, 96.206140f, 96.209238f, 96.211436f, 96.212736f, 96.213136f, 96.212637f, 96.211239f, 96.208942f, 96.205746f, 96.202172f, 96.206080f, 96.211983f, 96.216952f, 96.220985f, 96.224083f, 96.226245f, 96.227472f, 96.227763f, 96.227120f, 96.225540f, 96.223026f, 96.219576f, 96.215191f, 96.209870f, 96.203614f, 96.196422f, -96.188295f, 96.179233f, 96.169236f, 96.158018f, 96.146775f, 96.136665f, 96.127694f, 96.119860f, 96.113164f, 96.107605f, 96.103185f, 96.099902f, 96.097757f, 96.096749f, 96.096880f, 96.098148f, 96.100554f, 96.104098f, 96.108779f, 96.114599f, 96.121556f, 96.129651f, 96.136653f, 96.134085f, 96.131201f, 96.129394f, 96.128664f, 96.129010f, 96.130434f, 96.132934f, 96.136511f, 96.141165f, 96.146895f, 96.153703f, 96.161587f, 96.170548f, 96.180586f, 96.191701f, 96.203893f, 96.217161f, 96.231506f, 96.246885f, 96.261347f, 96.274526f, 96.287027f, 96.298848f, 96.309991f, 96.320455f, 96.330239f, 96.339345f, -96.347771f, 96.355519f, 96.362587f, 96.368976f, 96.374687f, 96.379718f, 96.384071f, 96.387744f, 96.390739f, 96.396351f, 96.408823f, 96.420312f, 96.430472f, 96.439304f, 96.446806f, 96.452981f, 96.457826f, 96.461343f, 96.463531f, 96.464390f, 96.463920f, 96.462122f, 96.458995f, 96.454539f, 96.448755f, 96.441641f, 96.433630f, 96.430547f, 96.429176f, 96.427751f, 96.426270f, 96.424734f, 96.423143f, 96.421497f, 96.419795f, 96.418039f, 96.416227f, 96.414360f, 96.412439f, 96.410462f, 96.408430f, 96.406342f, 96.404200f, 96.402002f, 96.398006f, 96.391886f, 96.386596f, 96.382161f, 96.378582f, 96.375859f, -96.373991f, 96.372979f, 96.372823f, 96.373523f, 96.375078f, 96.377489f, 96.380755f, 96.384878f, 96.389856f, 96.395689f, 96.402379f, 96.409073f, 96.410492f, 96.411274f, 96.412575f, 96.414393f, 96.416730f, 96.419585f, 96.422958f, 96.426849f, 96.431258f, 96.436185f, 96.441630f, 96.447594f, 96.454075f, 96.461075f, 96.468593f, 96.476629f, 96.485182f, 96.493559f, 96.501244f, 96.508588f, 96.515591f, 96.522253f, 96.528573f, 96.534553f, 96.540191f, 96.545489f, 96.550445f, 96.555060f, 96.559334f, 96.563267f, 96.566859f, 96.570110f, 96.573020f, 96.575588f, 96.578902f, 96.586245f, 96.593374f, 96.599779f, -96.605459f, 96.610414f, 96.614644f, 96.618150f, 96.620931f, 96.622988f, 96.624319f, 96.624926f, 96.624809f, 96.623966f, 96.622399f, 96.620108f, 96.617091f, 96.613400f, 96.611535f, 96.610967f, 96.610492f, 96.610110f, 96.609821f, 96.609624f, 96.609521f, 96.609510f, 96.609592f, 96.609767f, 96.610035f, 96.610395f, 96.610848f, 96.611394f, 96.612033f, 96.612765f, 96.613589f, 96.612641f, 96.607452f, 96.603151f, 96.600003f, 96.598008f, 96.597167f, 96.597478f, 96.598942f, 96.601559f, 96.605330f, 96.610253f, 96.616329f, 96.623559f, 96.631941f, 96.641477f, 96.652165f, 96.664007f, 96.676778f, 96.686737f, -96.695272f, 96.703491f, 96.711395f, 96.718984f, 96.726257f, 96.733215f, 96.739858f, 96.746186f, 96.752198f, 96.757895f, 96.763277f, 96.768343f, 96.773094f, 96.777530f, 96.781650f, 96.785456f, 96.795482f, 96.814460f, 96.830799f, 96.844335f, 96.855068f, 96.862999f, 96.868127f, 96.870453f, 96.869976f, 96.866697f, 96.860615f, 96.851731f, 96.840044f, 96.825555f, 96.808263f, 96.788168f, 96.765272f, 96.739962f, 96.716653f, 96.695957f, 96.677482f, 96.661227f, 96.647193f, 96.635380f, 96.625787f, 96.618414f, 96.613262f, 96.610331f, 96.609620f, 96.611129f, 96.614859f, 96.620810f, 96.628981f, 96.639373f, -96.651985f, 96.640816f, 96.609258f, 96.584664f, 96.567033f, 96.556365f, 96.552661f, 96.555919f, 96.566141f, 96.583326f, 96.607474f, 96.638586f, 96.676661f, 96.721699f, 96.773700f, 96.832664f, 96.898592f, 96.971482f, 97.058486f, 97.176150f, 97.283359f, 97.375360f, 97.452152f, 97.513736f, 97.560111f, 97.591278f, 97.607236f, 97.607985f, 97.593525f, 97.563857f, 97.518981f, 97.458896f, 97.383602f, 97.293099f, 97.187388f, 97.066495f, 96.939865f, 96.830590f, 96.742106f, 96.674414f, 96.627512f, 96.601402f, 96.596083f, 96.611555f, 96.637171f, 96.594325f, 96.551892f, 96.529899f, 96.528347f, 96.547236f, -96.586566f, 96.646337f, 96.726549f, 96.822686f, 96.908184f, 96.976272f, 97.027193f, 97.060947f, 97.077533f, 97.076953f, 97.059205f, 97.024291f, 97.003520f, 97.007412f, 97.008029f, 97.005365f, 96.999422f, 96.990200f, 96.977698f, 96.961916f, 96.942105f, 96.908733f, 96.879464f, 96.860835f, 96.852844f, 96.855491f, 96.868778f, 96.892703f, 96.927266f, 96.969383f, 97.001060f, 97.026524f, 97.047222f, 97.063155f, 97.074321f, 97.080722f, 97.082356f, 97.079225f, 97.079171f, 97.089596f, 97.097627f, 97.102993f, 97.105693f, 97.105727f, 97.103095f, 97.097798f, 97.089864f, 97.082353f, 97.077180f, 97.073480f, -97.071252f, 97.070498f, 97.071216f, 97.073407f, 97.077071f, 97.081569f, 97.081531f, 97.080659f, 97.080231f, 97.080246f, 97.080705f, 97.081607f, 97.082953f, 97.084742f, 97.086974f, 97.089650f, 97.092769f, 97.096332f, 97.100338f, 97.104787f, 97.109680f, 97.115016f, 97.120796f, 97.125695f, 97.129304f, 97.132923f, 97.136553f, 97.140193f, 97.143843f, 97.147504f, 97.151176f, 97.154858f, 97.158551f, 97.162255f, 97.165969f, 97.169693f, 97.173428f, 97.177174f, 97.180930f, 97.184697f, 97.188211f, 97.190520f, 97.192940f, 97.195665f, 97.198695f, 97.202031f, 97.205673f, 97.209621f, 97.213874f, 97.218432f, -97.223297f, 97.228467f, 97.233942f, 97.239724f, 97.245810f, 97.252203f, 97.258901f, 97.265912f, 97.273753f, 97.281706f, 97.289230f, 97.296324f, 97.302990f, 97.309225f, 97.315032f, 97.320409f, 97.325357f, 97.329875f, 97.333964f, 97.337623f, 97.340854f, 97.343655f, 97.346026f, 97.347968f, 97.349481f, 97.350565f, 97.351219f, 97.351444f, 97.351239f, 97.350605f, 97.349542f, 97.348049f, 97.346127f, 97.343776f, 97.340995f, 97.337785f, 97.334145f, 97.330076f, 97.325578f, 97.320651f, 97.315294f, 97.309508f, 97.303308f, 97.297327f, 97.292069f, 97.287490f, 97.283591f, 97.280371f, 97.277830f, 97.275969f, -97.274787f, 97.274284f, 97.274461f, 97.275317f, 97.276852f, 97.279067f, 97.281961f, 97.282869f, 97.278361f, 97.274854f, 97.272637f, 97.271708f, 97.272068f, 97.273717f, 97.276655f, 97.280882f, 97.286398f, 97.293203f, 97.301297f, 97.310679f, 97.321351f, 97.333121f, 97.341952f, 97.349242f, 97.356364f, 97.363318f, 97.370105f, 97.376724f, 97.383175f, 97.389459f, 97.395575f, 97.401523f, 97.407303f, 97.412916f, 97.418361f, 97.423639f, 97.428772f, 97.433968f, 97.439334f, 97.444869f, 97.450575f, 97.456451f, 97.462497f, 97.468714f, 97.475100f, 97.481657f, 97.488384f, 97.495281f, 97.502348f, 97.509586f, -97.517207f, 97.528578f, 97.540563f, 97.551619f, 97.561746f, 97.570945f, 97.579214f, 97.586555f, 97.592967f, 97.598451f, 97.603005f, 97.606631f, 97.609328f, 97.611096f, 97.611935f, 97.611846f, 97.610827f, 97.608880f, 97.606005f, 97.602200f, 97.597467f, 97.591804f, 97.585213f, 97.577694f, 97.569245f, 97.559868f, 97.549562f, 97.538327f, 97.526163f, 97.512770f, 97.494631f, 97.477584f, 97.463734f, 97.453081f, 97.445624f, 97.441364f, 97.440301f, 97.442434f, 97.447763f, 97.456290f, 97.468012f, 97.474644f, 97.473067f, 97.473084f, 97.474727f, 97.477996f, 97.482892f, 97.489414f, 97.497563f, 97.507338f, -97.518740f, 97.531768f, 97.546015f, 97.558979f, 97.570675f, 97.581225f, 97.590628f, 97.598884f, 97.605994f, 97.611957f, 97.616773f, 97.620443f, 97.622966f, 97.624515f, 97.629297f, 97.635341f, 97.640914f, 97.646015f, 97.650644f, 97.654801f, 97.658487f, 97.661700f, 97.664442f, 97.666712f, 97.668510f, 97.669944f, 97.671391f, 97.672908f, 97.674494f, 97.676150f, 97.677875f, 97.679670f, 97.681534f, 97.683468f, 97.685471f, 97.687543f, 97.689685f, 97.691896f, 97.694177f, 97.696527f, 97.698947f, 97.701436f, 97.703995f, 97.706623f, 97.709320f, 97.712087f, 97.714924f, 97.717821f, 97.720422f, 97.722852f, -97.725282f, 97.727715f, 97.730149f, 97.732584f, 97.735020f, 97.737458f, 97.739898f, 97.742338f, 97.744781f, 97.747224f, 97.749669f, 97.752116f, 97.754563f, 97.757013f, 97.759463f, 97.761915f, 97.764369f, 97.766823f, 97.769280f, 97.771737f, 97.774092f, 97.775808f, 97.777515f, 97.779344f, 97.781295f, 97.783368f, 97.785563f, 97.787881f, 97.790320f, 97.792882f, 97.795565f, 97.798371f, 97.801299f, 97.804349f, 97.807521f, 97.810815f, 97.814231f, 97.817769f, 97.821429f, 97.825211f, 97.829115f, 97.833142f, 97.837290f, 97.840785f, 97.842598f, 97.844371f, 97.846169f, 97.847993f, 97.849842f, 97.851716f, -97.853615f, 97.855540f, 97.857490f, 97.859465f, 97.861465f, 97.863491f, 97.865542f, 97.867619f, 97.869720f, 97.871847f, 97.874000f, 97.876177f, 97.878380f, 97.880608f, 97.882861f, 97.885140f, 97.887444f, 97.889773f, 97.892128f, 97.894508f, 97.896913f, 97.899343f, 97.901799f, 97.904280f, 97.906786f, 97.909317f, 97.911874f, 97.914456f, 97.917064f, 97.919696f, 97.922354f, 97.925038f, 97.927746f, 97.930480f, 97.933239f, 97.936023f, 97.938833f, 97.941668f, 97.944543f, 97.947590f, 97.950645f, 97.953660f, 97.956635f, 97.959568f, 97.962461f, 97.965314f, 97.968126f, 97.970897f, 97.973628f, 97.976318f, -97.978968f, 97.981577f, 97.984145f, 97.986673f, 97.989160f, 97.991606f, 97.994012f, 97.996377f, 97.998702f, 98.000986f, 98.003230f, 98.005432f, 98.007595f, 98.009716f, 98.011797f, 98.013838f, 98.015838f, 98.017797f, 98.019715f, 98.021593f, 98.023431f, 98.025228f, 98.026984f, 98.028699f, 98.030374f, 98.032009f, 98.033603f, 98.035156f, 98.036668f, 98.038140f, 98.039572f, 98.040962f, 98.042313f, 98.043622f, 98.044988f, 98.046473f, 98.047985f, 98.049526f, 98.051094f, 98.052691f, 98.054316f, 98.055970f, 98.057651f, 98.059361f, 98.061098f, 98.062864f, 98.064658f, 98.066480f, 98.068331f, 98.070209f, -98.072116f, 98.074051f, 98.076013f, 98.078005f, 98.080024f, 98.082071f, 98.084147f, 98.086251f, 98.088382f, 98.090542f, 98.092731f, 98.094947f, 98.097192f, 98.099464f, 98.101765f, 98.104094f, 98.106451f, 98.108836f, 98.111250f, 98.113691f, 98.116161f, 98.118659f, 98.121185f, 98.123739f, 98.126322f, 98.128932f, 98.131571f, 98.134238f, 98.136933f, 98.139613f, 98.142163f, 98.144848f, 98.147702f, 98.150724f, 98.153914f, 98.157273f, 98.160801f, 98.164497f, 98.168362f, 98.172395f, 98.176596f, 98.180966f, 98.185505f, 98.190212f, 98.195088f, 98.200132f, 98.205344f, 98.210725f, 98.216275f, 98.221993f, -98.227879f, 98.233934f, 98.239580f, 98.244307f, 98.249056f, 98.253846f, 98.258676f, 98.263546f, 98.268456f, 98.273407f, 98.278398f, 98.283429f, 98.288501f, 98.293613f, 98.298766f, 98.303959f, 98.309192f, 98.314465f, 98.319779f, 98.325133f, 98.330527f, 98.335962f, 98.341437f, 98.346953f, 98.352530f, 98.359395f, 98.366763f, 98.373808f, 98.380530f, 98.386929f, 98.393004f, 98.398756f, 98.404185f, 98.409290f, 98.414073f, 98.418532f, 98.422668f, 98.426480f, 98.429970f, 98.433136f, 98.435979f, 98.438499f, 98.440695f, 98.442568f, 98.444118f, 98.445345f, 98.446248f, 98.447608f, 98.454523f, 98.461977f, -98.468680f, 98.474630f, 98.479829f, 98.484276f, 98.487972f, 98.490916f, 98.493108f, 98.494548f, 98.495237f, 98.495173f, 98.494359f, 98.492792f, 98.490474f, 98.487404f, 98.483582f, 98.479009f, 98.473683f, 98.467607f, 98.460778f, 98.453198f, 98.446798f, 98.444899f, 98.443068f, 98.441065f, 98.438888f, 98.436539f, 98.434016f, 98.431320f, 98.428451f, 98.425409f, 98.422194f, 98.418806f, 98.415244f, 98.411510f, 98.407602f, 98.403521f, 98.399267f, 98.394840f, 98.390240f, 98.385467f, 98.380521f, 98.375401f, 98.370102f, 98.358628f, 98.343008f, 98.329144f, 98.317034f, 98.306681f, 98.298083f, 98.291240f, -98.286153f, 98.282821f, 98.281245f, 98.281425f, 98.283360f, 98.287050f, 98.292496f, 98.299698f, 98.308655f, 98.319367f, 98.331835f, 98.346059f, 98.362038f, 98.379772f, 98.399262f, 98.419114f, 98.422038f, 98.420791f, 98.421211f, 98.423299f, 98.427055f, 98.432479f, 98.439570f, 98.448329f, 98.458756f, 98.470850f, 98.484612f, 98.500041f, 98.517139f, 98.535903f, 98.556336f, 98.578436f, 98.602204f, 98.627640f, 98.654743f, 98.683514f, 98.713953f, 98.746059f, 98.787521f, 98.855514f, 98.918831f, 98.974467f, 99.022423f, 99.062699f, 99.095294f, 99.120210f, 99.137445f, 99.147000f, 99.148874f, 99.143069f, -99.129583f, 99.108417f, 99.079570f, 99.043044f, 98.998837f, 98.946950f, 98.887383f, 98.820136f, 98.745177f, 98.665894f, 98.595201f, 98.535711f, 98.487424f, 98.450340f, 98.424460f, 98.409783f, 98.406310f, 98.414040f, 98.432973f, 98.427322f, 98.393089f, 98.369745f, 98.357315f, 98.355800f, 98.365198f, 98.385510f, 98.416737f, 98.458878f, 98.511933f, 98.570310f, 98.617336f, 98.657508f, 98.691226f, 98.718491f, 98.739304f, 98.753664f, 98.761571f, 98.763025f, 98.758026f, 98.751737f, 98.766477f, 98.781073f, 98.791900f, 98.798957f, 98.802244f, 98.801762f, 98.797509f, 98.789487f, 98.777695f, 98.762483f, -98.749970f, 98.740538f, 98.733107f, 98.727675f, 98.724244f, 98.722813f, 98.723382f, 98.725952f, 98.730521f, 98.737023f, 98.738339f, 98.736475f, 98.736094f, 98.737196f, 98.739780f, 98.743847f, 98.749397f, 98.756429f, 98.764944f, 98.774942f, 98.783819f, 98.789582f, 98.795655f, 98.802074f, 98.808840f, 98.815952f, 98.823410f, 98.831215f, 98.839366f, 98.847864f, 98.856727f, 98.865535f, 98.873718f, 98.881255f, 98.888147f, 98.894393f, 98.899992f, 98.904946f, 98.909254f, 98.912916f, 98.915932f, 98.918302f, 98.920026f, 98.921104f, 98.921537f, 98.921323f, 98.920463f, 98.918958f, 98.916807f, 98.914009f, -98.910733f, 98.910864f, 98.912404f, 98.913732f, 98.914846f, 98.915748f, 98.916437f, 98.916913f, 98.917177f, 98.917227f, 98.917065f, 98.916689f, 98.916101f, 98.915301f, 98.914287f, 98.913060f, 98.911621f, 98.909969f, 98.908104f, 98.905397f, 98.901811f, 98.898735f, 98.896189f, 98.894174f, 98.892690f, 98.891737f, 98.891314f, 98.891423f, 98.892062f, 98.893232f, 98.894934f, 98.897166f, 98.899929f, 98.903222f, 98.907047f, 98.911403f, 98.916289f, 98.921031f, 98.921530f, 98.921611f, 98.922200f, 98.923296f, 98.924899f, 98.927010f, 98.929628f, 98.932754f, 98.936387f, 98.940528f, 98.945175f, 98.950331f, -98.955994f, 98.962164f, 98.968841f, 98.976026f, 98.983719f, 98.991913f, 98.999139f, 99.005302f, 99.011348f, 99.017275f, 99.023085f, 99.028777f, 99.034350f, 99.039806f, 99.045144f, 99.050364f, 99.055467f, 99.060451f, 99.065317f, 99.070066f, 99.074696f, 99.079209f, 99.083604f, 99.087881f, 99.092929f, 99.100425f, 99.107601f, 99.114253f, 99.120383f, 99.125990f, 99.131073f, 99.135634f, 99.139671f, 99.143186f, 99.146177f, 99.148645f, 99.150590f, 99.152012f, 99.152911f, 99.153287f, 99.153140f, 99.152470f, 99.151482f, 99.153481f, 99.156417f, 99.159086f, 99.161488f, 99.163622f, 99.165489f, 99.167089f, -99.168421f, 99.169486f, 99.170284f, 99.170814f, 99.171077f, 99.171073f, 99.170801f, 99.170262f, 99.169455f, 99.168382f, 99.167041f, 99.165666f, 99.164642f, 99.163817f, 99.163188f, 99.162755f, 99.162517f, 99.162476f, 99.162631f, 99.162981f, 99.163528f, 99.164271f, 99.165209f, 99.166344f, 99.167675f, 99.169201f, 99.170924f, 99.172842f, 99.174957f, 99.176598f, 99.174749f, 99.172921f, 99.171750f, 99.171236f, 99.171378f, 99.172176f, 99.173631f, 99.175743f, 99.178511f, 99.181936f, 99.186017f, 99.190755f, 99.196149f, 99.202200f, 99.208907f, 99.216271f, 99.224292f, 99.232938f, 99.239519f, 99.244561f, -99.249642f, 99.254761f, 99.259919f, 99.265115f, 99.270350f, 99.275624f, 99.280935f, 99.286286f, 99.291674f, 99.297101f, 99.302567f, 99.308071f, 99.313614f, 99.319195f, 99.324815f, 99.330473f, 99.337531f, 99.347715f, 99.357188f, 99.365752f, 99.373409f, 99.380157f, 99.385997f, 99.390929f, 99.394952f, 99.398067f, 99.400273f, 99.401572f, 99.401962f, 99.401444f, 99.400017f, 99.397682f, 99.394439f, 99.390287f, 99.385692f, 99.386166f, 99.387960f, 99.389360f, 99.390366f, 99.390980f, 99.391199f, 99.391025f, 99.390458f, 99.389497f, 99.388142f, 99.386394f, 99.384253f, 99.381718f, 99.378789f, 99.375467f, -99.371752f, 99.367643f, 99.363140f, 99.356415f, 99.348044f, 99.340801f, 99.334692f, 99.329718f, 99.325877f, 99.323171f, 99.321598f, 99.321160f, 99.321855f, 99.323685f, 99.326649f, 99.330746f, 99.335978f, 99.342344f, 99.349844f, 99.358478f, 99.368246f, 99.377083f, 99.377174f, 99.377006f, 99.377848f, 99.379702f, 99.382567f, 99.386443f, 99.391331f, 99.397230f, 99.404141f, 99.412063f, 99.420996f, 99.430940f, 99.441896f, 99.453863f, 99.466841f, 99.480831f, 99.495832f, 99.511866f, 99.529327f, 99.546286f, 99.561978f, 99.576403f, 99.589560f, 99.601450f, 99.612073f, 99.621429f, 99.629518f, 99.636339f, -99.641893f, 99.646180f, 99.649200f, 99.650953f, 99.651438f, 99.650656f, 99.648607f, 99.645291f, 99.648079f, 99.665101f, 99.679972f, 99.692093f, 99.701464f, 99.708084f, 99.711955f, 99.713075f, 99.711445f, 99.707064f, 99.699933f, 99.690052f, 99.677421f, 99.662040f, 99.643908f, 99.623026f, 99.599393f, 99.573011f, 99.543011f, 99.504264f, 99.467702f, 99.435917f, 99.408909f, 99.386679f, 99.369226f, 99.356551f, 99.348653f, 99.345532f, 99.347188f, 99.353622f, 99.364834f, 99.380822f, 99.401588f, 99.427131f, 99.457452f, 99.492550f, 99.532425f, 99.571230f, 99.603359f, 99.631869f, 99.656759f, 99.678029f, -99.695679f, 99.709710f, 99.720120f, 99.726911f, 99.741884f, 99.774430f, 99.798990f, 99.814834f, 99.821964f, 99.820380f, 99.810080f, 99.791067f, 99.763338f, 99.729508f, 99.703283f, 99.682824f, 99.667409f, 99.657036f, 99.651707f, 99.651422f, 99.656180f, 99.665981f, 99.679074f, 99.680875f, 99.680832f, 99.682233f, 99.685077f, 99.689365f, 99.695096f, 99.702270f, 99.710887f, 99.720897f, 99.730363f, 99.738607f, 99.746052f, 99.752696f, 99.758540f, 99.763583f, 99.767827f, 99.771270f, 99.773914f, 99.777091f, 99.781056f, 99.784772f, 99.788241f, 99.791461f, 99.794434f, 99.797158f, 99.799634f, 99.801862f, -99.803842f, 99.805574f, 99.807058f, 99.808294f, 99.809282f, 99.810021f, 99.810513f, 99.810756f, 99.810751f, 99.810808f, 99.811855f, 99.813005f, 99.814161f, 99.815324f, 99.816492f, 99.817667f, 99.818848f, 99.820034f, 99.821227f, 99.822426f, 99.823631f, 99.824842f, 99.826060f, 99.827283f, 99.828512f, 99.829748f, 99.830990f, 99.832189f, 99.832358f, 99.832304f, 99.832509f, 99.832974f, 99.833699f, 99.834684f, 99.835929f, 99.837434f, 99.839199f, 99.841224f, 99.843508f, 99.846053f, 99.848858f, 99.851923f, 99.855247f, 99.858832f, 99.862676f, 99.866781f, 99.870159f, 99.872063f, 99.874116f, 99.876345f, -99.878752f, 99.881337f, 99.884098f, 99.887036f, 99.890152f, 99.893445f, 99.896915f, 99.900562f, 99.904387f, 99.908389f, 99.912567f, 99.916923f, 99.921456f, 99.926167f, 99.930998f, 99.935521f, 99.939883f, 99.944145f, 99.948309f, 99.952373f, 99.956338f, 99.960204f, 99.963971f, 99.967639f, 99.971207f, 99.974676f, 99.978046f, 99.981317f, 99.984489f, 99.987561f, 99.990534f, 99.993408f, 99.996181f, 99.998467f, 100.000597f, 100.002957f, 100.005547f, 100.008367f, 100.011417f, 100.014697f, 100.018208f, 100.021948f, 100.025918f, 100.030119f, 100.034549f, 100.039209f, 100.044100f, 100.049220f, 100.054571f, 100.060152f, -100.065962f, 100.072131f, 100.078667f, 100.084925f, 100.090869f, 100.096501f, 100.101818f, 100.106822f, 100.111513f, 100.115890f, 100.119953f, 100.123703f, 100.127140f, 100.130263f, 100.133072f, 100.135568f, 100.137750f, 100.139619f, 100.141174f, 100.142416f, 100.143344f, 100.143959f, 100.144260f, 100.144248f, 100.143922f, 100.143282f, 100.142329f, 100.141063f, 100.139483f, 100.137589f, 100.135382f, 100.132862f, 100.130028f, 100.126880f, 100.123419f, 100.119644f, 100.115556f, 100.111154f, 100.106477f, 100.102132f, 100.098452f, 100.095438f, 100.093091f, 100.091410f, 100.090395f, 100.090046f, 100.090364f, 100.091348f, 100.092998f, 100.095314f, -100.098296f, 100.101945f, 100.106053f, 100.106421f, 100.105657f, 100.105533f, 100.106049f, 100.107206f, 100.109002f, 100.111438f, 100.114514f, 100.118230f, 100.122586f, 100.127582f, 100.133218f, 100.139494f, 100.146410f, 100.153424f, 100.158587f, 100.163476f, 100.168303f, 100.173069f, 100.177773f, 100.182415f, 100.186996f, 100.191516f, 100.195974f, 100.200370f, 100.204704f, 100.208978f, 100.213189f, 100.217339f, 100.221032f, 100.224399f, 100.228025f, 100.231912f, 100.236059f, 100.240467f, 100.245135f, 100.250064f, 100.255254f, 100.260704f, 100.266415f, 100.272386f, 100.278618f, 100.285110f, 100.291847f, 100.298450f, 100.304701f, 100.310631f, -100.316242f, 100.321532f, 100.326502f, 100.331152f, 100.335481f, 100.339491f, 100.343180f, 100.346549f, 100.349598f, 100.352327f, 100.354735f, 100.356823f, 100.358592f, 100.360040f, 100.361167f, 100.361975f, 100.362462f, 100.362630f, 100.362477f, 100.362003f, 100.361210f, 100.360097f, 100.358663f, 100.356909f, 100.354835f, 100.352441f, 100.351512f, 100.352213f, 100.352904f, 100.353586f, 100.354258f, 100.354920f, 100.355573f, 100.356216f, 100.356849f, 100.357472f, 100.358086f, 100.358690f, 100.359285f, 100.359869f, 100.360401f, 100.360472f, 100.360545f, 100.360771f, 100.361152f, 100.361687f, 100.362376f, 100.363220f, 100.364217f, 100.365369f, -100.366675f, 100.368135f, 100.369749f, 100.371518f, 100.373440f, 100.373433f, 100.367968f, 100.362875f, 100.358520f, 100.354904f, 100.352025f, 100.349884f, 100.348482f, 100.347818f, 100.347892f, 100.348704f, 100.350254f, 100.352542f, 100.355568f, 100.359333f, 100.363836f, 100.369076f, 100.375055f, 100.381772f, 100.389227f, 100.397421f, 100.406352f, 100.416022f, 100.426429f, 100.437575f, 100.449459f, 100.462081f, 100.475442f, 100.489540f, 100.505343f, 100.529229f, 100.552241f, 100.572110f, 100.588839f, 100.602425f, 100.612871f, 100.620174f, 100.624337f, 100.625357f, 100.623237f, 100.617974f, 100.613147f, 100.619784f, 100.626196f, 100.631021f, -100.634259f, 100.635910f, 100.635974f, 100.634450f, 100.631340f, 100.626642f, 100.620357f, 100.612484f, 100.604120f, 100.597660f, 100.592178f, 100.587635f, 100.584032f, 100.581369f, 100.579645f, 100.578861f, 100.579017f, 100.580113f, 100.582148f, 100.585120f, 100.585212f, 100.582812f, 100.581306f, 100.580693f, 100.580974f, 100.582149f, 100.584218f, 100.587181f, 100.591037f, 100.595787f, 100.601431f, 100.607821f, 100.611286f, 100.613334f, 100.615423f, 100.617552f, 100.619723f, 100.621934f, 100.624185f, 100.626478f, 100.628810f, 100.631184f, 100.633598f, 100.636053f, 100.638549f, 100.641085f, 100.643662f, 100.646279f, 100.648937f, 100.651636f, -100.654375f, 100.657155f, 100.659976f, 100.662838f, 100.665740f, 100.668609f, 100.671250f, 100.673873f, 100.676499f, 100.679128f, 100.681759f, 100.684392f, 100.687028f, 100.689666f, 100.692307f, 100.694950f, 100.697596f, 100.700245f, 100.702896f, 100.705549f, 100.708205f, 100.710863f, 100.713524f, 100.716188f, 100.718853f, 100.721522f, 100.724193f, 100.726866f, 100.729543f, 100.732450f, 100.735495f, 100.738483f, 100.741415f, 100.744290f, 100.747108f, 100.749870f, 100.752575f, 100.755224f, 100.757816f, 100.760351f, 100.762830f, 100.765253f, 100.767619f, 100.769928f, 100.772180f, 100.774377f, 100.776516f, 100.778599f, 100.780625f, 100.782595f, -100.784508f, 100.786365f, 100.787923f, 100.787912f, 100.787721f, 100.787641f, 100.787672f, 100.787813f, 100.788065f, 100.788427f, 100.788900f, 100.789484f, 100.790179f, 100.790984f, 100.791900f, 100.792926f, 100.794064f, 100.795311f, 100.796670f, 100.798139f, 100.799719f, 100.801410f, 100.803211f, 100.805123f, 100.807146f, 100.809279f, 100.811523f, 100.813877f, 100.816343f, 100.818919f, 100.821605f, 100.824403f, 100.827311f, 100.830329f, 100.833459f, 100.836699f, 100.840049f, 100.843511f, 100.847083f, 100.850765f, 100.854559f, 100.858463f, 100.862477f, 100.866603f, 100.870839f, 100.875185f, 100.879643f, 100.884211f, 100.888890f, 100.893716f, -100.899173f, 100.904845f, 100.910544f, 100.916270f, 100.922023f, 100.927802f, 100.933608f, 100.939441f, 100.945301f, 100.951188f, 100.957101f, 100.963042f, 100.969009f, 100.975003f, 100.981023f, 100.987071f, 100.993145f, 100.999246f, 101.005415f, 101.013313f, 101.021768f, 101.029748f, 101.037255f, 101.044288f, 101.050847f, 101.056932f, 101.062543f, 101.067680f, 101.072343f, 101.076532f, 101.080247f, 101.083488f, 101.086256f, 101.088549f, 101.090369f, 101.091714f, 101.092586f, 101.092987f, 101.096666f, 101.103065f, 101.108822f, 101.113936f, 101.118408f, 101.122238f, 101.125426f, 101.127971f, 101.129873f, 101.131134f, 101.131752f, 101.131727f, -101.131060f, 101.129751f, 101.127800f, 101.125206f, 101.121969f, 101.118091f, 101.113570f, 101.110829f, 101.111273f, 101.111456f, 101.111346f, 101.110943f, 101.110245f, 101.109255f, 101.107970f, 101.106392f, 101.104520f, 101.102354f, 101.099895f, 101.097143f, 101.094096f, 101.090756f, 101.087122f, 101.083195f, 101.078974f, 101.074459f, 101.068370f, 101.059591f, 101.051715f, 101.044892f, 101.039122f, 101.034406f, 101.030742f, 101.028131f, 101.026573f, 101.026068f, 101.026616f, 101.028217f, 101.030871f, 101.034578f, 101.039338f, 101.045151f, 101.052017f, 101.059936f, 101.068907f, 101.076151f, 101.073717f, 101.071448f, 101.070435f, 101.070678f, -101.072177f, 101.074932f, 101.078943f, 101.084210f, 101.090733f, 101.098512f, 101.107547f, 101.117837f, 101.129384f, 101.142187f, 101.156246f, 101.171561f, 101.188132f, 101.205959f, 101.224794f, 101.242448f, 101.258939f, 101.274464f, 101.289022f, 101.302615f, 101.315241f, 101.326900f, 101.337594f, 101.347321f, 101.356083f, 101.363877f, 101.370706f, 101.376569f, 101.381465f, 101.385395f, 101.388359f, 101.390356f, 101.391388f, 101.393048f, 101.413241f, 101.436287f, 101.455872f, 101.471995f, 101.484658f, 101.493859f, 101.499599f, 101.501878f, 101.500696f, 101.496052f, 101.487947f, 101.476382f, 101.461354f, 101.442866f, 101.420917f, 101.395506f, -101.366634f, 101.334301f, 101.298416f, 101.257917f, 101.219392f, 101.185132f, 101.155136f, 101.129403f, 101.107934f, 101.090729f, 101.077788f, 101.069111f, 101.064698f, 101.064548f, 101.068663f, 101.077041f, 101.089683f, 101.106589f, 101.127758f, 101.153192f, 101.182889f, 101.216806f, 101.240601f, 101.255141f, 101.270958f, 101.288051f, 101.306421f, 101.326068f, 101.346992f, 101.369192f, 101.392669f, 101.420667f, 101.465858f, 101.504740f, 101.533318f, 101.551592f, 101.559562f, 101.557228f, 101.544590f, 101.521649f, 101.488403f, 101.457093f, 101.439868f, 101.425218f, 101.413067f, 101.403414f, 101.396259f, 101.391603f, 101.389446f, 101.389787f, -101.390940f, 101.375876f, 101.361011f, 101.352324f, 101.349816f, 101.353486f, 101.363335f, 101.379362f, 101.401568f, 101.429952f, 101.463729f, 101.495309f, 101.520532f, 101.539360f, 101.551793f, 101.557832f, 101.557476f, 101.550725f, 101.537580f, 101.518423f, 101.506018f, 101.499324f, 101.493905f, 101.489761f, 101.486894f, 101.485302f, 101.484986f, 101.485945f, 101.488181f, 101.489943f, 101.486868f, 101.484633f, 101.483777f, 101.484299f, 101.486199f, 101.489477f, 101.494134f, 101.500169f, 101.507565f, 101.512495f, 101.514956f, 101.517458f, 101.520003f, 101.522590f, 101.525219f, 101.527890f, 101.530604f, 101.533360f, 101.536158f, 101.538998f, -101.541880f, 101.544805f, 101.547771f, 101.550780f, 101.553831f, 101.556925f, 101.560060f, 101.563238f, 101.566388f, 101.569453f, 101.572497f, 101.575521f, 101.578525f, 101.581509f, 101.584473f, 101.587417f, 101.590341f, 101.593245f, 101.596128f, 101.598992f, 101.601836f, 101.604659f, 101.607463f, 101.610246f, 101.613010f, 101.615753f, 101.618476f, 101.620883f, 101.622756f, 101.624783f, 101.626985f, 101.629362f, 101.631914f, 101.634640f, 101.637542f, 101.640618f, 101.643869f, 101.647296f, 101.650897f, 101.654673f, 101.658623f, 101.662749f, 101.667050f, 101.671525f, 101.676176f, 101.681001f, 101.685051f, 101.686076f, 101.686985f, 101.688035f, -101.689225f, 101.690555f, 101.692025f, 101.693636f, 101.695387f, 101.697279f, 101.699311f, 101.701483f, 101.703796f, 101.706249f, 101.708842f, 101.711576f, 101.714450f, 101.717464f, 101.720619f, 101.723914f, 101.727350f, 101.730926f, 101.734642f, 101.738499f, 101.742496f, 101.746633f, 101.750910f, 101.755328f, 101.759887f, 101.764586f, 101.769425f, 101.774404f, 101.779524f, 101.784784f, 101.790185f, 101.795726f, 101.801407f, 101.807228f, 101.813503f, 101.822903f, 101.832689f, 101.841990f, 101.850806f, 101.859136f, 101.866981f, 101.874342f, 101.881217f, 101.887606f, 101.893511f, 101.898930f, 101.903864f, 101.908313f, 101.912277f, 101.915755f, -101.918749f, 101.921257f, 101.923280f, 101.924817f, 101.925870f, 101.926437f, 101.926519f, 101.926116f, 101.925228f, 101.923854f, 101.921995f, 101.919651f, 101.916822f, 101.913508f, 101.909708f, 101.905423f, 101.900653f, 101.895398f, 101.889658f, 101.883432f, 101.876721f, 101.869525f, 101.861868f, 101.856511f, 101.852942f, 101.849405f, 101.845902f, 101.842432f, 101.838994f, 101.835590f, 101.832218f, 101.828880f, 101.825574f, 101.822301f, 101.819061f, 101.815854f, 101.812680f, 101.809539f, 101.806431f, 101.807814f, 101.823603f, 101.836735f, 101.845621f, 101.850261f, 101.850655f, 101.846803f, 101.838705f, 101.826360f, 101.809769f, 101.788932f, -101.763849f, 101.734520f, 101.700945f, 101.663123f, 101.621055f, 101.571169f, 101.388443f, 101.265771f, 101.316210f, 101.539762f, 100.122644f, 90.476861f, 77.940204f, 64.467952f, 51.328201f, 39.377482f, 28.086747f, 20.044960f, 12.552455f, 6.182592f, 2.538719f, -0.100834f, -2.448053f, -3.284514f, -3.573116f, -3.280497f, -2.376872f, -0.873345f, 0.478556f, 1.734965f, 4.107486f, 6.038079f, 8.164180f, 9.496014f, 11.386878f, 13.806034f, 15.176567f, 16.358720f, 18.165444f, 19.201755f, 19.856068f, 20.966906f, 21.526461f, 21.662649f, 22.117522f, 22.216883f, 21.940653f, 21.867024f, 21.621539f, 21.111552f, -20.693368f, 20.259722f, 19.718018f, 19.182535f, 18.707868f, 18.266860f, 17.811642f, 17.399645f, 17.092564f, 16.797613f, 16.494755f, 16.321852f, 16.201167f, 16.030757f, 15.970686f, 15.981668f, 15.937079f, 15.963397f, 16.055743f, 16.114348f, 16.195112f, 16.308680f, 16.416918f, 16.520492f, 16.625309f, 16.735907f, 16.839934f, 16.933135f, 17.023914f, 17.103114f, 17.163033f, 17.203671f, 17.225028f, 17.227103f, 17.236830f, 17.275810f, 17.299884f, 17.308248f, 17.300905f, 17.277852f, 17.247870f, 17.243338f, 17.234959f, 17.218581f, 17.194205f, 17.161829f, 17.120673f, 17.068009f, 17.027350f, 17.004160f, -16.998440f, 17.010188f, 17.032793f, 17.041852f, 17.044793f, 17.042677f, 17.035504f, 17.023262f, 17.002999f, 16.987063f, 16.983212f, 16.991444f, 17.011760f, 17.037024f, 17.054210f, 17.064775f, 17.068723f, 17.066055f, 17.061433f, 17.061741f, 17.063152f, 17.065613f, 17.069124f, 17.074749f, 17.080878f, 17.083391f, 17.082209f, 17.077332f, 17.070405f, 17.066168f, 17.062853f, 17.060314f, 17.058550f, 17.057561f, 17.057347f, 17.057908f, 17.059244f, 17.061355f, 17.063919f, 17.065444f, 17.066552f, 17.067365f, 17.067883f, 17.068106f, 17.068035f, 17.067668f, 17.067006f, 17.066050f, 17.064890f, 17.064251f, -17.063772f, 17.063324f, 17.062908f, 17.062524f, 17.062170f, 17.061849f, 17.061559f, 17.061300f, 17.061027f, 17.060101f, 17.059178f, 17.058548f, 17.058210f, 17.058165f, 17.058412f, 17.058951f, 17.059783f, 17.060908f, 17.062326f, 17.063964f, 17.065486f, 17.066812f, 17.067944f, 17.068881f, 17.069622f, 17.070168f, 17.070519f, 17.070675f, 17.070635f, 17.070400f, 17.069970f, 17.069345f, 17.068525f, 17.067509f, 17.066299f, 17.064893f, 17.063291f, 17.061495f, 17.059503f, 17.056064f, 17.051099f, 17.047493f, 17.045297f, 17.044510f, 17.045132f, 17.047165f, 17.050607f, 17.055458f, 17.061719f, 17.069778f, -17.078713f, 17.085654f, 17.090467f, 17.093153f, 17.093711f, 17.092142f, 17.088445f, 17.082621f, 17.074669f, 17.064487f, 17.053521f, 17.044905f, 17.038879f, 17.035442f, 17.034595f, 17.036338f, 17.040670f, 17.047592f, 17.057103f, 17.069324f, 17.083398f, 17.094849f, 17.103025f, 17.107926f, 17.109553f, 17.107905f, 17.102983f, 17.094786f, 17.083314f, 17.068474f, 17.050144f, 17.034566f, 17.023401f, 17.016648f, 17.014307f, 17.016377f, 17.022860f, 17.033755f, 17.049061f, 17.068810f, 17.094910f, 17.118425f, 17.134760f, 17.143915f, 17.145889f, 17.140683f, 17.128297f, 17.108731f, 17.081984f, 17.048022f, -17.017107f, 16.995105f, 16.982021f, 16.977856f, 16.982608f, 16.996279f, 17.018869f, 17.050376f, 17.091391f, 17.130621f, 17.157512f, 17.171923f, 17.173853f, 17.163303f, 17.140272f, 17.104760f, 17.059738f, 17.021830f, 16.996650f, 16.984193f, 16.984461f, 16.997452f, 17.023168f, 17.058613f, 17.086306f, 17.104514f, 17.113731f, 17.113957f, 17.105191f, 17.087592f, 17.071407f, 17.061705f, 17.055873f, 17.053912f, 17.055821f, 17.061601f, 17.069849f, 17.075097f, 17.078408f, 17.079973f, 17.079792f, 17.077865f, 17.074225f, 17.070508f, 17.068115f, 17.066917f, 17.066914f, 17.068105f, 17.070491f, 17.073933f, -17.076989f, 17.078810f, 17.079388f, 17.078721f, 17.076811f, 17.073644f, 17.069746f, 17.067031f, 17.065828f, 17.066137f, 17.067959f, 17.071294f, 17.076285f, 17.081095f, 17.083765f, 17.084261f, 17.082584f, 17.078732f, 17.072579f, 17.064199f, 17.058419f, 17.056222f, 17.057610f, 17.062581f, 17.071137f, 17.084511f, 17.097839f, 17.104762f, 17.105206f, 17.099171f, 17.086657f, 17.066892f, 17.038794f, 17.019990f, 17.013775f, 17.020148f, 17.039109f, 17.070659f, 17.122406f, 17.173852f, 17.198830f, 17.197201f, 17.168965f, 17.114122f, 17.028029f, 16.906433f, 16.827794f, 16.804467f, 16.836450f, 16.923743f, -17.066348f, 17.296847f, 17.506715f, 17.593865f, 17.558192f, 17.399695f, 17.117370f, 16.714013f, 16.434556f, 16.362178f, 16.496881f, 16.838664f, 17.415451f, 17.921067f, 18.097711f, 17.943894f, 17.459617f, 16.640716f, 15.853117f, 15.548218f, 15.744472f, 16.441878f, 17.666558f, 18.274839f, 17.699607f, 17.302395f, 17.404292f, 17.689659f, 17.620632f, 17.227012f, 17.038364f, 17.224956f, 17.414660f, 17.258769f, 16.931287f, 16.856289f, 17.017992f, 17.084321f, 17.003923f, 16.913542f, 16.911494f, 16.946186f, 16.953849f, 16.945425f, 16.942503f, 16.962769f, 16.979474f, 16.991338f, 17.003024f, 17.014534f, -17.026279f, 17.038953f, 17.050476f, 17.060658f, 17.069500f, 17.080067f, 17.092225f, 17.100427f, 17.104626f, 17.104974f, 17.108010f, 17.112031f, 17.113982f, 17.113863f, 17.112556f, 17.115625f, 17.118298f, 17.119367f, 17.118832f, 17.116693f, 17.112951f, 17.107604f, 17.100653f, 17.092097f, 17.080449f, 17.068968f, 17.060505f, 17.055061f, 17.052635f, 17.053228f, 17.056839f, 17.063468f, 17.073115f, 17.086050f, 17.100268f, 17.110879f, 17.117543f, 17.120259f, 17.119029f, 17.113851f, 17.104727f, 17.091655f, 17.074547f, 17.050666f, 17.029685f, 17.016092f, 17.009888f, 17.011073f, 17.019646f, 17.035608f, -17.058959f, 17.090240f, 17.123072f, 17.146840f, 17.161043f, 17.165680f, 17.160753f, 17.146260f, 17.122202f, 17.088501f, 17.046256f, 17.012267f, 16.991524f, 16.984027f, 16.989775f, 17.008769f, 17.041008f, 17.086440f, 17.133637f, 17.165873f, 17.182033f, 17.182116f, 17.166123f, 17.134054f, 17.087988f, 17.047488f, 17.022474f, 17.012952f, 17.018924f, 17.040388f, 17.076248f, 17.109664f, 17.130825f, 17.140076f, 17.137418f, 17.122851f, 17.096581f, 17.068597f, 17.050018f, 17.040952f, 17.041399f, 17.051360f, 17.070830f, 17.094896f, 17.112054f, 17.120977f, 17.121664f, 17.114114f, 17.098328f, 17.075430f, -17.056262f, 17.046288f, 17.045533f, 17.053997f, 17.071681f, 17.099042f, 17.126961f, 17.142839f, 17.146172f, 17.136960f, 17.115205f, 17.080135f, 17.038709f, 17.013192f, 17.005885f, 17.016789f, 17.045904f, 17.094052f, 17.161175f, 17.207433f, 17.223655f, 17.209840f, 17.165990f, 17.091919f, 16.980001f, 16.891567f, 16.854607f, 16.869121f, 16.935108f, 17.052570f, 17.236404f, 17.403428f, 17.473485f, 17.446167f, 17.321472f, 17.097726f, 16.821152f, 16.649978f, 16.608119f, 16.695576f, 16.912349f, 17.273197f, 17.630268f, 17.794031f, 17.759726f, 17.527352f, 17.094890f, 16.505208f, 16.129464f, 16.064041f, -16.308938f, 16.869271f, 17.691158f, 17.856913f, 17.465207f, 17.354194f, 17.431797f, 17.464336f, 17.339888f, 17.196189f, 17.173603f, 17.201220f, 17.169120f, 17.093044f, 17.044526f, 17.031915f, 17.016843f, 17.003978f, 16.995119f, 16.990134f, 16.977593f, 16.966175f, 16.965219f, 16.974724f, 16.991198f, 16.999773f, 17.008352f, 17.018271f, 17.029533f, 17.041710f, 17.052857f, 17.062470f, 17.070550f, 17.077466f, 17.084778f, 17.091311f, 17.096872f, 17.101461f, 17.105078f, 17.107722f, 17.109393f, 17.110092f, 17.110865f, 17.115785f, 17.119943f, 17.122570f, 17.123664f, 17.123227f, 17.121258f, 17.117756f, -17.113003f, 17.109783f, 17.107441f, 17.105535f, 17.104067f, 17.103036f, 17.102442f, 17.102285f, 17.102581f, 17.103293f, 17.103574f, 17.103238f, 17.102285f, 17.100717f, 17.098532f, 17.095730f, 17.092284f, 17.087359f, 17.082898f, 17.079863f, 17.078254f, 17.078070f, 17.079311f, 17.081978f, 17.086069f, 17.089593f, 17.091659f, 17.093446f, 17.094956f, 17.096187f, 17.097140f, 17.097816f, 17.098213f, 17.099438f, 17.101625f, 17.103071f, 17.103764f, 17.103706f, 17.102897f, 17.101335f, 17.099021f, 17.095903f, 17.092844f, 17.090753f, 17.089645f, 17.089521f, 17.090381f, 17.092225f, 17.095052f, 17.098770f, -17.102441f, 17.105539f, 17.108057f, 17.109997f, 17.111357f, 17.112138f, 17.112340f, 17.111963f, 17.111007f, 17.109472f, 17.107358f, 17.104664f, 17.101392f, 17.097540f, 17.093109f, 17.087654f, 17.078520f, 17.070101f, 17.063430f, 17.058506f, 17.055329f, 17.053900f, 17.054218f, 17.056284f, 17.060097f, 17.065657f, 17.072965f, 17.082020f, 17.092823f, 17.105373f, 17.123528f, 17.145768f, 17.158916f, 17.162606f, 17.156838f, 17.141613f, 17.123900f, 17.113799f, 17.106791f, 17.102838f, 17.101941f, 17.104090f, 17.105452f, 17.104589f, 17.103646f, 17.102622f, 17.101518f, 17.100314f, 17.098461f, 17.096633f, -17.095227f, 17.094242f, 17.093679f, 17.093537f, 17.093818f, 17.094519f, 17.095643f, 17.097188f, 17.099155f, 17.101594f, 17.104452f, 17.106844f, 17.108654f, 17.109881f, 17.110526f, 17.110587f, 17.110067f, 17.108963f, 17.107278f, 17.105009f, 17.102158f, 17.098411f, 17.094051f, 17.090611f, 17.088149f, 17.086667f, 17.086163f, 17.086638f, 17.088091f, 17.090524f, 17.093935f, 17.098326f, 17.103695f, 17.110924f, 17.118241f, 17.123908f, 17.127925f, 17.130292f, 17.131009f, 17.130076f, 17.127492f, 17.123259f, 17.117375f, 17.109842f, 17.100535f, 17.088446f, 17.077711f, 17.069561f, 17.063997f, 17.061019f, -17.060626f, 17.062818f, 17.067597f, 17.074961f, 17.084910f, 17.097445f, 17.112739f, 17.129124f, 17.142460f, 17.152420f, 17.159003f, 17.162209f, 17.162038f, 17.158491f, 17.151568f, 17.141267f, 17.127590f, 17.100474f, 17.068571f, 17.047216f, 17.036412f, 17.036157f, 17.046452f, 17.067298f, 17.098693f, 17.140639f, 17.193134f, 17.265461f, 17.392162f, 17.460327f, 17.448863f, 17.357769f, 17.208552f, 17.102093f, 17.037058f, 17.010707f, 17.023041f, 17.044603f, 17.039831f, 17.030960f, 17.018123f, 17.000994f, 16.971748f, 16.950878f, 16.947670f, 16.962125f, 16.991847f, 17.012763f, 17.027530f, 17.039283f, -17.048022f, 17.054636f, 17.062986f, 17.072651f, 17.083502f, 17.095539f, 17.111306f, 17.126934f, 17.136210f, 17.139113f, 17.135796f, 17.133939f, 17.134727f, 17.135552f, 17.136413f, 17.137309f, 17.138241f, 17.139209f, 17.140213f, 17.141253f, 17.144420f, 17.152716f, 17.158395f, 17.160919f, 17.160289f, 17.156503f, 17.149562f, 17.139466f, 17.126215f, 17.109712f, 17.087926f, 17.068345f, 17.054089f, 17.045157f, 17.041549f, 17.043266f, 17.050306f, 17.062671f, 17.080361f, 17.105856f, 17.137472f, 17.160096f, 17.172877f, 17.175815f, 17.168908f, 17.152159f, 17.125566f, 17.089130f, 17.042031f, 16.962970f, -16.906470f, 16.897973f, 16.937479f, 17.020587f, 17.091063f, 17.134422f, 17.154981f, 17.152738f, 17.135864f, 17.134625f, 17.138521f, 17.146071f, 17.157276f, 17.174923f, 17.191259f, 17.197088f, 17.192374f, 17.177341f, 17.164372f, 17.156581f, 17.149992f, 17.144604f, 17.140079f, 17.133908f, 17.128323f, 17.123970f, 17.120852f, 17.118030f, 17.113520f, 17.110216f, 17.108352f, 17.107927f, 17.107631f, 17.106282f, 17.105209f, 17.104422f, 17.103919f, 17.103701f, 17.103768f, 17.104120f, 17.104756f, 17.105521f, 17.105227f, 17.104913f, 17.104884f, 17.105141f, 17.105684f, 17.106512f, 17.107626f, 17.109025f, -17.110710f, 17.112804f, 17.114903f, 17.116555f, 17.117758f, 17.118512f, 17.118818f, 17.118674f, 17.118082f, 17.117042f, 17.115558f, 17.113976f, 17.112822f, 17.112138f, 17.111925f, 17.112181f, 17.112907f, 17.114104f, 17.115770f, 17.117907f, 17.120972f, 17.124271f, 17.126623f, 17.128023f, 17.128470f, 17.127964f, 17.126505f, 17.124094f, 17.120730f, 17.116253f, 17.110307f, 17.105545f, 17.102441f, 17.100992f, 17.101200f, 17.103064f, 17.106584f, 17.111761f, 17.118594f, 17.128920f, 17.140290f, 17.148137f, 17.152436f, 17.153188f, 17.150393f, 17.144050f, 17.134160f, 17.120722f, 17.103140f, 17.080001f, -17.061286f, 17.048784f, 17.042496f, 17.042421f, 17.048560f, 17.060912f, 17.079477f, 17.104256f, 17.138775f, 17.169099f, 17.180444f, 17.172741f, 17.150483f, 17.137247f, 17.132223f, 17.134507f, 17.143959f, 17.154727f, 17.159052f, 17.156405f, 17.146808f, 17.135534f, 17.128400f, 17.124405f, 17.123550f, 17.124154f, 17.122734f, 17.121089f, 17.119292f, 17.117173f, 17.114367f, 17.112191f, 17.110812f, 17.110229f, 17.110444f, 17.111455f, 17.113263f, 17.115868f, 17.118727f, 17.120914f, 17.122544f, 17.123617f, 17.124132f, 17.124090f, 17.123490f, 17.122333f, 17.120636f, 17.118895f, 17.117673f, 17.116999f, -17.116875f, 17.117300f, 17.118275f, 17.119798f, 17.121870f, 17.124485f, 17.127082f, 17.129287f, 17.131100f, 17.132518f, 17.133543f, 17.134175f, 17.134413f, 17.134257f, 17.133708f, 17.132765f, 17.131429f, 17.129699f, 17.127576f, 17.125059f, 17.122148f, 17.118844f, 17.113903f, 17.107135f, 17.102381f, 17.099763f, 17.099281f, 17.100936f, 17.104726f, 17.110653f, 17.118711f, 17.127696f, 17.134873f, 17.139867f, 17.142677f, 17.143305f, 17.141749f, 17.138010f, 17.132087f, 17.124327f, 17.117419f, 17.112502f, 17.109569f, 17.108619f, 17.109654f, 17.112672f, 17.117674f, 17.124672f, 17.133632f, 17.141177f, -17.146130f, 17.148491f, 17.148259f, 17.145435f, 17.140019f, 17.132011f, 17.120885f, 17.107585f, 17.097957f, 17.092498f, 17.091206f, 17.094083f, 17.101126f, 17.112338f, 17.127720f, 17.150100f, 17.171317f, 17.185083f, 17.191398f, 17.190263f, 17.181677f, 17.165640f, 17.142153f, 17.109867f, 17.068792f, 17.038996f, 17.023161f, 17.021288f, 17.033375f, 17.059424f, 17.099434f, 17.155415f, 17.211143f, 17.245712f, 17.258657f, 17.249978f, 17.219676f, 17.167749f, 17.095619f, 17.032702f, 16.997760f, 16.990937f, 17.012233f, 17.061647f, 17.138084f, 17.211527f, 17.252481f, 17.259855f, 17.233650f, 17.173866f, -17.093691f, 17.036429f, 17.010625f, 17.016277f, 17.053385f, 17.121388f, 17.194147f, 17.235182f, 17.241794f, 17.213984f, 17.152751f, 17.088358f, 17.052002f, 17.044636f, 17.066261f, 17.116415f, 17.172759f, 17.205118f, 17.211553f, 17.192061f, 17.146893f, 17.094219f, 17.062783f, 17.054965f, 17.070763f, 17.110106f, 17.159439f, 17.189390f, 17.196123f, 17.179638f, 17.139851f, 17.083775f, 17.047920f, 17.041276f, 17.063842f, 17.115711f, 17.193551f, 17.246950f, 17.258773f, 17.229021f, 17.157657f, 17.047789f, 16.969657f, 16.951286f, 16.992677f, 17.093829f, 17.254379f, 17.369835f, 17.388530f, 17.310464f, -17.136470f, 16.946190f, 16.860522f, 16.888633f, 17.030523f, 17.275228f, 17.459279f, 17.496289f, 17.386027f, 17.127578f, 16.820783f, 16.689458f, 16.759469f, 17.030850f, 17.387357f, 17.553797f, 17.492732f, 17.204027f, 16.797640f, 16.595300f, 16.653019f, 16.971226f, 17.482303f, 17.777523f, 17.740969f, 17.372639f, 16.774261f, 16.412611f, 16.418513f, 16.791969f, 17.484706f, 17.947040f, 17.910157f, 17.377452f, 16.686407f, 16.405789f, 16.588043f, 17.218716f, 17.826500f, 17.891662f, 17.395186f, 16.731922f, 16.513334f, 16.787348f, 17.423613f, 17.745975f, 17.536377f, 17.001361f, 16.754971f, 16.875984f, -17.217086f, 17.361654f, 17.258230f, 17.004395f, 16.896772f, 16.984114f, 17.185740f, 17.272936f, 17.211331f, 17.069792f, 17.014473f, 17.062904f, 17.161676f, 17.203761f, 17.183715f, 17.138920f, 17.124001f, 17.135436f, 17.158529f, 17.166720f, 17.158838f, 17.144614f, 17.139080f, 17.142281f, 17.149747f, 17.154123f, 17.155101f, 17.152683f, 17.146868f, 17.137612f, 17.124578f, 17.114649f, 17.110232f, 17.111327f, 17.117936f, 17.130039f, 17.144454f, 17.154375f, 17.158932f, 17.158125f, 17.151952f, 17.140414f, 17.126277f, 17.116831f, 17.113210f, 17.115413f, 17.123441f, 17.137329f, 17.154368f, 17.165940f, -17.170616f, 17.168397f, 17.159283f, 17.143175f, 17.123046f, 17.109896f, 17.105537f, 17.109970f, 17.123195f, 17.145513f, 17.174057f, 17.192684f, 17.198664f, 17.191998f, 17.172685f, 17.140513f, 17.102492f, 17.078016f, 17.069548f, 17.077087f, 17.100633f, 17.140415f, 17.186322f, 17.214985f, 17.223869f, 17.212975f, 17.182304f, 17.131805f, 17.076241f, 17.041763f, 17.030634f, 17.042853f, 17.078421f, 17.136064f, 17.192475f, 17.227989f, 17.242062f, 17.234695f, 17.205887f, 17.158179f, 17.117207f, 17.092300f, 17.082725f, 17.088485f, 17.109579f, 17.144212f, 17.174492f, 17.192445f, 17.198302f, 17.192060f, -17.173720f, 17.144898f, 17.119981f, 17.104937f, 17.099615f, 17.104017f, 17.118142f, 17.140249f, 17.158227f, 17.168870f, 17.172387f, 17.168776f, 17.158039f, 17.141471f, 17.127889f, 17.120051f, 17.117872f, 17.121353f, 17.130495f, 17.144073f, 17.154974f, 17.161426f, 17.163496f, 17.161186f, 17.154493f, 17.144572f, 17.137010f, 17.132736f, 17.131697f, 17.133894f, 17.139326f, 17.147113f, 17.153144f, 17.156619f, 17.157560f, 17.155968f, 17.151842f, 17.146346f, 17.142425f, 17.139360f, 17.137112f, 17.135680f, 17.135064f, 17.135266f, 17.136284f, 17.138119f, 17.140770f, 17.144238f, 17.148523f, 17.153806f, -17.159002f, 17.163045f, 17.165929f, 17.167654f, 17.168219f, 17.167625f, 17.165872f, 17.162959f, 17.158887f, 17.153552f, 17.142846f, 17.132186f, 17.125080f, 17.121530f, 17.121534f, 17.125092f, 17.132206f, 17.142875f, 17.157098f, 17.174876f, 17.199034f, 17.241100f, 17.266704f, 17.269161f, 17.248473f, 17.204664f, 17.153607f, 17.118763f, 17.098656f, 17.093285f, 17.102651f, 17.121212f, 17.128912f, 17.131624f, 17.130157f, 17.124511f, 17.114451f, 17.100359f, 17.091754f, 17.090544f, 17.096730f, 17.110312f, 17.126581f, 17.137666f, 17.145146f, 17.149027f, 17.149309f, 17.147337f, 17.148861f, 17.150580f, -17.151938f, 17.152933f, 17.153607f, 17.154989f, 17.156483f, 17.157608f, 17.158364f, 17.158752f, 17.158770f, 17.158419f, 17.157700f, 17.156611f, 17.155154f, 17.153320f, 17.151364f, 17.149788f, 17.148634f, 17.147903f, 17.147594f, 17.147707f, 17.148243f, 17.149202f, 17.150582f, 17.152386f, 17.154665f, 17.156997f, 17.158803f, 17.160066f, 17.160786f, 17.160965f, 17.160601f, 17.159694f, 17.158246f, 17.156255f, 17.153721f, 17.150495f, 17.147469f, 17.145187f, 17.143651f, 17.142861f, 17.142816f, 17.143517f, 17.144963f, 17.147155f, 17.150092f, 17.153797f, 17.158162f, 17.161864f, 17.164624f, 17.166442f, -17.167319f, 17.167254f, 17.166247f, 17.164298f, 17.161408f, 17.157576f, 17.152736f, 17.147489f, 17.143301f, 17.140283f, 17.138437f, 17.137761f, 17.138257f, 17.139923f, 17.142760f, 17.146768f, 17.151946f, 17.158307f, 17.164450f, 17.169234f, 17.172644f, 17.174680f, 17.175343f, 17.174631f, 17.172546f, 17.169088f, 17.164255f, 17.158048f, 17.150889f, 17.144704f, 17.139984f, 17.136729f, 17.134938f, 17.134611f, 17.135750f, 17.138353f, 17.142421f, 17.147954f, 17.154890f, 17.161750f, 17.167314f, 17.171593f, 17.174585f, 17.176291f, 17.176711f, 17.175845f, 17.173693f, 17.170255f, 17.165530f, 17.159579f, -17.153725f, 17.149183f, 17.145995f, 17.144161f, 17.143680f, 17.144554f, 17.146781f, 17.150362f, 17.155297f, 17.161586f, 17.172204f, 17.185342f, 17.194240f, 17.198838f, 17.199137f, 17.195136f, 17.186836f, 17.174237f, 17.157338f, 17.135615f, 17.100103f, 17.069186f, 17.050530f, 17.044136f, 17.050004f, 17.068133f, 17.098524f, 17.141176f, 17.199011f, 17.256646f, 17.294777f, 17.313128f, 17.311700f, 17.290493f, 17.249506f, 17.189165f, 17.126436f, 17.080456f, 17.051819f, 17.040526f, 17.046577f, 17.069972f, 17.110711f, 17.164189f, 17.207196f, 17.234827f, 17.247247f, 17.244456f, 17.226453f, 17.193330f, -17.159281f, 17.136875f, 17.122916f, 17.117404f, 17.120340f, 17.131723f, 17.150311f, 17.164135f, 17.172849f, 17.177559f, 17.178265f, 17.174967f, 17.167665f, 17.157695f, 17.150217f, 17.145815f, 17.144459f, 17.146150f, 17.150888f, 17.158674f, 17.168134f, 17.175109f, 17.178843f, 17.179336f, 17.176587f, 17.170596f, 17.161326f, 17.151369f, 17.145037f, 17.142637f, 17.144171f, 17.149637f, 17.159036f, 17.182643f, 17.213108f, 17.224750f, 17.217295f, 17.190745f, 17.145098f, 17.076521f -}; +const float ADA_SIMULATED_PRESSURE_SPEED[] = { + -0.948363f, -0.948363f, -13.274254f, -40.403141f, -79.405710f, + -119.092389f, -162.040265f, -202.082736f, -248.586283f, -290.768883f, + -337.645715f, -384.558541f, -432.175827f, -480.794502f, -529.952635f, + -579.542043f, -629.587822f, -679.694102f, -730.011778f, -780.203192f, + -829.813236f, -880.431945f, -926.994001f, -976.108202f, -1023.883786f, + -1068.006391f, -1112.696323f, -1154.021320f, -1195.316651f, -1234.653809f, + -1273.929067f, -1310.511099f, -1347.088722f, -1382.465466f, -1416.661632f, + -1449.831084f, -1481.806571f, -1513.344488f, -1540.857087f, -1568.891304f, + -1594.511632f, -1618.411951f, -1640.640080f, -1661.274003f, -1680.543531f, + -1698.611728f, -1715.810816f, -1732.354168f, -1748.137780f, -1763.107652f, + -1776.721145f, -1788.499697f, -1798.538005f, -1806.435685f, -1812.224381f, + -1815.949022f, -1817.553582f, -1817.171319f, -1814.966301f, -1811.231493f, + -1806.753756f, -1801.857044f, -1796.509169f, -1790.627019f, -1784.103445f, + -1777.117924f, -1769.990709f, -1762.227380f, -1754.404480f, -1745.979311f, + -1737.335373f, -1728.525613f, -1719.795091f, -1711.258934f, -1702.929259f, + -1694.800914f, -1686.813373f, -1678.869836f, -1671.086967f, -1663.075796f, + -1655.039355f, -1646.941808f, -1638.694844f, -1630.433859f, -1622.248517f, + -1613.625054f, -1605.889735f, -1597.673857f, -1589.993489f, -1582.175234f, + -1574.569597f, -1567.002497f, -1559.461641f, -1551.920197f, -1543.990283f, + -1536.939837f, -1528.941596f, -1521.197540f, -1513.481103f, -1505.580544f, + -1498.009587f, -1490.323611f, -1482.776293f, -1475.332504f, -1467.997921f, + -1460.717334f, -1453.574783f, -1446.397141f, -1439.208953f, -1432.054093f, + -1424.858679f, -1417.619623f, -1410.407338f, -1403.087371f, -1395.818595f, + -1388.674458f, -1380.934283f, -1374.409215f, -1367.040802f, -1359.990206f, + -1352.955497f, -1346.187796f, -1339.196030f, -1332.469424f, -1325.581498f, + -1318.828149f, -1312.021043f, -1305.204915f, -1298.391547f, -1291.524853f, + -1284.758862f, -1277.881065f, -1271.019585f, -1264.245204f, -1257.465943f, + -1250.744429f, -1244.071819f, -1237.451756f, -1230.887296f, -1224.369212f, + -1217.892631f, -1211.445773f, -1205.017845f, -1198.602846f, -1192.194381f, + -1185.786708f, -1179.378571f, -1172.971336f, -1166.502184f, -1160.135006f, + -1153.662157f, -1147.259526f, -1140.955712f, -1134.153584f, -1128.135193f, + -1122.182990f, -1115.614306f, -1109.342914f, -1103.202287f, -1097.032092f, + -1090.888438f, -1084.926840f, -1078.721265f, -1072.685980f, -1066.649538f, + -1060.612288f, -1054.572058f, -1048.557423f, -1042.504375f, -1036.526748f, + -1030.554832f, -1024.465625f, -1018.501725f, -1012.513563f, -1006.546597f, + -1000.598756f, -994.666982f, -988.780946f, -983.229207f, -976.066977f, + -971.068754f, -965.819476f, -959.862593f, -953.788488f, -948.079682f, + -942.534301f, -936.654908f, -930.943397f, -925.285974f, -919.712081f, + -914.143991f, -908.340575f, -902.711404f, -897.145734f, -891.495687f, + -885.841221f, -880.343976f, -874.712701f, -869.088132f, -863.571373f, + -857.911559f, -852.415494f, -846.870857f, -841.360507f, -835.847587f, + -830.379206f, -824.903727f, -819.469763f, -814.032513f, -808.630569f, + -803.209037f, -797.943025f, -792.531988f, -787.160589f, -781.845686f, + -776.541833f, -771.236783f, -765.948975f, -760.656941f, -755.392663f, + -750.474171f, -744.119438f, -739.368150f, -734.811494f, -729.390960f, + -723.916522f, -718.757363f, -713.514328f, -708.282230f, -703.171187f, + -697.987194f, -692.762326f, -687.681524f, -682.559624f, -677.369340f, + -672.302322f, -667.244692f, -662.109798f, -657.033467f, -652.040492f, + -646.968855f, -641.889054f, -636.948245f, -631.935895f, -626.861884f, + -621.941216f, -616.985557f, -611.937612f, -607.009210f, -602.108093f, + -597.092236f, -592.091690f, -587.222108f, -582.266663f, -577.278266f, + -572.456691f, -567.551570f, -562.542502f, -557.678809f, -552.825696f, + -547.895296f, -543.060442f, -538.289163f, -533.401076f, -528.500840f, + -523.761905f, -518.928770f, -514.025605f, -509.298459f, -504.527584f, + -499.662678f, -494.943221f, -490.239079f, -485.415426f, -480.637675f, + -475.986668f, -471.220600f, -466.417382f, -461.802760f, -457.092949f, + -452.367494f, -448.454713f, -443.428587f, -436.768857f, -431.720285f, + -428.436103f, -424.690453f, -420.483337f, -415.814754f, -410.684704f, + -405.293589f, -400.769121f, -396.352320f, -391.866193f, -387.310742f, + -382.685966f, -377.991866f, -373.376069f, -368.866194f, -364.279610f, + -359.615876f, -354.874993f, -350.057910f, -345.601793f, -341.369310f, + -336.968681f, -332.399905f, -327.662983f, -322.769446f, -318.371461f, + -314.180197f, -309.805962f, -305.248756f, -300.508579f, -295.620549f, + -291.338380f, -287.186287f, -282.834150f, -278.281970f, -273.529748f, + -268.653028f, -264.488035f, -260.374530f, -256.040505f, -251.485963f, + -246.710901f, -241.853583f, -237.809455f, -233.735053f, -229.414096f, + -224.846583f, -220.032514f, -215.202584f, -211.278819f, -207.241447f, + -202.927874f, -198.338100f, -193.472125f, -188.694335f, -184.904575f, + -180.910215f, -176.598786f, -171.970288f, -167.024721f, -162.804402f, + -160.333689f, -156.612294f, -151.503833f, -145.008306f, -137.125714f, + -130.607074f, -128.233382f, -125.671089f, -122.828499f, -119.705613f, + -116.302430f, -112.618951f, -108.655176f, -104.411103f, -99.886735f, + -95.082070f, -89.998441f, -85.471831f, -81.320716f, -76.805696f, + -71.926771f, -66.683942f, -61.124979f, -58.025683f, -55.607063f, + -51.974028f, -47.126579f, -41.064716f, -34.013352f, -31.082730f, + -29.184286f, -26.054765f, -21.694166f, -16.102490f, -9.710043f, + -7.113163f, -3.699538f, 1.261139f, 4.956837f, 8.178839f, + 11.089240f, 13.877067f, 16.424262f, 18.969779f, 21.425959f, + 23.829639f, 26.190556f, 28.515170f, 30.807700f, 33.070689f, + 35.305395f, 37.482641f, 39.800572f, 41.849296f, 43.849612f, + 46.081010f, 48.067826f, 50.140332f, 52.069499f, 54.044469f, + 55.906150f, 57.774978f, 59.559850f, 61.314128f, 63.013778f, + 64.542838f, 66.321117f, 68.044085f, 69.336995f, 70.603586f, + 71.847410f, 73.183871f, 74.647901f, 75.900694f, 76.931359f, + 77.873919f, 79.069182f, 80.114781f, 80.973104f, 81.714515f, + 82.665612f, 83.519602f, 84.219700f, 84.797987f, 85.533954f, + 86.218707f, 86.779738f, 87.225675f, 87.738076f, 88.271803f, + 88.771024f, 89.235311f, 89.639452f, 89.997206f, 90.324905f, + 90.622550f, 90.890140f, 91.127676f, 91.335156f, 91.512583f, + 91.734407f, 92.035394f, 92.299645f, 92.525915f, 92.714206f, + 92.864516f, 92.976846f, 93.051195f, 93.120986f, 93.283226f, + 93.435300f, 93.566170f, 93.675835f, 93.764296f, 93.831552f, + 93.877603f, 93.908696f, 93.986962f, 94.071469f, 94.145719f, + 94.209712f, 94.263448f, 94.306928f, 94.340151f, 94.363302f, + 94.393229f, 94.429648f, 94.463797f, 94.495675f, 94.525282f, + 94.552619f, 94.577684f, 94.600479f, 94.621003f, 94.639255f, + 94.655237f, 94.668949f, 94.680389f, 94.689558f, 94.696457f, + 94.701085f, 94.707253f, 94.726611f, 94.746091f, 94.764164f, + 94.780830f, 94.796090f, 94.809944f, 94.822391f, 94.833431f, + 94.843064f, 94.851291f, 94.858112f, 94.863526f, 94.867533f, + 94.870134f, 94.871328f, 94.871191f, 94.878544f, 94.890648f, + 94.901428f, 94.910882f, 94.919011f, 94.925815f, 94.931294f, + 94.935448f, 94.938277f, 94.939781f, 94.939959f, 94.938813f, + 94.936341f, 94.932545f, 94.927423f, 94.920976f, 94.914409f, + 94.912257f, 94.910765f, 94.909394f, 94.908142f, 94.907011f, + 94.905999f, 94.905106f, 94.904334f, 94.903681f, 94.903149f, + 94.902736f, 94.902443f, 94.902269f, 94.902216f, 94.902282f, + 94.902434f, 94.896842f, 94.888282f, 94.881575f, 94.876721f, + 94.873720f, 94.872572f, 94.873278f, 94.875836f, 94.880247f, + 94.886511f, 94.894628f, 94.904599f, 94.916422f, 94.930098f, + 94.945628f, 94.963010f, 94.981112f, 94.994828f, 95.007306f, + 95.019117f, 95.030260f, 95.040735f, 95.050543f, 95.059683f, + 95.068155f, 95.075960f, 95.083097f, 95.089566f, 95.095368f, + 95.100502f, 95.104968f, 95.108767f, 95.111943f, 95.127988f, + 95.151816f, 95.171674f, 95.187562f, 95.199479f, 95.207426f, + 95.211403f, 95.211410f, 95.207447f, 95.199513f, 95.187609f, + 95.171735f, 95.151891f, 95.128076f, 95.100291f, 95.068536f, + 95.033090f, 94.998207f, 94.967148f, 94.939993f, 94.916741f, + 94.897392f, 94.881947f, 94.870406f, 94.862768f, 94.859033f, + 94.859202f, 94.863274f, 94.871249f, 94.883128f, 94.898911f, + 94.918596f, 94.942137f, 94.935235f, 94.905522f, 94.883777f, + 94.870001f, 94.864195f, 94.866358f, 94.876489f, 94.894590f, + 94.920659f, 94.954698f, 94.996706f, 95.046683f, 95.104629f, + 95.170544f, 95.244427f, 95.326280f, 95.425384f, 95.571000f, + 95.704180f, 95.816472f, 95.907876f, 95.978392f, 96.028020f, + 96.056760f, 96.064612f, 96.051577f, 96.017653f, 95.962841f, + 95.887141f, 95.790554f, 95.673078f, 95.534715f, 95.375456f, + 95.184213f, 95.006962f, 94.875436f, 94.789634f, 94.749556f, + 94.755203f, 94.806573f, 94.875420f, 94.857304f, 94.844080f, + 94.848751f, 94.871317f, 94.911778f, 94.970134f, 95.046420f, + 95.133913f, 95.207073f, 95.260493f, 95.294175f, 95.308118f, + 95.302323f, 95.276789f, 95.245125f, 95.239799f, 95.237223f, + 95.235781f, 95.235472f, 95.236297f, 95.238255f, 95.240892f, + 95.238018f, 95.235537f, 95.236081f, 95.239649f, 95.246242f, + 95.255860f, 95.268502f, 95.280038f, 95.286065f, 95.291866f, + 95.297523f, 95.303038f, 95.308409f, 95.313637f, 95.318723f, + 95.323665f, 95.328463f, 95.333119f, 95.337632f, 95.342001f, + 95.346228f, 95.350311f, 95.355889f, 95.362632f, 95.368844f, + 95.374527f, 95.379680f, 95.384303f, 95.388397f, 95.391960f, + 95.394995f, 95.397499f, 95.399474f, 95.400919f, 95.401834f, + 95.402220f, 95.402110f, 95.403448f, 95.405735f, 95.407955f, + 95.410108f, 95.412193f, 95.414211f, 95.416161f, 95.418043f, + 95.419858f, 95.421606f, 95.423286f, 95.424898f, 95.426444f, + 95.427921f, 95.429309f, 95.430336f, 95.431390f, 95.432619f, + 95.434025f, 95.435606f, 95.437362f, 95.439295f, 95.441403f, + 95.443687f, 95.446147f, 95.448782f, 95.451593f, 95.454580f, + 95.457742f, 95.460880f, 95.462555f, 95.463923f, 95.465319f, + 95.466743f, 95.468195f, 95.469675f, 95.471183f, 95.472719f, + 95.474283f, 95.475876f, 95.477496f, 95.479144f, 95.480820f, + 95.482524f, 95.484256f, 95.486016f, 95.487804f, 95.489620f, + 95.491465f, 95.493337f, 95.495237f, 95.497165f, 95.499121f, + 95.501105f, 95.503117f, 95.505157f, 95.507226f, 95.509322f, + 95.511446f, 95.513383f, 95.514773f, 95.516185f, 95.517655f, + 95.519182f, 95.520767f, 95.522410f, 95.524110f, 95.525868f, + 95.527683f, 95.529556f, 95.531486f, 95.533474f, 95.535520f, + 95.537623f, 95.539783f, 95.542002f, 95.544277f, 95.546611f, + 95.549002f, 95.551450f, 95.553956f, 95.556520f, 95.559141f, + 95.561819f, 95.564556f, 95.567349f, 95.570201f, 95.573110f, + 95.576076f, 95.577858f, 95.578561f, 95.579508f, 95.580699f, + 95.582133f, 95.583812f, 95.585735f, 95.587901f, 95.590312f, + 95.592966f, 95.595864f, 95.599007f, 95.602393f, 95.606023f, + 95.609897f, 95.614015f, 95.618377f, 95.622983f, 95.627833f, + 95.632926f, 95.638264f, 95.643846f, 95.649671f, 95.655741f, + 95.662054f, 95.668611f, 95.675320f, 95.680205f, 95.684411f, + 95.688763f, 95.693261f, 95.697905f, 95.702696f, 95.707634f, + 95.712717f, 95.717948f, 95.723324f, 95.728847f, 95.734517f, + 95.740333f, 95.746295f, 95.752403f, 95.758658f, 95.765060f, + 95.771608f, 95.778302f, 95.785143f, 95.792130f, 95.799263f, + 95.806543f, 95.814132f, 95.823742f, 95.833521f, 95.842796f, + 95.851565f, 95.859828f, 95.867586f, 95.874839f, 95.881586f, + 95.887827f, 95.893563f, 95.898794f, 95.903519f, 95.907739f, + 95.911453f, 95.914662f, 95.917366f, 95.919564f, 95.921256f, + 95.922443f, 95.923125f, 95.923301f, 95.925791f, 95.933186f, + 95.940043f, 95.946219f, 95.951713f, 95.956526f, 95.960657f, + 95.964107f, 95.966875f, 95.968962f, 95.970368f, 95.971091f, + 95.971134f, 95.970495f, 95.969174f, 95.967172f, 95.964488f, + 95.961123f, 95.957076f, 95.952348f, 95.946938f, 95.941124f, + 95.939281f, 95.938784f, 95.938085f, 95.937185f, 95.936083f, + 95.934779f, 95.933274f, 95.931568f, 95.929659f, 95.927549f, + 95.925238f, 95.922725f, 95.920010f, 95.917094f, 95.913976f, + 95.910656f, 95.907135f, 95.903413f, 95.899488f, 95.895362f, + 95.891035f, 95.885223f, 95.876891f, 95.869337f, 95.862679f, + 95.856918f, 95.852055f, 95.848088f, 95.845018f, 95.842846f, + 95.841570f, 95.841191f, 95.841710f, 95.843125f, 95.845437f, + 95.848646f, 95.852753f, 95.857756f, 95.863656f, 95.870453f, + 95.877222f, 95.876480f, 95.874586f, 95.873550f, 95.873371f, + 95.874050f, 95.875586f, 95.877980f, 95.881231f, 95.885340f, + 95.890306f, 95.896130f, 95.902811f, 95.910350f, 95.918746f, + 95.928000f, 95.938111f, 95.949079f, 95.960906f, 95.973586f, + 95.984581f, 95.993505f, 96.002256f, 96.010835f, 96.019240f, + 96.027473f, 96.035534f, 96.043422f, 96.051137f, 96.058679f, + 96.066049f, 96.073246f, 96.080270f, 96.087122f, 96.093801f, + 96.100307f, 96.106641f, 96.112802f, 96.118790f, 96.126106f, + 96.137803f, 96.148992f, 96.159283f, 96.168674f, 96.177166f, + 96.184759f, 96.191453f, 96.197248f, 96.202144f, 96.206140f, + 96.209238f, 96.211436f, 96.212736f, 96.213136f, 96.212637f, + 96.211239f, 96.208942f, 96.205746f, 96.202172f, 96.206080f, + 96.211983f, 96.216952f, 96.220985f, 96.224083f, 96.226245f, + 96.227472f, 96.227763f, 96.227120f, 96.225540f, 96.223026f, + 96.219576f, 96.215191f, 96.209870f, 96.203614f, 96.196422f, + 96.188295f, 96.179233f, 96.169236f, 96.158018f, 96.146775f, + 96.136665f, 96.127694f, 96.119860f, 96.113164f, 96.107605f, + 96.103185f, 96.099902f, 96.097757f, 96.096749f, 96.096880f, + 96.098148f, 96.100554f, 96.104098f, 96.108779f, 96.114599f, + 96.121556f, 96.129651f, 96.136653f, 96.134085f, 96.131201f, + 96.129394f, 96.128664f, 96.129010f, 96.130434f, 96.132934f, + 96.136511f, 96.141165f, 96.146895f, 96.153703f, 96.161587f, + 96.170548f, 96.180586f, 96.191701f, 96.203893f, 96.217161f, + 96.231506f, 96.246885f, 96.261347f, 96.274526f, 96.287027f, + 96.298848f, 96.309991f, 96.320455f, 96.330239f, 96.339345f, + 96.347771f, 96.355519f, 96.362587f, 96.368976f, 96.374687f, + 96.379718f, 96.384071f, 96.387744f, 96.390739f, 96.396351f, + 96.408823f, 96.420312f, 96.430472f, 96.439304f, 96.446806f, + 96.452981f, 96.457826f, 96.461343f, 96.463531f, 96.464390f, + 96.463920f, 96.462122f, 96.458995f, 96.454539f, 96.448755f, + 96.441641f, 96.433630f, 96.430547f, 96.429176f, 96.427751f, + 96.426270f, 96.424734f, 96.423143f, 96.421497f, 96.419795f, + 96.418039f, 96.416227f, 96.414360f, 96.412439f, 96.410462f, + 96.408430f, 96.406342f, 96.404200f, 96.402002f, 96.398006f, + 96.391886f, 96.386596f, 96.382161f, 96.378582f, 96.375859f, + 96.373991f, 96.372979f, 96.372823f, 96.373523f, 96.375078f, + 96.377489f, 96.380755f, 96.384878f, 96.389856f, 96.395689f, + 96.402379f, 96.409073f, 96.410492f, 96.411274f, 96.412575f, + 96.414393f, 96.416730f, 96.419585f, 96.422958f, 96.426849f, + 96.431258f, 96.436185f, 96.441630f, 96.447594f, 96.454075f, + 96.461075f, 96.468593f, 96.476629f, 96.485182f, 96.493559f, + 96.501244f, 96.508588f, 96.515591f, 96.522253f, 96.528573f, + 96.534553f, 96.540191f, 96.545489f, 96.550445f, 96.555060f, + 96.559334f, 96.563267f, 96.566859f, 96.570110f, 96.573020f, + 96.575588f, 96.578902f, 96.586245f, 96.593374f, 96.599779f, + 96.605459f, 96.610414f, 96.614644f, 96.618150f, 96.620931f, + 96.622988f, 96.624319f, 96.624926f, 96.624809f, 96.623966f, + 96.622399f, 96.620108f, 96.617091f, 96.613400f, 96.611535f, + 96.610967f, 96.610492f, 96.610110f, 96.609821f, 96.609624f, + 96.609521f, 96.609510f, 96.609592f, 96.609767f, 96.610035f, + 96.610395f, 96.610848f, 96.611394f, 96.612033f, 96.612765f, + 96.613589f, 96.612641f, 96.607452f, 96.603151f, 96.600003f, + 96.598008f, 96.597167f, 96.597478f, 96.598942f, 96.601559f, + 96.605330f, 96.610253f, 96.616329f, 96.623559f, 96.631941f, + 96.641477f, 96.652165f, 96.664007f, 96.676778f, 96.686737f, + 96.695272f, 96.703491f, 96.711395f, 96.718984f, 96.726257f, + 96.733215f, 96.739858f, 96.746186f, 96.752198f, 96.757895f, + 96.763277f, 96.768343f, 96.773094f, 96.777530f, 96.781650f, + 96.785456f, 96.795482f, 96.814460f, 96.830799f, 96.844335f, + 96.855068f, 96.862999f, 96.868127f, 96.870453f, 96.869976f, + 96.866697f, 96.860615f, 96.851731f, 96.840044f, 96.825555f, + 96.808263f, 96.788168f, 96.765272f, 96.739962f, 96.716653f, + 96.695957f, 96.677482f, 96.661227f, 96.647193f, 96.635380f, + 96.625787f, 96.618414f, 96.613262f, 96.610331f, 96.609620f, + 96.611129f, 96.614859f, 96.620810f, 96.628981f, 96.639373f, + 96.651985f, 96.640816f, 96.609258f, 96.584664f, 96.567033f, + 96.556365f, 96.552661f, 96.555919f, 96.566141f, 96.583326f, + 96.607474f, 96.638586f, 96.676661f, 96.721699f, 96.773700f, + 96.832664f, 96.898592f, 96.971482f, 97.058486f, 97.176150f, + 97.283359f, 97.375360f, 97.452152f, 97.513736f, 97.560111f, + 97.591278f, 97.607236f, 97.607985f, 97.593525f, 97.563857f, + 97.518981f, 97.458896f, 97.383602f, 97.293099f, 97.187388f, + 97.066495f, 96.939865f, 96.830590f, 96.742106f, 96.674414f, + 96.627512f, 96.601402f, 96.596083f, 96.611555f, 96.637171f, + 96.594325f, 96.551892f, 96.529899f, 96.528347f, 96.547236f, + 96.586566f, 96.646337f, 96.726549f, 96.822686f, 96.908184f, + 96.976272f, 97.027193f, 97.060947f, 97.077533f, 97.076953f, + 97.059205f, 97.024291f, 97.003520f, 97.007412f, 97.008029f, + 97.005365f, 96.999422f, 96.990200f, 96.977698f, 96.961916f, + 96.942105f, 96.908733f, 96.879464f, 96.860835f, 96.852844f, + 96.855491f, 96.868778f, 96.892703f, 96.927266f, 96.969383f, + 97.001060f, 97.026524f, 97.047222f, 97.063155f, 97.074321f, + 97.080722f, 97.082356f, 97.079225f, 97.079171f, 97.089596f, + 97.097627f, 97.102993f, 97.105693f, 97.105727f, 97.103095f, + 97.097798f, 97.089864f, 97.082353f, 97.077180f, 97.073480f, + 97.071252f, 97.070498f, 97.071216f, 97.073407f, 97.077071f, + 97.081569f, 97.081531f, 97.080659f, 97.080231f, 97.080246f, + 97.080705f, 97.081607f, 97.082953f, 97.084742f, 97.086974f, + 97.089650f, 97.092769f, 97.096332f, 97.100338f, 97.104787f, + 97.109680f, 97.115016f, 97.120796f, 97.125695f, 97.129304f, + 97.132923f, 97.136553f, 97.140193f, 97.143843f, 97.147504f, + 97.151176f, 97.154858f, 97.158551f, 97.162255f, 97.165969f, + 97.169693f, 97.173428f, 97.177174f, 97.180930f, 97.184697f, + 97.188211f, 97.190520f, 97.192940f, 97.195665f, 97.198695f, + 97.202031f, 97.205673f, 97.209621f, 97.213874f, 97.218432f, + 97.223297f, 97.228467f, 97.233942f, 97.239724f, 97.245810f, + 97.252203f, 97.258901f, 97.265912f, 97.273753f, 97.281706f, + 97.289230f, 97.296324f, 97.302990f, 97.309225f, 97.315032f, + 97.320409f, 97.325357f, 97.329875f, 97.333964f, 97.337623f, + 97.340854f, 97.343655f, 97.346026f, 97.347968f, 97.349481f, + 97.350565f, 97.351219f, 97.351444f, 97.351239f, 97.350605f, + 97.349542f, 97.348049f, 97.346127f, 97.343776f, 97.340995f, + 97.337785f, 97.334145f, 97.330076f, 97.325578f, 97.320651f, + 97.315294f, 97.309508f, 97.303308f, 97.297327f, 97.292069f, + 97.287490f, 97.283591f, 97.280371f, 97.277830f, 97.275969f, + 97.274787f, 97.274284f, 97.274461f, 97.275317f, 97.276852f, + 97.279067f, 97.281961f, 97.282869f, 97.278361f, 97.274854f, + 97.272637f, 97.271708f, 97.272068f, 97.273717f, 97.276655f, + 97.280882f, 97.286398f, 97.293203f, 97.301297f, 97.310679f, + 97.321351f, 97.333121f, 97.341952f, 97.349242f, 97.356364f, + 97.363318f, 97.370105f, 97.376724f, 97.383175f, 97.389459f, + 97.395575f, 97.401523f, 97.407303f, 97.412916f, 97.418361f, + 97.423639f, 97.428772f, 97.433968f, 97.439334f, 97.444869f, + 97.450575f, 97.456451f, 97.462497f, 97.468714f, 97.475100f, + 97.481657f, 97.488384f, 97.495281f, 97.502348f, 97.509586f, + 97.517207f, 97.528578f, 97.540563f, 97.551619f, 97.561746f, + 97.570945f, 97.579214f, 97.586555f, 97.592967f, 97.598451f, + 97.603005f, 97.606631f, 97.609328f, 97.611096f, 97.611935f, + 97.611846f, 97.610827f, 97.608880f, 97.606005f, 97.602200f, + 97.597467f, 97.591804f, 97.585213f, 97.577694f, 97.569245f, + 97.559868f, 97.549562f, 97.538327f, 97.526163f, 97.512770f, + 97.494631f, 97.477584f, 97.463734f, 97.453081f, 97.445624f, + 97.441364f, 97.440301f, 97.442434f, 97.447763f, 97.456290f, + 97.468012f, 97.474644f, 97.473067f, 97.473084f, 97.474727f, + 97.477996f, 97.482892f, 97.489414f, 97.497563f, 97.507338f, + 97.518740f, 97.531768f, 97.546015f, 97.558979f, 97.570675f, + 97.581225f, 97.590628f, 97.598884f, 97.605994f, 97.611957f, + 97.616773f, 97.620443f, 97.622966f, 97.624515f, 97.629297f, + 97.635341f, 97.640914f, 97.646015f, 97.650644f, 97.654801f, + 97.658487f, 97.661700f, 97.664442f, 97.666712f, 97.668510f, + 97.669944f, 97.671391f, 97.672908f, 97.674494f, 97.676150f, + 97.677875f, 97.679670f, 97.681534f, 97.683468f, 97.685471f, + 97.687543f, 97.689685f, 97.691896f, 97.694177f, 97.696527f, + 97.698947f, 97.701436f, 97.703995f, 97.706623f, 97.709320f, + 97.712087f, 97.714924f, 97.717821f, 97.720422f, 97.722852f, + 97.725282f, 97.727715f, 97.730149f, 97.732584f, 97.735020f, + 97.737458f, 97.739898f, 97.742338f, 97.744781f, 97.747224f, + 97.749669f, 97.752116f, 97.754563f, 97.757013f, 97.759463f, + 97.761915f, 97.764369f, 97.766823f, 97.769280f, 97.771737f, + 97.774092f, 97.775808f, 97.777515f, 97.779344f, 97.781295f, + 97.783368f, 97.785563f, 97.787881f, 97.790320f, 97.792882f, + 97.795565f, 97.798371f, 97.801299f, 97.804349f, 97.807521f, + 97.810815f, 97.814231f, 97.817769f, 97.821429f, 97.825211f, + 97.829115f, 97.833142f, 97.837290f, 97.840785f, 97.842598f, + 97.844371f, 97.846169f, 97.847993f, 97.849842f, 97.851716f, + 97.853615f, 97.855540f, 97.857490f, 97.859465f, 97.861465f, + 97.863491f, 97.865542f, 97.867619f, 97.869720f, 97.871847f, + 97.874000f, 97.876177f, 97.878380f, 97.880608f, 97.882861f, + 97.885140f, 97.887444f, 97.889773f, 97.892128f, 97.894508f, + 97.896913f, 97.899343f, 97.901799f, 97.904280f, 97.906786f, + 97.909317f, 97.911874f, 97.914456f, 97.917064f, 97.919696f, + 97.922354f, 97.925038f, 97.927746f, 97.930480f, 97.933239f, + 97.936023f, 97.938833f, 97.941668f, 97.944543f, 97.947590f, + 97.950645f, 97.953660f, 97.956635f, 97.959568f, 97.962461f, + 97.965314f, 97.968126f, 97.970897f, 97.973628f, 97.976318f, + 97.978968f, 97.981577f, 97.984145f, 97.986673f, 97.989160f, + 97.991606f, 97.994012f, 97.996377f, 97.998702f, 98.000986f, + 98.003230f, 98.005432f, 98.007595f, 98.009716f, 98.011797f, + 98.013838f, 98.015838f, 98.017797f, 98.019715f, 98.021593f, + 98.023431f, 98.025228f, 98.026984f, 98.028699f, 98.030374f, + 98.032009f, 98.033603f, 98.035156f, 98.036668f, 98.038140f, + 98.039572f, 98.040962f, 98.042313f, 98.043622f, 98.044988f, + 98.046473f, 98.047985f, 98.049526f, 98.051094f, 98.052691f, + 98.054316f, 98.055970f, 98.057651f, 98.059361f, 98.061098f, + 98.062864f, 98.064658f, 98.066480f, 98.068331f, 98.070209f, + 98.072116f, 98.074051f, 98.076013f, 98.078005f, 98.080024f, + 98.082071f, 98.084147f, 98.086251f, 98.088382f, 98.090542f, + 98.092731f, 98.094947f, 98.097192f, 98.099464f, 98.101765f, + 98.104094f, 98.106451f, 98.108836f, 98.111250f, 98.113691f, + 98.116161f, 98.118659f, 98.121185f, 98.123739f, 98.126322f, + 98.128932f, 98.131571f, 98.134238f, 98.136933f, 98.139613f, + 98.142163f, 98.144848f, 98.147702f, 98.150724f, 98.153914f, + 98.157273f, 98.160801f, 98.164497f, 98.168362f, 98.172395f, + 98.176596f, 98.180966f, 98.185505f, 98.190212f, 98.195088f, + 98.200132f, 98.205344f, 98.210725f, 98.216275f, 98.221993f, + 98.227879f, 98.233934f, 98.239580f, 98.244307f, 98.249056f, + 98.253846f, 98.258676f, 98.263546f, 98.268456f, 98.273407f, + 98.278398f, 98.283429f, 98.288501f, 98.293613f, 98.298766f, + 98.303959f, 98.309192f, 98.314465f, 98.319779f, 98.325133f, + 98.330527f, 98.335962f, 98.341437f, 98.346953f, 98.352530f, + 98.359395f, 98.366763f, 98.373808f, 98.380530f, 98.386929f, + 98.393004f, 98.398756f, 98.404185f, 98.409290f, 98.414073f, + 98.418532f, 98.422668f, 98.426480f, 98.429970f, 98.433136f, + 98.435979f, 98.438499f, 98.440695f, 98.442568f, 98.444118f, + 98.445345f, 98.446248f, 98.447608f, 98.454523f, 98.461977f, + 98.468680f, 98.474630f, 98.479829f, 98.484276f, 98.487972f, + 98.490916f, 98.493108f, 98.494548f, 98.495237f, 98.495173f, + 98.494359f, 98.492792f, 98.490474f, 98.487404f, 98.483582f, + 98.479009f, 98.473683f, 98.467607f, 98.460778f, 98.453198f, + 98.446798f, 98.444899f, 98.443068f, 98.441065f, 98.438888f, + 98.436539f, 98.434016f, 98.431320f, 98.428451f, 98.425409f, + 98.422194f, 98.418806f, 98.415244f, 98.411510f, 98.407602f, + 98.403521f, 98.399267f, 98.394840f, 98.390240f, 98.385467f, + 98.380521f, 98.375401f, 98.370102f, 98.358628f, 98.343008f, + 98.329144f, 98.317034f, 98.306681f, 98.298083f, 98.291240f, + 98.286153f, 98.282821f, 98.281245f, 98.281425f, 98.283360f, + 98.287050f, 98.292496f, 98.299698f, 98.308655f, 98.319367f, + 98.331835f, 98.346059f, 98.362038f, 98.379772f, 98.399262f, + 98.419114f, 98.422038f, 98.420791f, 98.421211f, 98.423299f, + 98.427055f, 98.432479f, 98.439570f, 98.448329f, 98.458756f, + 98.470850f, 98.484612f, 98.500041f, 98.517139f, 98.535903f, + 98.556336f, 98.578436f, 98.602204f, 98.627640f, 98.654743f, + 98.683514f, 98.713953f, 98.746059f, 98.787521f, 98.855514f, + 98.918831f, 98.974467f, 99.022423f, 99.062699f, 99.095294f, + 99.120210f, 99.137445f, 99.147000f, 99.148874f, 99.143069f, + 99.129583f, 99.108417f, 99.079570f, 99.043044f, 98.998837f, + 98.946950f, 98.887383f, 98.820136f, 98.745177f, 98.665894f, + 98.595201f, 98.535711f, 98.487424f, 98.450340f, 98.424460f, + 98.409783f, 98.406310f, 98.414040f, 98.432973f, 98.427322f, + 98.393089f, 98.369745f, 98.357315f, 98.355800f, 98.365198f, + 98.385510f, 98.416737f, 98.458878f, 98.511933f, 98.570310f, + 98.617336f, 98.657508f, 98.691226f, 98.718491f, 98.739304f, + 98.753664f, 98.761571f, 98.763025f, 98.758026f, 98.751737f, + 98.766477f, 98.781073f, 98.791900f, 98.798957f, 98.802244f, + 98.801762f, 98.797509f, 98.789487f, 98.777695f, 98.762483f, + 98.749970f, 98.740538f, 98.733107f, 98.727675f, 98.724244f, + 98.722813f, 98.723382f, 98.725952f, 98.730521f, 98.737023f, + 98.738339f, 98.736475f, 98.736094f, 98.737196f, 98.739780f, + 98.743847f, 98.749397f, 98.756429f, 98.764944f, 98.774942f, + 98.783819f, 98.789582f, 98.795655f, 98.802074f, 98.808840f, + 98.815952f, 98.823410f, 98.831215f, 98.839366f, 98.847864f, + 98.856727f, 98.865535f, 98.873718f, 98.881255f, 98.888147f, + 98.894393f, 98.899992f, 98.904946f, 98.909254f, 98.912916f, + 98.915932f, 98.918302f, 98.920026f, 98.921104f, 98.921537f, + 98.921323f, 98.920463f, 98.918958f, 98.916807f, 98.914009f, + 98.910733f, 98.910864f, 98.912404f, 98.913732f, 98.914846f, + 98.915748f, 98.916437f, 98.916913f, 98.917177f, 98.917227f, + 98.917065f, 98.916689f, 98.916101f, 98.915301f, 98.914287f, + 98.913060f, 98.911621f, 98.909969f, 98.908104f, 98.905397f, + 98.901811f, 98.898735f, 98.896189f, 98.894174f, 98.892690f, + 98.891737f, 98.891314f, 98.891423f, 98.892062f, 98.893232f, + 98.894934f, 98.897166f, 98.899929f, 98.903222f, 98.907047f, + 98.911403f, 98.916289f, 98.921031f, 98.921530f, 98.921611f, + 98.922200f, 98.923296f, 98.924899f, 98.927010f, 98.929628f, + 98.932754f, 98.936387f, 98.940528f, 98.945175f, 98.950331f, + 98.955994f, 98.962164f, 98.968841f, 98.976026f, 98.983719f, + 98.991913f, 98.999139f, 99.005302f, 99.011348f, 99.017275f, + 99.023085f, 99.028777f, 99.034350f, 99.039806f, 99.045144f, + 99.050364f, 99.055467f, 99.060451f, 99.065317f, 99.070066f, + 99.074696f, 99.079209f, 99.083604f, 99.087881f, 99.092929f, + 99.100425f, 99.107601f, 99.114253f, 99.120383f, 99.125990f, + 99.131073f, 99.135634f, 99.139671f, 99.143186f, 99.146177f, + 99.148645f, 99.150590f, 99.152012f, 99.152911f, 99.153287f, + 99.153140f, 99.152470f, 99.151482f, 99.153481f, 99.156417f, + 99.159086f, 99.161488f, 99.163622f, 99.165489f, 99.167089f, + 99.168421f, 99.169486f, 99.170284f, 99.170814f, 99.171077f, + 99.171073f, 99.170801f, 99.170262f, 99.169455f, 99.168382f, + 99.167041f, 99.165666f, 99.164642f, 99.163817f, 99.163188f, + 99.162755f, 99.162517f, 99.162476f, 99.162631f, 99.162981f, + 99.163528f, 99.164271f, 99.165209f, 99.166344f, 99.167675f, + 99.169201f, 99.170924f, 99.172842f, 99.174957f, 99.176598f, + 99.174749f, 99.172921f, 99.171750f, 99.171236f, 99.171378f, + 99.172176f, 99.173631f, 99.175743f, 99.178511f, 99.181936f, + 99.186017f, 99.190755f, 99.196149f, 99.202200f, 99.208907f, + 99.216271f, 99.224292f, 99.232938f, 99.239519f, 99.244561f, + 99.249642f, 99.254761f, 99.259919f, 99.265115f, 99.270350f, + 99.275624f, 99.280935f, 99.286286f, 99.291674f, 99.297101f, + 99.302567f, 99.308071f, 99.313614f, 99.319195f, 99.324815f, + 99.330473f, 99.337531f, 99.347715f, 99.357188f, 99.365752f, + 99.373409f, 99.380157f, 99.385997f, 99.390929f, 99.394952f, + 99.398067f, 99.400273f, 99.401572f, 99.401962f, 99.401444f, + 99.400017f, 99.397682f, 99.394439f, 99.390287f, 99.385692f, + 99.386166f, 99.387960f, 99.389360f, 99.390366f, 99.390980f, + 99.391199f, 99.391025f, 99.390458f, 99.389497f, 99.388142f, + 99.386394f, 99.384253f, 99.381718f, 99.378789f, 99.375467f, + 99.371752f, 99.367643f, 99.363140f, 99.356415f, 99.348044f, + 99.340801f, 99.334692f, 99.329718f, 99.325877f, 99.323171f, + 99.321598f, 99.321160f, 99.321855f, 99.323685f, 99.326649f, + 99.330746f, 99.335978f, 99.342344f, 99.349844f, 99.358478f, + 99.368246f, 99.377083f, 99.377174f, 99.377006f, 99.377848f, + 99.379702f, 99.382567f, 99.386443f, 99.391331f, 99.397230f, + 99.404141f, 99.412063f, 99.420996f, 99.430940f, 99.441896f, + 99.453863f, 99.466841f, 99.480831f, 99.495832f, 99.511866f, + 99.529327f, 99.546286f, 99.561978f, 99.576403f, 99.589560f, + 99.601450f, 99.612073f, 99.621429f, 99.629518f, 99.636339f, + 99.641893f, 99.646180f, 99.649200f, 99.650953f, 99.651438f, + 99.650656f, 99.648607f, 99.645291f, 99.648079f, 99.665101f, + 99.679972f, 99.692093f, 99.701464f, 99.708084f, 99.711955f, + 99.713075f, 99.711445f, 99.707064f, 99.699933f, 99.690052f, + 99.677421f, 99.662040f, 99.643908f, 99.623026f, 99.599393f, + 99.573011f, 99.543011f, 99.504264f, 99.467702f, 99.435917f, + 99.408909f, 99.386679f, 99.369226f, 99.356551f, 99.348653f, + 99.345532f, 99.347188f, 99.353622f, 99.364834f, 99.380822f, + 99.401588f, 99.427131f, 99.457452f, 99.492550f, 99.532425f, + 99.571230f, 99.603359f, 99.631869f, 99.656759f, 99.678029f, + 99.695679f, 99.709710f, 99.720120f, 99.726911f, 99.741884f, + 99.774430f, 99.798990f, 99.814834f, 99.821964f, 99.820380f, + 99.810080f, 99.791067f, 99.763338f, 99.729508f, 99.703283f, + 99.682824f, 99.667409f, 99.657036f, 99.651707f, 99.651422f, + 99.656180f, 99.665981f, 99.679074f, 99.680875f, 99.680832f, + 99.682233f, 99.685077f, 99.689365f, 99.695096f, 99.702270f, + 99.710887f, 99.720897f, 99.730363f, 99.738607f, 99.746052f, + 99.752696f, 99.758540f, 99.763583f, 99.767827f, 99.771270f, + 99.773914f, 99.777091f, 99.781056f, 99.784772f, 99.788241f, + 99.791461f, 99.794434f, 99.797158f, 99.799634f, 99.801862f, + 99.803842f, 99.805574f, 99.807058f, 99.808294f, 99.809282f, + 99.810021f, 99.810513f, 99.810756f, 99.810751f, 99.810808f, + 99.811855f, 99.813005f, 99.814161f, 99.815324f, 99.816492f, + 99.817667f, 99.818848f, 99.820034f, 99.821227f, 99.822426f, + 99.823631f, 99.824842f, 99.826060f, 99.827283f, 99.828512f, + 99.829748f, 99.830990f, 99.832189f, 99.832358f, 99.832304f, + 99.832509f, 99.832974f, 99.833699f, 99.834684f, 99.835929f, + 99.837434f, 99.839199f, 99.841224f, 99.843508f, 99.846053f, + 99.848858f, 99.851923f, 99.855247f, 99.858832f, 99.862676f, + 99.866781f, 99.870159f, 99.872063f, 99.874116f, 99.876345f, + 99.878752f, 99.881337f, 99.884098f, 99.887036f, 99.890152f, + 99.893445f, 99.896915f, 99.900562f, 99.904387f, 99.908389f, + 99.912567f, 99.916923f, 99.921456f, 99.926167f, 99.930998f, + 99.935521f, 99.939883f, 99.944145f, 99.948309f, 99.952373f, + 99.956338f, 99.960204f, 99.963971f, 99.967639f, 99.971207f, + 99.974676f, 99.978046f, 99.981317f, 99.984489f, 99.987561f, + 99.990534f, 99.993408f, 99.996181f, 99.998467f, 100.000597f, + 100.002957f, 100.005547f, 100.008367f, 100.011417f, 100.014697f, + 100.018208f, 100.021948f, 100.025918f, 100.030119f, 100.034549f, + 100.039209f, 100.044100f, 100.049220f, 100.054571f, 100.060152f, + 100.065962f, 100.072131f, 100.078667f, 100.084925f, 100.090869f, + 100.096501f, 100.101818f, 100.106822f, 100.111513f, 100.115890f, + 100.119953f, 100.123703f, 100.127140f, 100.130263f, 100.133072f, + 100.135568f, 100.137750f, 100.139619f, 100.141174f, 100.142416f, + 100.143344f, 100.143959f, 100.144260f, 100.144248f, 100.143922f, + 100.143282f, 100.142329f, 100.141063f, 100.139483f, 100.137589f, + 100.135382f, 100.132862f, 100.130028f, 100.126880f, 100.123419f, + 100.119644f, 100.115556f, 100.111154f, 100.106477f, 100.102132f, + 100.098452f, 100.095438f, 100.093091f, 100.091410f, 100.090395f, + 100.090046f, 100.090364f, 100.091348f, 100.092998f, 100.095314f, + 100.098296f, 100.101945f, 100.106053f, 100.106421f, 100.105657f, + 100.105533f, 100.106049f, 100.107206f, 100.109002f, 100.111438f, + 100.114514f, 100.118230f, 100.122586f, 100.127582f, 100.133218f, + 100.139494f, 100.146410f, 100.153424f, 100.158587f, 100.163476f, + 100.168303f, 100.173069f, 100.177773f, 100.182415f, 100.186996f, + 100.191516f, 100.195974f, 100.200370f, 100.204704f, 100.208978f, + 100.213189f, 100.217339f, 100.221032f, 100.224399f, 100.228025f, + 100.231912f, 100.236059f, 100.240467f, 100.245135f, 100.250064f, + 100.255254f, 100.260704f, 100.266415f, 100.272386f, 100.278618f, + 100.285110f, 100.291847f, 100.298450f, 100.304701f, 100.310631f, + 100.316242f, 100.321532f, 100.326502f, 100.331152f, 100.335481f, + 100.339491f, 100.343180f, 100.346549f, 100.349598f, 100.352327f, + 100.354735f, 100.356823f, 100.358592f, 100.360040f, 100.361167f, + 100.361975f, 100.362462f, 100.362630f, 100.362477f, 100.362003f, + 100.361210f, 100.360097f, 100.358663f, 100.356909f, 100.354835f, + 100.352441f, 100.351512f, 100.352213f, 100.352904f, 100.353586f, + 100.354258f, 100.354920f, 100.355573f, 100.356216f, 100.356849f, + 100.357472f, 100.358086f, 100.358690f, 100.359285f, 100.359869f, + 100.360401f, 100.360472f, 100.360545f, 100.360771f, 100.361152f, + 100.361687f, 100.362376f, 100.363220f, 100.364217f, 100.365369f, + 100.366675f, 100.368135f, 100.369749f, 100.371518f, 100.373440f, + 100.373433f, 100.367968f, 100.362875f, 100.358520f, 100.354904f, + 100.352025f, 100.349884f, 100.348482f, 100.347818f, 100.347892f, + 100.348704f, 100.350254f, 100.352542f, 100.355568f, 100.359333f, + 100.363836f, 100.369076f, 100.375055f, 100.381772f, 100.389227f, + 100.397421f, 100.406352f, 100.416022f, 100.426429f, 100.437575f, + 100.449459f, 100.462081f, 100.475442f, 100.489540f, 100.505343f, + 100.529229f, 100.552241f, 100.572110f, 100.588839f, 100.602425f, + 100.612871f, 100.620174f, 100.624337f, 100.625357f, 100.623237f, + 100.617974f, 100.613147f, 100.619784f, 100.626196f, 100.631021f, + 100.634259f, 100.635910f, 100.635974f, 100.634450f, 100.631340f, + 100.626642f, 100.620357f, 100.612484f, 100.604120f, 100.597660f, + 100.592178f, 100.587635f, 100.584032f, 100.581369f, 100.579645f, + 100.578861f, 100.579017f, 100.580113f, 100.582148f, 100.585120f, + 100.585212f, 100.582812f, 100.581306f, 100.580693f, 100.580974f, + 100.582149f, 100.584218f, 100.587181f, 100.591037f, 100.595787f, + 100.601431f, 100.607821f, 100.611286f, 100.613334f, 100.615423f, + 100.617552f, 100.619723f, 100.621934f, 100.624185f, 100.626478f, + 100.628810f, 100.631184f, 100.633598f, 100.636053f, 100.638549f, + 100.641085f, 100.643662f, 100.646279f, 100.648937f, 100.651636f, + 100.654375f, 100.657155f, 100.659976f, 100.662838f, 100.665740f, + 100.668609f, 100.671250f, 100.673873f, 100.676499f, 100.679128f, + 100.681759f, 100.684392f, 100.687028f, 100.689666f, 100.692307f, + 100.694950f, 100.697596f, 100.700245f, 100.702896f, 100.705549f, + 100.708205f, 100.710863f, 100.713524f, 100.716188f, 100.718853f, + 100.721522f, 100.724193f, 100.726866f, 100.729543f, 100.732450f, + 100.735495f, 100.738483f, 100.741415f, 100.744290f, 100.747108f, + 100.749870f, 100.752575f, 100.755224f, 100.757816f, 100.760351f, + 100.762830f, 100.765253f, 100.767619f, 100.769928f, 100.772180f, + 100.774377f, 100.776516f, 100.778599f, 100.780625f, 100.782595f, + 100.784508f, 100.786365f, 100.787923f, 100.787912f, 100.787721f, + 100.787641f, 100.787672f, 100.787813f, 100.788065f, 100.788427f, + 100.788900f, 100.789484f, 100.790179f, 100.790984f, 100.791900f, + 100.792926f, 100.794064f, 100.795311f, 100.796670f, 100.798139f, + 100.799719f, 100.801410f, 100.803211f, 100.805123f, 100.807146f, + 100.809279f, 100.811523f, 100.813877f, 100.816343f, 100.818919f, + 100.821605f, 100.824403f, 100.827311f, 100.830329f, 100.833459f, + 100.836699f, 100.840049f, 100.843511f, 100.847083f, 100.850765f, + 100.854559f, 100.858463f, 100.862477f, 100.866603f, 100.870839f, + 100.875185f, 100.879643f, 100.884211f, 100.888890f, 100.893716f, + 100.899173f, 100.904845f, 100.910544f, 100.916270f, 100.922023f, + 100.927802f, 100.933608f, 100.939441f, 100.945301f, 100.951188f, + 100.957101f, 100.963042f, 100.969009f, 100.975003f, 100.981023f, + 100.987071f, 100.993145f, 100.999246f, 101.005415f, 101.013313f, + 101.021768f, 101.029748f, 101.037255f, 101.044288f, 101.050847f, + 101.056932f, 101.062543f, 101.067680f, 101.072343f, 101.076532f, + 101.080247f, 101.083488f, 101.086256f, 101.088549f, 101.090369f, + 101.091714f, 101.092586f, 101.092987f, 101.096666f, 101.103065f, + 101.108822f, 101.113936f, 101.118408f, 101.122238f, 101.125426f, + 101.127971f, 101.129873f, 101.131134f, 101.131752f, 101.131727f, + 101.131060f, 101.129751f, 101.127800f, 101.125206f, 101.121969f, + 101.118091f, 101.113570f, 101.110829f, 101.111273f, 101.111456f, + 101.111346f, 101.110943f, 101.110245f, 101.109255f, 101.107970f, + 101.106392f, 101.104520f, 101.102354f, 101.099895f, 101.097143f, + 101.094096f, 101.090756f, 101.087122f, 101.083195f, 101.078974f, + 101.074459f, 101.068370f, 101.059591f, 101.051715f, 101.044892f, + 101.039122f, 101.034406f, 101.030742f, 101.028131f, 101.026573f, + 101.026068f, 101.026616f, 101.028217f, 101.030871f, 101.034578f, + 101.039338f, 101.045151f, 101.052017f, 101.059936f, 101.068907f, + 101.076151f, 101.073717f, 101.071448f, 101.070435f, 101.070678f, + 101.072177f, 101.074932f, 101.078943f, 101.084210f, 101.090733f, + 101.098512f, 101.107547f, 101.117837f, 101.129384f, 101.142187f, + 101.156246f, 101.171561f, 101.188132f, 101.205959f, 101.224794f, + 101.242448f, 101.258939f, 101.274464f, 101.289022f, 101.302615f, + 101.315241f, 101.326900f, 101.337594f, 101.347321f, 101.356083f, + 101.363877f, 101.370706f, 101.376569f, 101.381465f, 101.385395f, + 101.388359f, 101.390356f, 101.391388f, 101.393048f, 101.413241f, + 101.436287f, 101.455872f, 101.471995f, 101.484658f, 101.493859f, + 101.499599f, 101.501878f, 101.500696f, 101.496052f, 101.487947f, + 101.476382f, 101.461354f, 101.442866f, 101.420917f, 101.395506f, + 101.366634f, 101.334301f, 101.298416f, 101.257917f, 101.219392f, + 101.185132f, 101.155136f, 101.129403f, 101.107934f, 101.090729f, + 101.077788f, 101.069111f, 101.064698f, 101.064548f, 101.068663f, + 101.077041f, 101.089683f, 101.106589f, 101.127758f, 101.153192f, + 101.182889f, 101.216806f, 101.240601f, 101.255141f, 101.270958f, + 101.288051f, 101.306421f, 101.326068f, 101.346992f, 101.369192f, + 101.392669f, 101.420667f, 101.465858f, 101.504740f, 101.533318f, + 101.551592f, 101.559562f, 101.557228f, 101.544590f, 101.521649f, + 101.488403f, 101.457093f, 101.439868f, 101.425218f, 101.413067f, + 101.403414f, 101.396259f, 101.391603f, 101.389446f, 101.389787f, + 101.390940f, 101.375876f, 101.361011f, 101.352324f, 101.349816f, + 101.353486f, 101.363335f, 101.379362f, 101.401568f, 101.429952f, + 101.463729f, 101.495309f, 101.520532f, 101.539360f, 101.551793f, + 101.557832f, 101.557476f, 101.550725f, 101.537580f, 101.518423f, + 101.506018f, 101.499324f, 101.493905f, 101.489761f, 101.486894f, + 101.485302f, 101.484986f, 101.485945f, 101.488181f, 101.489943f, + 101.486868f, 101.484633f, 101.483777f, 101.484299f, 101.486199f, + 101.489477f, 101.494134f, 101.500169f, 101.507565f, 101.512495f, + 101.514956f, 101.517458f, 101.520003f, 101.522590f, 101.525219f, + 101.527890f, 101.530604f, 101.533360f, 101.536158f, 101.538998f, + 101.541880f, 101.544805f, 101.547771f, 101.550780f, 101.553831f, + 101.556925f, 101.560060f, 101.563238f, 101.566388f, 101.569453f, + 101.572497f, 101.575521f, 101.578525f, 101.581509f, 101.584473f, + 101.587417f, 101.590341f, 101.593245f, 101.596128f, 101.598992f, + 101.601836f, 101.604659f, 101.607463f, 101.610246f, 101.613010f, + 101.615753f, 101.618476f, 101.620883f, 101.622756f, 101.624783f, + 101.626985f, 101.629362f, 101.631914f, 101.634640f, 101.637542f, + 101.640618f, 101.643869f, 101.647296f, 101.650897f, 101.654673f, + 101.658623f, 101.662749f, 101.667050f, 101.671525f, 101.676176f, + 101.681001f, 101.685051f, 101.686076f, 101.686985f, 101.688035f, + 101.689225f, 101.690555f, 101.692025f, 101.693636f, 101.695387f, + 101.697279f, 101.699311f, 101.701483f, 101.703796f, 101.706249f, + 101.708842f, 101.711576f, 101.714450f, 101.717464f, 101.720619f, + 101.723914f, 101.727350f, 101.730926f, 101.734642f, 101.738499f, + 101.742496f, 101.746633f, 101.750910f, 101.755328f, 101.759887f, + 101.764586f, 101.769425f, 101.774404f, 101.779524f, 101.784784f, + 101.790185f, 101.795726f, 101.801407f, 101.807228f, 101.813503f, + 101.822903f, 101.832689f, 101.841990f, 101.850806f, 101.859136f, + 101.866981f, 101.874342f, 101.881217f, 101.887606f, 101.893511f, + 101.898930f, 101.903864f, 101.908313f, 101.912277f, 101.915755f, + 101.918749f, 101.921257f, 101.923280f, 101.924817f, 101.925870f, + 101.926437f, 101.926519f, 101.926116f, 101.925228f, 101.923854f, + 101.921995f, 101.919651f, 101.916822f, 101.913508f, 101.909708f, + 101.905423f, 101.900653f, 101.895398f, 101.889658f, 101.883432f, + 101.876721f, 101.869525f, 101.861868f, 101.856511f, 101.852942f, + 101.849405f, 101.845902f, 101.842432f, 101.838994f, 101.835590f, + 101.832218f, 101.828880f, 101.825574f, 101.822301f, 101.819061f, + 101.815854f, 101.812680f, 101.809539f, 101.806431f, 101.807814f, + 101.823603f, 101.836735f, 101.845621f, 101.850261f, 101.850655f, + 101.846803f, 101.838705f, 101.826360f, 101.809769f, 101.788932f, + 101.763849f, 101.734520f, 101.700945f, 101.663123f, 101.621055f, + 101.571169f, 101.388443f, 101.265771f, 101.316210f, 101.539762f, + 100.122644f, 90.476861f, 77.940204f, 64.467952f, 51.328201f, + 39.377482f, 28.086747f, 20.044960f, 12.552455f, 6.182592f, + 2.538719f, -0.100834f, -2.448053f, -3.284514f, -3.573116f, + -3.280497f, -2.376872f, -0.873345f, 0.478556f, 1.734965f, + 4.107486f, 6.038079f, 8.164180f, 9.496014f, 11.386878f, + 13.806034f, 15.176567f, 16.358720f, 18.165444f, 19.201755f, + 19.856068f, 20.966906f, 21.526461f, 21.662649f, 22.117522f, + 22.216883f, 21.940653f, 21.867024f, 21.621539f, 21.111552f, + 20.693368f, 20.259722f, 19.718018f, 19.182535f, 18.707868f, + 18.266860f, 17.811642f, 17.399645f, 17.092564f, 16.797613f, + 16.494755f, 16.321852f, 16.201167f, 16.030757f, 15.970686f, + 15.981668f, 15.937079f, 15.963397f, 16.055743f, 16.114348f, + 16.195112f, 16.308680f, 16.416918f, 16.520492f, 16.625309f, + 16.735907f, 16.839934f, 16.933135f, 17.023914f, 17.103114f, + 17.163033f, 17.203671f, 17.225028f, 17.227103f, 17.236830f, + 17.275810f, 17.299884f, 17.308248f, 17.300905f, 17.277852f, + 17.247870f, 17.243338f, 17.234959f, 17.218581f, 17.194205f, + 17.161829f, 17.120673f, 17.068009f, 17.027350f, 17.004160f, + 16.998440f, 17.010188f, 17.032793f, 17.041852f, 17.044793f, + 17.042677f, 17.035504f, 17.023262f, 17.002999f, 16.987063f, + 16.983212f, 16.991444f, 17.011760f, 17.037024f, 17.054210f, + 17.064775f, 17.068723f, 17.066055f, 17.061433f, 17.061741f, + 17.063152f, 17.065613f, 17.069124f, 17.074749f, 17.080878f, + 17.083391f, 17.082209f, 17.077332f, 17.070405f, 17.066168f, + 17.062853f, 17.060314f, 17.058550f, 17.057561f, 17.057347f, + 17.057908f, 17.059244f, 17.061355f, 17.063919f, 17.065444f, + 17.066552f, 17.067365f, 17.067883f, 17.068106f, 17.068035f, + 17.067668f, 17.067006f, 17.066050f, 17.064890f, 17.064251f, + 17.063772f, 17.063324f, 17.062908f, 17.062524f, 17.062170f, + 17.061849f, 17.061559f, 17.061300f, 17.061027f, 17.060101f, + 17.059178f, 17.058548f, 17.058210f, 17.058165f, 17.058412f, + 17.058951f, 17.059783f, 17.060908f, 17.062326f, 17.063964f, + 17.065486f, 17.066812f, 17.067944f, 17.068881f, 17.069622f, + 17.070168f, 17.070519f, 17.070675f, 17.070635f, 17.070400f, + 17.069970f, 17.069345f, 17.068525f, 17.067509f, 17.066299f, + 17.064893f, 17.063291f, 17.061495f, 17.059503f, 17.056064f, + 17.051099f, 17.047493f, 17.045297f, 17.044510f, 17.045132f, + 17.047165f, 17.050607f, 17.055458f, 17.061719f, 17.069778f, + 17.078713f, 17.085654f, 17.090467f, 17.093153f, 17.093711f, + 17.092142f, 17.088445f, 17.082621f, 17.074669f, 17.064487f, + 17.053521f, 17.044905f, 17.038879f, 17.035442f, 17.034595f, + 17.036338f, 17.040670f, 17.047592f, 17.057103f, 17.069324f, + 17.083398f, 17.094849f, 17.103025f, 17.107926f, 17.109553f, + 17.107905f, 17.102983f, 17.094786f, 17.083314f, 17.068474f, + 17.050144f, 17.034566f, 17.023401f, 17.016648f, 17.014307f, + 17.016377f, 17.022860f, 17.033755f, 17.049061f, 17.068810f, + 17.094910f, 17.118425f, 17.134760f, 17.143915f, 17.145889f, + 17.140683f, 17.128297f, 17.108731f, 17.081984f, 17.048022f, + 17.017107f, 16.995105f, 16.982021f, 16.977856f, 16.982608f, + 16.996279f, 17.018869f, 17.050376f, 17.091391f, 17.130621f, + 17.157512f, 17.171923f, 17.173853f, 17.163303f, 17.140272f, + 17.104760f, 17.059738f, 17.021830f, 16.996650f, 16.984193f, + 16.984461f, 16.997452f, 17.023168f, 17.058613f, 17.086306f, + 17.104514f, 17.113731f, 17.113957f, 17.105191f, 17.087592f, + 17.071407f, 17.061705f, 17.055873f, 17.053912f, 17.055821f, + 17.061601f, 17.069849f, 17.075097f, 17.078408f, 17.079973f, + 17.079792f, 17.077865f, 17.074225f, 17.070508f, 17.068115f, + 17.066917f, 17.066914f, 17.068105f, 17.070491f, 17.073933f, + 17.076989f, 17.078810f, 17.079388f, 17.078721f, 17.076811f, + 17.073644f, 17.069746f, 17.067031f, 17.065828f, 17.066137f, + 17.067959f, 17.071294f, 17.076285f, 17.081095f, 17.083765f, + 17.084261f, 17.082584f, 17.078732f, 17.072579f, 17.064199f, + 17.058419f, 17.056222f, 17.057610f, 17.062581f, 17.071137f, + 17.084511f, 17.097839f, 17.104762f, 17.105206f, 17.099171f, + 17.086657f, 17.066892f, 17.038794f, 17.019990f, 17.013775f, + 17.020148f, 17.039109f, 17.070659f, 17.122406f, 17.173852f, + 17.198830f, 17.197201f, 17.168965f, 17.114122f, 17.028029f, + 16.906433f, 16.827794f, 16.804467f, 16.836450f, 16.923743f, + 17.066348f, 17.296847f, 17.506715f, 17.593865f, 17.558192f, + 17.399695f, 17.117370f, 16.714013f, 16.434556f, 16.362178f, + 16.496881f, 16.838664f, 17.415451f, 17.921067f, 18.097711f, + 17.943894f, 17.459617f, 16.640716f, 15.853117f, 15.548218f, + 15.744472f, 16.441878f, 17.666558f, 18.274839f, 17.699607f, + 17.302395f, 17.404292f, 17.689659f, 17.620632f, 17.227012f, + 17.038364f, 17.224956f, 17.414660f, 17.258769f, 16.931287f, + 16.856289f, 17.017992f, 17.084321f, 17.003923f, 16.913542f, + 16.911494f, 16.946186f, 16.953849f, 16.945425f, 16.942503f, + 16.962769f, 16.979474f, 16.991338f, 17.003024f, 17.014534f, + 17.026279f, 17.038953f, 17.050476f, 17.060658f, 17.069500f, + 17.080067f, 17.092225f, 17.100427f, 17.104626f, 17.104974f, + 17.108010f, 17.112031f, 17.113982f, 17.113863f, 17.112556f, + 17.115625f, 17.118298f, 17.119367f, 17.118832f, 17.116693f, + 17.112951f, 17.107604f, 17.100653f, 17.092097f, 17.080449f, + 17.068968f, 17.060505f, 17.055061f, 17.052635f, 17.053228f, + 17.056839f, 17.063468f, 17.073115f, 17.086050f, 17.100268f, + 17.110879f, 17.117543f, 17.120259f, 17.119029f, 17.113851f, + 17.104727f, 17.091655f, 17.074547f, 17.050666f, 17.029685f, + 17.016092f, 17.009888f, 17.011073f, 17.019646f, 17.035608f, + 17.058959f, 17.090240f, 17.123072f, 17.146840f, 17.161043f, + 17.165680f, 17.160753f, 17.146260f, 17.122202f, 17.088501f, + 17.046256f, 17.012267f, 16.991524f, 16.984027f, 16.989775f, + 17.008769f, 17.041008f, 17.086440f, 17.133637f, 17.165873f, + 17.182033f, 17.182116f, 17.166123f, 17.134054f, 17.087988f, + 17.047488f, 17.022474f, 17.012952f, 17.018924f, 17.040388f, + 17.076248f, 17.109664f, 17.130825f, 17.140076f, 17.137418f, + 17.122851f, 17.096581f, 17.068597f, 17.050018f, 17.040952f, + 17.041399f, 17.051360f, 17.070830f, 17.094896f, 17.112054f, + 17.120977f, 17.121664f, 17.114114f, 17.098328f, 17.075430f, + 17.056262f, 17.046288f, 17.045533f, 17.053997f, 17.071681f, + 17.099042f, 17.126961f, 17.142839f, 17.146172f, 17.136960f, + 17.115205f, 17.080135f, 17.038709f, 17.013192f, 17.005885f, + 17.016789f, 17.045904f, 17.094052f, 17.161175f, 17.207433f, + 17.223655f, 17.209840f, 17.165990f, 17.091919f, 16.980001f, + 16.891567f, 16.854607f, 16.869121f, 16.935108f, 17.052570f, + 17.236404f, 17.403428f, 17.473485f, 17.446167f, 17.321472f, + 17.097726f, 16.821152f, 16.649978f, 16.608119f, 16.695576f, + 16.912349f, 17.273197f, 17.630268f, 17.794031f, 17.759726f, + 17.527352f, 17.094890f, 16.505208f, 16.129464f, 16.064041f, + 16.308938f, 16.869271f, 17.691158f, 17.856913f, 17.465207f, + 17.354194f, 17.431797f, 17.464336f, 17.339888f, 17.196189f, + 17.173603f, 17.201220f, 17.169120f, 17.093044f, 17.044526f, + 17.031915f, 17.016843f, 17.003978f, 16.995119f, 16.990134f, + 16.977593f, 16.966175f, 16.965219f, 16.974724f, 16.991198f, + 16.999773f, 17.008352f, 17.018271f, 17.029533f, 17.041710f, + 17.052857f, 17.062470f, 17.070550f, 17.077466f, 17.084778f, + 17.091311f, 17.096872f, 17.101461f, 17.105078f, 17.107722f, + 17.109393f, 17.110092f, 17.110865f, 17.115785f, 17.119943f, + 17.122570f, 17.123664f, 17.123227f, 17.121258f, 17.117756f, + 17.113003f, 17.109783f, 17.107441f, 17.105535f, 17.104067f, + 17.103036f, 17.102442f, 17.102285f, 17.102581f, 17.103293f, + 17.103574f, 17.103238f, 17.102285f, 17.100717f, 17.098532f, + 17.095730f, 17.092284f, 17.087359f, 17.082898f, 17.079863f, + 17.078254f, 17.078070f, 17.079311f, 17.081978f, 17.086069f, + 17.089593f, 17.091659f, 17.093446f, 17.094956f, 17.096187f, + 17.097140f, 17.097816f, 17.098213f, 17.099438f, 17.101625f, + 17.103071f, 17.103764f, 17.103706f, 17.102897f, 17.101335f, + 17.099021f, 17.095903f, 17.092844f, 17.090753f, 17.089645f, + 17.089521f, 17.090381f, 17.092225f, 17.095052f, 17.098770f, + 17.102441f, 17.105539f, 17.108057f, 17.109997f, 17.111357f, + 17.112138f, 17.112340f, 17.111963f, 17.111007f, 17.109472f, + 17.107358f, 17.104664f, 17.101392f, 17.097540f, 17.093109f, + 17.087654f, 17.078520f, 17.070101f, 17.063430f, 17.058506f, + 17.055329f, 17.053900f, 17.054218f, 17.056284f, 17.060097f, + 17.065657f, 17.072965f, 17.082020f, 17.092823f, 17.105373f, + 17.123528f, 17.145768f, 17.158916f, 17.162606f, 17.156838f, + 17.141613f, 17.123900f, 17.113799f, 17.106791f, 17.102838f, + 17.101941f, 17.104090f, 17.105452f, 17.104589f, 17.103646f, + 17.102622f, 17.101518f, 17.100314f, 17.098461f, 17.096633f, + 17.095227f, 17.094242f, 17.093679f, 17.093537f, 17.093818f, + 17.094519f, 17.095643f, 17.097188f, 17.099155f, 17.101594f, + 17.104452f, 17.106844f, 17.108654f, 17.109881f, 17.110526f, + 17.110587f, 17.110067f, 17.108963f, 17.107278f, 17.105009f, + 17.102158f, 17.098411f, 17.094051f, 17.090611f, 17.088149f, + 17.086667f, 17.086163f, 17.086638f, 17.088091f, 17.090524f, + 17.093935f, 17.098326f, 17.103695f, 17.110924f, 17.118241f, + 17.123908f, 17.127925f, 17.130292f, 17.131009f, 17.130076f, + 17.127492f, 17.123259f, 17.117375f, 17.109842f, 17.100535f, + 17.088446f, 17.077711f, 17.069561f, 17.063997f, 17.061019f, + 17.060626f, 17.062818f, 17.067597f, 17.074961f, 17.084910f, + 17.097445f, 17.112739f, 17.129124f, 17.142460f, 17.152420f, + 17.159003f, 17.162209f, 17.162038f, 17.158491f, 17.151568f, + 17.141267f, 17.127590f, 17.100474f, 17.068571f, 17.047216f, + 17.036412f, 17.036157f, 17.046452f, 17.067298f, 17.098693f, + 17.140639f, 17.193134f, 17.265461f, 17.392162f, 17.460327f, + 17.448863f, 17.357769f, 17.208552f, 17.102093f, 17.037058f, + 17.010707f, 17.023041f, 17.044603f, 17.039831f, 17.030960f, + 17.018123f, 17.000994f, 16.971748f, 16.950878f, 16.947670f, + 16.962125f, 16.991847f, 17.012763f, 17.027530f, 17.039283f, + 17.048022f, 17.054636f, 17.062986f, 17.072651f, 17.083502f, + 17.095539f, 17.111306f, 17.126934f, 17.136210f, 17.139113f, + 17.135796f, 17.133939f, 17.134727f, 17.135552f, 17.136413f, + 17.137309f, 17.138241f, 17.139209f, 17.140213f, 17.141253f, + 17.144420f, 17.152716f, 17.158395f, 17.160919f, 17.160289f, + 17.156503f, 17.149562f, 17.139466f, 17.126215f, 17.109712f, + 17.087926f, 17.068345f, 17.054089f, 17.045157f, 17.041549f, + 17.043266f, 17.050306f, 17.062671f, 17.080361f, 17.105856f, + 17.137472f, 17.160096f, 17.172877f, 17.175815f, 17.168908f, + 17.152159f, 17.125566f, 17.089130f, 17.042031f, 16.962970f, + 16.906470f, 16.897973f, 16.937479f, 17.020587f, 17.091063f, + 17.134422f, 17.154981f, 17.152738f, 17.135864f, 17.134625f, + 17.138521f, 17.146071f, 17.157276f, 17.174923f, 17.191259f, + 17.197088f, 17.192374f, 17.177341f, 17.164372f, 17.156581f, + 17.149992f, 17.144604f, 17.140079f, 17.133908f, 17.128323f, + 17.123970f, 17.120852f, 17.118030f, 17.113520f, 17.110216f, + 17.108352f, 17.107927f, 17.107631f, 17.106282f, 17.105209f, + 17.104422f, 17.103919f, 17.103701f, 17.103768f, 17.104120f, + 17.104756f, 17.105521f, 17.105227f, 17.104913f, 17.104884f, + 17.105141f, 17.105684f, 17.106512f, 17.107626f, 17.109025f, + 17.110710f, 17.112804f, 17.114903f, 17.116555f, 17.117758f, + 17.118512f, 17.118818f, 17.118674f, 17.118082f, 17.117042f, + 17.115558f, 17.113976f, 17.112822f, 17.112138f, 17.111925f, + 17.112181f, 17.112907f, 17.114104f, 17.115770f, 17.117907f, + 17.120972f, 17.124271f, 17.126623f, 17.128023f, 17.128470f, + 17.127964f, 17.126505f, 17.124094f, 17.120730f, 17.116253f, + 17.110307f, 17.105545f, 17.102441f, 17.100992f, 17.101200f, + 17.103064f, 17.106584f, 17.111761f, 17.118594f, 17.128920f, + 17.140290f, 17.148137f, 17.152436f, 17.153188f, 17.150393f, + 17.144050f, 17.134160f, 17.120722f, 17.103140f, 17.080001f, + 17.061286f, 17.048784f, 17.042496f, 17.042421f, 17.048560f, + 17.060912f, 17.079477f, 17.104256f, 17.138775f, 17.169099f, + 17.180444f, 17.172741f, 17.150483f, 17.137247f, 17.132223f, + 17.134507f, 17.143959f, 17.154727f, 17.159052f, 17.156405f, + 17.146808f, 17.135534f, 17.128400f, 17.124405f, 17.123550f, + 17.124154f, 17.122734f, 17.121089f, 17.119292f, 17.117173f, + 17.114367f, 17.112191f, 17.110812f, 17.110229f, 17.110444f, + 17.111455f, 17.113263f, 17.115868f, 17.118727f, 17.120914f, + 17.122544f, 17.123617f, 17.124132f, 17.124090f, 17.123490f, + 17.122333f, 17.120636f, 17.118895f, 17.117673f, 17.116999f, + 17.116875f, 17.117300f, 17.118275f, 17.119798f, 17.121870f, + 17.124485f, 17.127082f, 17.129287f, 17.131100f, 17.132518f, + 17.133543f, 17.134175f, 17.134413f, 17.134257f, 17.133708f, + 17.132765f, 17.131429f, 17.129699f, 17.127576f, 17.125059f, + 17.122148f, 17.118844f, 17.113903f, 17.107135f, 17.102381f, + 17.099763f, 17.099281f, 17.100936f, 17.104726f, 17.110653f, + 17.118711f, 17.127696f, 17.134873f, 17.139867f, 17.142677f, + 17.143305f, 17.141749f, 17.138010f, 17.132087f, 17.124327f, + 17.117419f, 17.112502f, 17.109569f, 17.108619f, 17.109654f, + 17.112672f, 17.117674f, 17.124672f, 17.133632f, 17.141177f, + 17.146130f, 17.148491f, 17.148259f, 17.145435f, 17.140019f, + 17.132011f, 17.120885f, 17.107585f, 17.097957f, 17.092498f, + 17.091206f, 17.094083f, 17.101126f, 17.112338f, 17.127720f, + 17.150100f, 17.171317f, 17.185083f, 17.191398f, 17.190263f, + 17.181677f, 17.165640f, 17.142153f, 17.109867f, 17.068792f, + 17.038996f, 17.023161f, 17.021288f, 17.033375f, 17.059424f, + 17.099434f, 17.155415f, 17.211143f, 17.245712f, 17.258657f, + 17.249978f, 17.219676f, 17.167749f, 17.095619f, 17.032702f, + 16.997760f, 16.990937f, 17.012233f, 17.061647f, 17.138084f, + 17.211527f, 17.252481f, 17.259855f, 17.233650f, 17.173866f, + 17.093691f, 17.036429f, 17.010625f, 17.016277f, 17.053385f, + 17.121388f, 17.194147f, 17.235182f, 17.241794f, 17.213984f, + 17.152751f, 17.088358f, 17.052002f, 17.044636f, 17.066261f, + 17.116415f, 17.172759f, 17.205118f, 17.211553f, 17.192061f, + 17.146893f, 17.094219f, 17.062783f, 17.054965f, 17.070763f, + 17.110106f, 17.159439f, 17.189390f, 17.196123f, 17.179638f, + 17.139851f, 17.083775f, 17.047920f, 17.041276f, 17.063842f, + 17.115711f, 17.193551f, 17.246950f, 17.258773f, 17.229021f, + 17.157657f, 17.047789f, 16.969657f, 16.951286f, 16.992677f, + 17.093829f, 17.254379f, 17.369835f, 17.388530f, 17.310464f, + 17.136470f, 16.946190f, 16.860522f, 16.888633f, 17.030523f, + 17.275228f, 17.459279f, 17.496289f, 17.386027f, 17.127578f, + 16.820783f, 16.689458f, 16.759469f, 17.030850f, 17.387357f, + 17.553797f, 17.492732f, 17.204027f, 16.797640f, 16.595300f, + 16.653019f, 16.971226f, 17.482303f, 17.777523f, 17.740969f, + 17.372639f, 16.774261f, 16.412611f, 16.418513f, 16.791969f, + 17.484706f, 17.947040f, 17.910157f, 17.377452f, 16.686407f, + 16.405789f, 16.588043f, 17.218716f, 17.826500f, 17.891662f, + 17.395186f, 16.731922f, 16.513334f, 16.787348f, 17.423613f, + 17.745975f, 17.536377f, 17.001361f, 16.754971f, 16.875984f, + 17.217086f, 17.361654f, 17.258230f, 17.004395f, 16.896772f, + 16.984114f, 17.185740f, 17.272936f, 17.211331f, 17.069792f, + 17.014473f, 17.062904f, 17.161676f, 17.203761f, 17.183715f, + 17.138920f, 17.124001f, 17.135436f, 17.158529f, 17.166720f, + 17.158838f, 17.144614f, 17.139080f, 17.142281f, 17.149747f, + 17.154123f, 17.155101f, 17.152683f, 17.146868f, 17.137612f, + 17.124578f, 17.114649f, 17.110232f, 17.111327f, 17.117936f, + 17.130039f, 17.144454f, 17.154375f, 17.158932f, 17.158125f, + 17.151952f, 17.140414f, 17.126277f, 17.116831f, 17.113210f, + 17.115413f, 17.123441f, 17.137329f, 17.154368f, 17.165940f, + 17.170616f, 17.168397f, 17.159283f, 17.143175f, 17.123046f, + 17.109896f, 17.105537f, 17.109970f, 17.123195f, 17.145513f, + 17.174057f, 17.192684f, 17.198664f, 17.191998f, 17.172685f, + 17.140513f, 17.102492f, 17.078016f, 17.069548f, 17.077087f, + 17.100633f, 17.140415f, 17.186322f, 17.214985f, 17.223869f, + 17.212975f, 17.182304f, 17.131805f, 17.076241f, 17.041763f, + 17.030634f, 17.042853f, 17.078421f, 17.136064f, 17.192475f, + 17.227989f, 17.242062f, 17.234695f, 17.205887f, 17.158179f, + 17.117207f, 17.092300f, 17.082725f, 17.088485f, 17.109579f, + 17.144212f, 17.174492f, 17.192445f, 17.198302f, 17.192060f, + 17.173720f, 17.144898f, 17.119981f, 17.104937f, 17.099615f, + 17.104017f, 17.118142f, 17.140249f, 17.158227f, 17.168870f, + 17.172387f, 17.168776f, 17.158039f, 17.141471f, 17.127889f, + 17.120051f, 17.117872f, 17.121353f, 17.130495f, 17.144073f, + 17.154974f, 17.161426f, 17.163496f, 17.161186f, 17.154493f, + 17.144572f, 17.137010f, 17.132736f, 17.131697f, 17.133894f, + 17.139326f, 17.147113f, 17.153144f, 17.156619f, 17.157560f, + 17.155968f, 17.151842f, 17.146346f, 17.142425f, 17.139360f, + 17.137112f, 17.135680f, 17.135064f, 17.135266f, 17.136284f, + 17.138119f, 17.140770f, 17.144238f, 17.148523f, 17.153806f, + 17.159002f, 17.163045f, 17.165929f, 17.167654f, 17.168219f, + 17.167625f, 17.165872f, 17.162959f, 17.158887f, 17.153552f, + 17.142846f, 17.132186f, 17.125080f, 17.121530f, 17.121534f, + 17.125092f, 17.132206f, 17.142875f, 17.157098f, 17.174876f, + 17.199034f, 17.241100f, 17.266704f, 17.269161f, 17.248473f, + 17.204664f, 17.153607f, 17.118763f, 17.098656f, 17.093285f, + 17.102651f, 17.121212f, 17.128912f, 17.131624f, 17.130157f, + 17.124511f, 17.114451f, 17.100359f, 17.091754f, 17.090544f, + 17.096730f, 17.110312f, 17.126581f, 17.137666f, 17.145146f, + 17.149027f, 17.149309f, 17.147337f, 17.148861f, 17.150580f, + 17.151938f, 17.152933f, 17.153607f, 17.154989f, 17.156483f, + 17.157608f, 17.158364f, 17.158752f, 17.158770f, 17.158419f, + 17.157700f, 17.156611f, 17.155154f, 17.153320f, 17.151364f, + 17.149788f, 17.148634f, 17.147903f, 17.147594f, 17.147707f, + 17.148243f, 17.149202f, 17.150582f, 17.152386f, 17.154665f, + 17.156997f, 17.158803f, 17.160066f, 17.160786f, 17.160965f, + 17.160601f, 17.159694f, 17.158246f, 17.156255f, 17.153721f, + 17.150495f, 17.147469f, 17.145187f, 17.143651f, 17.142861f, + 17.142816f, 17.143517f, 17.144963f, 17.147155f, 17.150092f, + 17.153797f, 17.158162f, 17.161864f, 17.164624f, 17.166442f, + 17.167319f, 17.167254f, 17.166247f, 17.164298f, 17.161408f, + 17.157576f, 17.152736f, 17.147489f, 17.143301f, 17.140283f, + 17.138437f, 17.137761f, 17.138257f, 17.139923f, 17.142760f, + 17.146768f, 17.151946f, 17.158307f, 17.164450f, 17.169234f, + 17.172644f, 17.174680f, 17.175343f, 17.174631f, 17.172546f, + 17.169088f, 17.164255f, 17.158048f, 17.150889f, 17.144704f, + 17.139984f, 17.136729f, 17.134938f, 17.134611f, 17.135750f, + 17.138353f, 17.142421f, 17.147954f, 17.154890f, 17.161750f, + 17.167314f, 17.171593f, 17.174585f, 17.176291f, 17.176711f, + 17.175845f, 17.173693f, 17.170255f, 17.165530f, 17.159579f, + 17.153725f, 17.149183f, 17.145995f, 17.144161f, 17.143680f, + 17.144554f, 17.146781f, 17.150362f, 17.155297f, 17.161586f, + 17.172204f, 17.185342f, 17.194240f, 17.198838f, 17.199137f, + 17.195136f, 17.186836f, 17.174237f, 17.157338f, 17.135615f, + 17.100103f, 17.069186f, 17.050530f, 17.044136f, 17.050004f, + 17.068133f, 17.098524f, 17.141176f, 17.199011f, 17.256646f, + 17.294777f, 17.313128f, 17.311700f, 17.290493f, 17.249506f, + 17.189165f, 17.126436f, 17.080456f, 17.051819f, 17.040526f, + 17.046577f, 17.069972f, 17.110711f, 17.164189f, 17.207196f, + 17.234827f, 17.247247f, 17.244456f, 17.226453f, 17.193330f, + 17.159281f, 17.136875f, 17.122916f, 17.117404f, 17.120340f, + 17.131723f, 17.150311f, 17.164135f, 17.172849f, 17.177559f, + 17.178265f, 17.174967f, 17.167665f, 17.157695f, 17.150217f, + 17.145815f, 17.144459f, 17.146150f, 17.150888f, 17.158674f, + 17.168134f, 17.175109f, 17.178843f, 17.179336f, 17.176587f, + 17.170596f, 17.161326f, 17.151369f, 17.145037f, 17.142637f, + 17.144171f, 17.149637f, 17.159036f, 17.182643f, 17.213108f, + 17.224750f, 17.217295f, 17.190745f, 17.145098f, 17.076521f}; /* -const float SIMULATED_NOISY_PRESSURE[DATA_SIZE] = {85440.0f, 85440.0f, 85410.0f, 85440.0f, 85440.0f, 85410.0f, 85410.0f, 85410.0f, 85380.0f, 85380.0f, 85350.0f, 85350.0f, 85320.0f, 85290.0f, 85260.0f, 85230.0f, 85200.0f, 85170.0f, 85140.0f, 85110.0f, 85050.0f, 85020.0f, 84960.0f, 84930.0f, 84870.0f, 84810.0f, 84750.0f, 84690.0f, 84630.0f, 84570.0f, 84510.0f, 84450.0f, 84390.0f, 84300.0f, 84240.0f, 84150.0f, 84090.0f, 84030.0f, 83940.0f, 83850.0f, 83790.0f, 83700.0f, 83610.0f, 83520.0f, 83460.0f, 83370.0f, 83280.0f, 83190.0f, 83100.0f, 83010.0f, -82920.0f, 82860.0f, 82740.0f, 82650.0f, 82560.0f, 82470.0f, 82380.0f, 82290.0f, 82200.0f, 82110.0f, 82020.0f, 81930.0f, 81840.0f, 81750.0f, 81660.0f, 81570.0f, 81510.0f, 81390.0f, 81300.0f, 81240.0f, 81120.0f, 81060.0f, 80970.0f, 80880.0f, 80790.0f, 80700.0f, 80640.0f, 80550.0f, 80460.0f, 80370.0f, 80280.0f, 80220.0f, 80130.0f, 80040.0f, 79980.0f, 79890.0f, 79800.0f, 79710.0f, 79650.0f, 79560.0f, 79500.0f, 79410.0f, 79320.0f, 79260.0f, 79170.0f, 79110.0f, 79020.0f, 78930.0f, 78870.0f, 78810.0f, -78720.0f, 78630.0f, 78570.0f, 78510.0f, 78420.0f, 78360.0f, 78300.0f, 78210.0f, 78120.0f, 78060.0f, 78000.0f, 77940.0f, 77850.0f, 77790.0f, 77700.0f, 77640.0f, 77580.0f, 77490.0f, 77430.0f, 77370.0f, 77310.0f, 77250.0f, 77160.0f, 77100.0f, 77040.0f, 76950.0f, 76920.0f, 76830.0f, 76770.0f, 76710.0f, 76650.0f, 76590.0f, 76530.0f, 76440.0f, 76380.0f, 76320.0f, 76260.0f, 76200.0f, 76140.0f, 76080.0f, 76020.0f, 75960.0f, 75900.0f, 75840.0f, 75780.0f, 75720.0f, 75660.0f, 75600.0f, 75540.0f, 75480.0f, -75420.0f, 75360.0f, 75330.0f, 75270.0f, 75210.0f, 75150.0f, 75090.0f, 75030.0f, 74970.0f, 74910.0f, 74880.0f, 74820.0f, 74760.0f, 74700.0f, 74670.0f, 74610.0f, 74550.0f, 74490.0f, 74460.0f, 74400.0f, 74340.0f, 74280.0f, 74220.0f, 74190.0f, 74130.0f, 74070.0f, 74040.0f, 73980.0f, 73920.0f, 73890.0f, 73830.0f, 73800.0f, 73740.0f, 73710.0f, 73650.0f, 73590.0f, 73560.0f, 73500.0f, 73470.0f, 73410.0f, 73350.0f, 73320.0f, 73260.0f, 73230.0f, 73200.0f, 73140.0f, 73110.0f, 73050.0f, 73020.0f, 72990.0f, -72930.0f, 72900.0f, 72840.0f, 72810.0f, 72780.0f, 72720.0f, 72690.0f, 72630.0f, 72600.0f, 72570.0f, 72510.0f, 72480.0f, 72420.0f, 72390.0f, 72360.0f, 72300.0f, 72270.0f, 72240.0f, 72210.0f, 72180.0f, 72150.0f, 72090.0f, 72060.0f, 72030.0f, 71970.0f, 71940.0f, 71910.0f, 71880.0f, 71850.0f, 71820.0f, 71790.0f, 71730.0f, 71700.0f, 71670.0f, 71640.0f, 71610.0f, 71550.0f, 71550.0f, 71490.0f, 71460.0f, 71430.0f, 71400.0f, 71370.0f, 71340.0f, 71310.0f, 71280.0f, 71250.0f, 71220.0f, 71190.0f, 71160.0f, -71130.0f, 71100.0f, 71070.0f, 71010.0f, 71010.0f, 70980.0f, 70950.0f, 70920.0f, 70890.0f, 70860.0f, 70830.0f, 70800.0f, 70770.0f, 70740.0f, 70740.0f, 70710.0f, 70680.0f, 70620.0f, 70620.0f, 70590.0f, 70590.0f, 70560.0f, 70530.0f, 70500.0f, 70500.0f, 70440.0f, 70410.0f, 70410.0f, 70380.0f, 70380.0f, 70350.0f, 70320.0f, 70290.0f, 70290.0f, 70260.0f, 70230.0f, 70200.0f, 70200.0f, 70140.0f, 70170.0f, 70140.0f, 70110.0f, 70080.0f, 70080.0f, 70050.0f, 70020.0f, 69990.0f, 69990.0f, 69960.0f, 69960.0f, -69930.0f, 69930.0f, 69900.0f, 69870.0f, 69870.0f, 69840.0f, 69840.0f, 69810.0f, 69810.0f, 69810.0f, 69780.0f, 69750.0f, 69750.0f, 69720.0f, 69720.0f, 69720.0f, 69690.0f, 69660.0f, 69660.0f, 69630.0f, 69630.0f, 69600.0f, 69600.0f, 69600.0f, 69570.0f, 69570.0f, 69570.0f, 69540.0f, 69510.0f, 69510.0f, 69510.0f, 69480.0f, 69480.0f, 69480.0f, 69480.0f, 69450.0f, 69450.0f, 69420.0f, 69420.0f, 69420.0f, 69390.0f, 69420.0f, 69390.0f, 69390.0f, 69390.0f, 69360.0f, 69360.0f, 69330.0f, 69330.0f, 69330.0f, -69330.0f, 69330.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69210.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69210.0f, 69210.0f, 69210.0f, 69210.0f, 69210.0f, 69210.0f, 69240.0f, 69210.0f, 69210.0f, 69210.0f, 69210.0f, 69210.0f, 69210.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, -69240.0f, 69240.0f, 69240.0f, 69240.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, 69360.0f, 69360.0f, 69360.0f, 69360.0f, 69390.0f, 69360.0f, 69360.0f, 69390.0f, 69390.0f, 69390.0f, 69390.0f, 69390.0f, 69390.0f, 69420.0f, 69420.0f, 69420.0f, 69420.0f, 69420.0f, 69420.0f, 69450.0f, -69450.0f, 69450.0f, 69450.0f, 69450.0f, 69480.0f, 69480.0f, 69450.0f, 69480.0f, 69480.0f, 69480.0f, 69480.0f, 69480.0f, 69510.0f, 69510.0f, 69510.0f, 69540.0f, 69510.0f, 69510.0f, 69540.0f, 69510.0f, 69540.0f, 69540.0f, 69540.0f, 69540.0f, 69540.0f, 69540.0f, 69570.0f, 69570.0f, 69570.0f, 69570.0f, 69570.0f, 69600.0f, 69600.0f, 69600.0f, 69600.0f, 69600.0f, 69630.0f, 69600.0f, 69630.0f, 69630.0f, 69630.0f, 69660.0f, 69630.0f, 69630.0f, 69630.0f, 69660.0f, 69660.0f, 69660.0f, 69660.0f, 69690.0f, -69660.0f, 69690.0f, 69690.0f, 69690.0f, 69690.0f, 69690.0f, 69690.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69750.0f, 69750.0f, 69750.0f, 69780.0f, 69780.0f, 69750.0f, 69780.0f, 69780.0f, 69780.0f, 69780.0f, 69810.0f, 69810.0f, 69810.0f, 69810.0f, 69810.0f, 69840.0f, 69810.0f, 69840.0f, 69840.0f, 69840.0f, 69840.0f, 69840.0f, 69840.0f, 69840.0f, 69870.0f, 69870.0f, 69870.0f, 69900.0f, 69870.0f, 69900.0f, 69900.0f, 69900.0f, 69900.0f, 69900.0f, 69900.0f, -69900.0f, 69930.0f, 69930.0f, 69930.0f, 69930.0f, 69930.0f, 69930.0f, 69930.0f, 69960.0f, 69960.0f, 69960.0f, 69960.0f, 69990.0f, 69990.0f, 69990.0f, 69990.0f, 69990.0f, 69990.0f, 70020.0f, 70020.0f, 70020.0f, 70020.0f, 70020.0f, 70020.0f, 70020.0f, 70050.0f, 70020.0f, 70050.0f, 70050.0f, 70050.0f, 70050.0f, 70050.0f, 70050.0f, 70080.0f, 70080.0f, 70080.0f, 70080.0f, 70080.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70140.0f, 70140.0f, 70140.0f, 70170.0f, -70140.0f, 70140.0f, 70140.0f, 70200.0f, 70200.0f, 70170.0f, 70200.0f, 70200.0f, 70200.0f, 70200.0f, 70200.0f, 70200.0f, 70200.0f, 70230.0f, 70230.0f, 70200.0f, 70230.0f, 70230.0f, 70230.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70290.0f, 70290.0f, 70290.0f, 70290.0f, 70290.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, 70350.0f, 70350.0f, 70350.0f, 70350.0f, 70350.0f, 70380.0f, 70380.0f, 70350.0f, 70380.0f, 70380.0f, 70380.0f, -70380.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70440.0f, 70440.0f, 70440.0f, 70440.0f, 70440.0f, 70470.0f, 70440.0f, 70470.0f, 70470.0f, 70470.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, 70530.0f, 70530.0f, 70530.0f, 70530.0f, 70530.0f, 70560.0f, 70560.0f, 70530.0f, 70560.0f, 70560.0f, 70590.0f, 70560.0f, 70590.0f, 70590.0f, 70590.0f, 70590.0f, 70590.0f, 70590.0f, 70620.0f, 70590.0f, 70620.0f, 70620.0f, 70620.0f, -70620.0f, 70620.0f, 70620.0f, 70650.0f, 70650.0f, 70650.0f, 70650.0f, 70650.0f, 70680.0f, 70650.0f, 70680.0f, 70680.0f, 70680.0f, 70680.0f, 70710.0f, 70680.0f, 70710.0f, 70710.0f, 70740.0f, 70710.0f, 70710.0f, 70710.0f, 70740.0f, 70740.0f, 70740.0f, 70740.0f, 70740.0f, 70770.0f, 70770.0f, 70770.0f, 70800.0f, 70770.0f, 70770.0f, 70770.0f, 70800.0f, 70800.0f, 70800.0f, 70800.0f, 70800.0f, 70800.0f, 70830.0f, 70830.0f, 70830.0f, 70830.0f, 70830.0f, 70830.0f, 70860.0f, 70860.0f, 70860.0f, 70860.0f, -70860.0f, 70860.0f, 70890.0f, 70890.0f, 70860.0f, 70890.0f, 70920.0f, 70890.0f, 70890.0f, 70890.0f, 70920.0f, 70920.0f, 70920.0f, 70950.0f, 70920.0f, 70920.0f, 70920.0f, 70950.0f, 70950.0f, 70950.0f, 70950.0f, 70950.0f, 70980.0f, 70980.0f, 70980.0f, 70980.0f, 70980.0f, 71010.0f, 71010.0f, 71010.0f, 71010.0f, 71010.0f, 71010.0f, 71010.0f, 71040.0f, 71040.0f, 71040.0f, 71040.0f, 71070.0f, 71040.0f, 71070.0f, 71070.0f, 71070.0f, 71070.0f, 71100.0f, 71070.0f, 71100.0f, 71100.0f, 71100.0f, 71100.0f, -71100.0f, 71100.0f, 71100.0f, 71130.0f, 71130.0f, 71130.0f, 71130.0f, 71130.0f, 71130.0f, 71130.0f, 71160.0f, 71160.0f, 71160.0f, 71160.0f, 71160.0f, 71190.0f, 71160.0f, 71190.0f, 71190.0f, 71220.0f, 71190.0f, 71220.0f, 71220.0f, 71220.0f, 71220.0f, 71250.0f, 71220.0f, 71220.0f, 71220.0f, 71220.0f, 71250.0f, 71250.0f, 71250.0f, 71280.0f, 71250.0f, 71280.0f, 71280.0f, 71280.0f, 71280.0f, 71310.0f, 71310.0f, 71310.0f, 71310.0f, 71310.0f, 71310.0f, 71310.0f, 71340.0f, 71340.0f, 71310.0f, 71340.0f, -71340.0f, 71340.0f, 71340.0f, 71370.0f, 71340.0f, 71370.0f, 71370.0f, 71370.0f, 71370.0f, 71370.0f, 71400.0f, 71400.0f, 71400.0f, 71400.0f, 71400.0f, 71430.0f, 71430.0f, 71430.0f, 71430.0f, 71430.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71490.0f, 71490.0f, 71490.0f, 71520.0f, 71490.0f, 71490.0f, 71490.0f, 71520.0f, 71520.0f, 71520.0f, 71520.0f, 71520.0f, 71550.0f, 71550.0f, 71550.0f, 71550.0f, 71550.0f, 71580.0f, 71580.0f, 71580.0f, 71580.0f, 71580.0f, -71580.0f, 71580.0f, 71580.0f, 71610.0f, 71610.0f, 71610.0f, 71610.0f, 71610.0f, 71610.0f, 71640.0f, 71640.0f, 71640.0f, 71640.0f, 71640.0f, 71640.0f, 71640.0f, 71670.0f, 71670.0f, 71670.0f, 71670.0f, 71670.0f, 71700.0f, 71700.0f, 71700.0f, 71700.0f, 71700.0f, 71730.0f, 71700.0f, 71730.0f, 71730.0f, 71730.0f, 71730.0f, 71730.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, 71790.0f, 71760.0f, 71790.0f, 71790.0f, 71790.0f, 71790.0f, 71790.0f, 71820.0f, 71820.0f, 71820.0f, -71820.0f, 71820.0f, 71850.0f, 71820.0f, 71850.0f, 71850.0f, 71850.0f, 71850.0f, 71850.0f, 71850.0f, 71880.0f, 71880.0f, 71880.0f, 71880.0f, 71880.0f, 71910.0f, 71910.0f, 71910.0f, 71910.0f, 71940.0f, 71910.0f, 71910.0f, 71910.0f, 71940.0f, 71940.0f, 71940.0f, 71940.0f, 71970.0f, 71970.0f, 71970.0f, 71970.0f, 71970.0f, 71970.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, 72030.0f, 72030.0f, 72030.0f, 72030.0f, 72030.0f, 72060.0f, 72060.0f, 72060.0f, 72030.0f, 72060.0f, -72060.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72120.0f, 72120.0f, 72120.0f, 72120.0f, 72120.0f, 72120.0f, 72150.0f, 72120.0f, 72150.0f, 72150.0f, 72150.0f, 72150.0f, 72180.0f, 72180.0f, 72180.0f, 72180.0f, 72180.0f, 72180.0f, 72210.0f, 72210.0f, 72210.0f, 72210.0f, 72210.0f, 72210.0f, 72210.0f, 72240.0f, 72240.0f, 72270.0f, 72240.0f, 72240.0f, 72240.0f, 72240.0f, 72270.0f, 72270.0f, 72300.0f, 72270.0f, 72270.0f, 72270.0f, 72270.0f, 72300.0f, 72300.0f, -72330.0f, 72300.0f, 72300.0f, 72330.0f, 72330.0f, 72330.0f, 72330.0f, 72330.0f, 72330.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, 72390.0f, 72390.0f, 72390.0f, 72390.0f, 72390.0f, 72420.0f, 72390.0f, 72420.0f, 72420.0f, 72420.0f, 72420.0f, 72420.0f, 72450.0f, 72420.0f, 72450.0f, 72450.0f, 72450.0f, 72450.0f, 72480.0f, 72480.0f, 72510.0f, 72450.0f, 72480.0f, 72480.0f, 72510.0f, 72510.0f, 72510.0f, 72510.0f, 72510.0f, 72540.0f, 72510.0f, 72510.0f, 72540.0f, 72540.0f, -72540.0f, 72540.0f, 72540.0f, 72570.0f, 72570.0f, 72570.0f, 72570.0f, 72570.0f, 72600.0f, 72600.0f, 72600.0f, 72600.0f, 72600.0f, 72630.0f, 72630.0f, 72630.0f, 72630.0f, 72630.0f, 72630.0f, 72630.0f, 72660.0f, 72630.0f, 72660.0f, 72630.0f, 72660.0f, 72660.0f, 72660.0f, 72660.0f, 72690.0f, 72690.0f, 72690.0f, 72690.0f, 72720.0f, 72690.0f, 72720.0f, 72720.0f, 72720.0f, 72720.0f, 72750.0f, 72720.0f, 72720.0f, 72720.0f, 72750.0f, 72750.0f, 72750.0f, 72780.0f, 72750.0f, 72780.0f, 72780.0f, 72780.0f, -72780.0f, 72780.0f, 72780.0f, 72780.0f, 72810.0f, 72810.0f, 72810.0f, 72810.0f, 72810.0f, 72810.0f, 72840.0f, 72840.0f, 72840.0f, 72840.0f, 72870.0f, 72840.0f, 72870.0f, 72870.0f, 72870.0f, 72870.0f, 72900.0f, 72900.0f, 72900.0f, 72900.0f, 72900.0f, 72930.0f, 72900.0f, 72930.0f, 72930.0f, 72930.0f, 72930.0f, 72930.0f, 72960.0f, 72930.0f, 72960.0f, 72960.0f, 72960.0f, 72960.0f, 72960.0f, 72960.0f, 72990.0f, 72990.0f, 72990.0f, 72990.0f, 72990.0f, 72990.0f, 72990.0f, 73020.0f, 73020.0f, 73020.0f, -73050.0f, 73050.0f, 73050.0f, 73050.0f, 73050.0f, 73050.0f, 73080.0f, 73080.0f, 73050.0f, 73080.0f, 73080.0f, 73080.0f, 73080.0f, 73110.0f, 73080.0f, 73110.0f, 73110.0f, 73110.0f, 73110.0f, 73140.0f, 73140.0f, 73110.0f, 73140.0f, 73140.0f, 73140.0f, 73140.0f, 73140.0f, 73170.0f, 73170.0f, 73170.0f, 73170.0f, 73170.0f, 73200.0f, 73200.0f, 73200.0f, 73200.0f, 73200.0f, 73200.0f, 73230.0f, 73230.0f, 73200.0f, 73230.0f, 73230.0f, 73230.0f, 73230.0f, 73230.0f, 73260.0f, 73260.0f, 73260.0f, 73260.0f, -73290.0f, 73260.0f, 73290.0f, 73260.0f, 73290.0f, 73290.0f, 73320.0f, 73290.0f, 73290.0f, 73320.0f, 73320.0f, 73320.0f, 73320.0f, 73320.0f, 73350.0f, 73350.0f, 73350.0f, 73350.0f, 73350.0f, 73380.0f, 73380.0f, 73380.0f, 73380.0f, 73380.0f, 73380.0f, 73410.0f, 73380.0f, 73410.0f, 73410.0f, 73410.0f, 73410.0f, 73410.0f, 73440.0f, 73440.0f, 73440.0f, 73440.0f, 73440.0f, 73470.0f, 73440.0f, 73470.0f, 73440.0f, 73470.0f, 73470.0f, 73470.0f, 73470.0f, 73500.0f, 73500.0f, 73500.0f, 73500.0f, 73530.0f, -73530.0f, 73530.0f, 73530.0f, 73530.0f, 73530.0f, 73530.0f, 73530.0f, 73560.0f, 73560.0f, 73560.0f, 73560.0f, 73560.0f, 73560.0f, 73590.0f, 73590.0f, 73590.0f, 73590.0f, 73590.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73650.0f, 73650.0f, 73680.0f, 73680.0f, 73650.0f, 73650.0f, 73680.0f, 73680.0f, 73680.0f, 73680.0f, 73680.0f, 73710.0f, 73710.0f, 73710.0f, 73710.0f, 73710.0f, 73710.0f, 73740.0f, 73740.0f, 73740.0f, 73740.0f, 73770.0f, 73740.0f, -73740.0f, 73770.0f, 73770.0f, 73770.0f, 73770.0f, 73770.0f, 73770.0f, 73800.0f, 73800.0f, 73830.0f, 73800.0f, 73800.0f, 73830.0f, 73830.0f, 73830.0f, 73830.0f, 73830.0f, 73860.0f, 73860.0f, 73860.0f, 73830.0f, 73860.0f, 73860.0f, 73860.0f, 73860.0f, 73890.0f, 73890.0f, 73890.0f, 73890.0f, 73890.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73950.0f, 73950.0f, 73950.0f, 73950.0f, 73950.0f, 73950.0f, 73980.0f, 73980.0f, 73980.0f, 73980.0f, 73980.0f, 74010.0f, -73980.0f, 74010.0f, 74010.0f, 74010.0f, 74010.0f, 74040.0f, 74010.0f, 74040.0f, 74040.0f, 74040.0f, 74040.0f, 74040.0f, 74040.0f, 74070.0f, 74070.0f, 74070.0f, 74070.0f, 74100.0f, 74070.0f, 74070.0f, 74100.0f, 74100.0f, 74100.0f, 74130.0f, 74130.0f, 74130.0f, 74100.0f, 74130.0f, 74130.0f, 74130.0f, 74130.0f, 74130.0f, 74160.0f, 74160.0f, 74160.0f, 74160.0f, 74160.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, 74220.0f, 74220.0f, 74220.0f, 74220.0f, 74220.0f, 74250.0f, -74220.0f, 74250.0f, 74250.0f, 74250.0f, 74280.0f, 74250.0f, 74280.0f, 74280.0f, 74280.0f, 74280.0f, 74280.0f, 74310.0f, 74280.0f, 74310.0f, 74310.0f, 74310.0f, 74310.0f, 74310.0f, 74340.0f, 74340.0f, 74340.0f, 74340.0f, 74340.0f, 74340.0f, 74370.0f, 74370.0f, 74370.0f, 74370.0f, 74370.0f, 74370.0f, 74400.0f, 74400.0f, 74400.0f, 74400.0f, 74430.0f, 74400.0f, 74430.0f, 74400.0f, 74430.0f, 74430.0f, 74430.0f, 74460.0f, 74430.0f, 74460.0f, 74460.0f, 74460.0f, 74460.0f, 74490.0f, 74460.0f, 74460.0f, -74490.0f, 74490.0f, 74490.0f, 74490.0f, 74520.0f, 74520.0f, 74520.0f, 74520.0f, 74520.0f, 74550.0f, 74520.0f, 74550.0f, 74550.0f, 74550.0f, 74550.0f, 74550.0f, 74550.0f, 74550.0f, 74580.0f, 74580.0f, 74580.0f, 74610.0f, 74580.0f, 74610.0f, 74610.0f, 74610.0f, 74610.0f, 74610.0f, 74610.0f, 74610.0f, 74640.0f, 74640.0f, 74640.0f, 74640.0f, 74640.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, 74730.0f, -74730.0f, 74730.0f, 74730.0f, 74760.0f, 74760.0f, 74760.0f, 74760.0f, 74790.0f, 74760.0f, 74790.0f, 74790.0f, 74790.0f, 74790.0f, 74790.0f, 74790.0f, 74790.0f, 74790.0f, 74820.0f, 74820.0f, 74820.0f, 74820.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74880.0f, 74880.0f, 74880.0f, 74880.0f, 74910.0f, 74880.0f, 74910.0f, 74910.0f, 74910.0f, 74910.0f, 74910.0f, 74940.0f, 74910.0f, 74940.0f, 74940.0f, 74940.0f, 74940.0f, 74940.0f, 74970.0f, 74970.0f, 74970.0f, -74970.0f, 74970.0f, 74970.0f, 75000.0f, 75000.0f, 75000.0f, 75000.0f, 75000.0f, 75030.0f, 75030.0f, 75030.0f, 75030.0f, 75030.0f, 75030.0f, 75030.0f, 75060.0f, 75060.0f, 75060.0f, 75060.0f, 75060.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75120.0f, 75090.0f, 75120.0f, 75120.0f, 75120.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, 75180.0f, 75180.0f, 75180.0f, 75180.0f, 75180.0f, 75180.0f, 75210.0f, 75210.0f, 75210.0f, 75210.0f, -75210.0f, 75240.0f, 75210.0f, 75240.0f, 75240.0f, 75240.0f, 75240.0f, 75240.0f, 75270.0f, 75270.0f, 75240.0f, 75270.0f, 75270.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75330.0f, 75360.0f, 75330.0f, 75330.0f, 75360.0f, 75360.0f, 75360.0f, 75360.0f, 75360.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, 75420.0f, 75420.0f, 75420.0f, 75450.0f, 75450.0f, 75420.0f, 75450.0f, 75450.0f, 75450.0f, 75450.0f, 75450.0f, -75480.0f, 75480.0f, 75480.0f, 75480.0f, 75480.0f, 75480.0f, 75510.0f, 75510.0f, 75510.0f, 75510.0f, 75510.0f, 75510.0f, 75510.0f, 75540.0f, 75540.0f, 75540.0f, 75540.0f, 75540.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75600.0f, 75600.0f, 75600.0f, 75600.0f, 75600.0f, 75600.0f, 75600.0f, 75630.0f, 75630.0f, 75630.0f, 75630.0f, 75660.0f, 75660.0f, 75630.0f, 75660.0f, 75660.0f, 75690.0f, 75660.0f, 75660.0f, 75690.0f, 75690.0f, 75690.0f, 75690.0f, 75690.0f, -75720.0f, 75720.0f, 75720.0f, 75720.0f, 75750.0f, 75750.0f, 75750.0f, 75720.0f, 75750.0f, 75750.0f, 75780.0f, 75750.0f, 75780.0f, 75750.0f, 75780.0f, 75810.0f, 75780.0f, 75810.0f, 75810.0f, 75780.0f, 75810.0f, 75810.0f, 75810.0f, 75810.0f, 75810.0f, 75810.0f, 75840.0f, 75840.0f, 75840.0f, 75840.0f, 75870.0f, 75870.0f, 75870.0f, 75870.0f, 75870.0f, 75900.0f, 75870.0f, 75900.0f, 75900.0f, 75900.0f, 75930.0f, 75900.0f, 75900.0f, 75930.0f, 75930.0f, 75930.0f, 75930.0f, 75930.0f, 75930.0f, 75960.0f, -75960.0f, 75960.0f, 75960.0f, 75960.0f, 75960.0f, 75990.0f, 75990.0f, 75990.0f, 75990.0f, 76020.0f, 75990.0f, 76020.0f, 76020.0f, 76020.0f, 76020.0f, 76020.0f, 76050.0f, 76050.0f, 76020.0f, 76050.0f, 76050.0f, 76050.0f, 76050.0f, 76080.0f, 76080.0f, 76080.0f, 76080.0f, 76080.0f, 76080.0f, 76110.0f, 76110.0f, 76110.0f, 76110.0f, 76110.0f, 76110.0f, 76140.0f, 76140.0f, 76140.0f, 76140.0f, 76140.0f, 76140.0f, 76140.0f, 76170.0f, 76170.0f, 76170.0f, 76170.0f, 76170.0f, 76170.0f, 76200.0f, 76200.0f, -76200.0f, 76200.0f, 76200.0f, 76230.0f, 76230.0f, 76230.0f, 76230.0f, 76230.0f, 76230.0f, 76230.0f, 76230.0f, 76260.0f, 76260.0f, 76260.0f, 76260.0f, 76260.0f, 76290.0f, 76290.0f, 76290.0f, 76290.0f, 76320.0f, 76290.0f, 76320.0f, 76320.0f, 76290.0f, 76320.0f, 76320.0f, 76350.0f, 76350.0f, 76350.0f, 76350.0f, 76350.0f, 76350.0f, 76380.0f, 76380.0f, 76350.0f, 76380.0f, 76380.0f, 76380.0f, 76380.0f, 76410.0f, 76410.0f, 76410.0f, 76410.0f, 76410.0f, 76440.0f, 76440.0f, 76410.0f, 76440.0f, 76440.0f, -76440.0f, 76440.0f, 76440.0f, 76440.0f, 76470.0f, 76470.0f, 76470.0f, 76470.0f, 76470.0f, 76500.0f, 76500.0f, 76500.0f, 76500.0f, 76500.0f, 76530.0f, 76500.0f, 76530.0f, 76530.0f, 76530.0f, 76530.0f, 76530.0f, 76530.0f, 76560.0f, 76560.0f, 76560.0f, 76560.0f, 76590.0f, 76560.0f, 76590.0f, 76590.0f, 76590.0f, 76620.0f, 76620.0f, 76590.0f, 76590.0f, 76590.0f, 76620.0f, 76620.0f, 76650.0f, 76620.0f, 76620.0f, 76650.0f, 76650.0f, 76650.0f, 76650.0f, 76650.0f, 76680.0f, 76680.0f, 76680.0f, 76680.0f, -76680.0f, 76710.0f, 76680.0f, 76710.0f, 76710.0f, 76740.0f, 76710.0f, 76710.0f, 76740.0f, 76710.0f, 76740.0f, 76740.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, 76800.0f, 76800.0f, 76800.0f, 76800.0f, 76800.0f, 76800.0f, 76830.0f, 76830.0f, 76830.0f, 76830.0f, 76830.0f, 76830.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76890.0f, 76890.0f, 76890.0f, 76890.0f, 76920.0f, 76920.0f, 76920.0f, 76920.0f, 76920.0f, 76920.0f, 76920.0f, -76950.0f, 76950.0f, 76950.0f, 76950.0f, 76950.0f, 76980.0f, 76980.0f, 76980.0f, 76980.0f, 76980.0f, 76980.0f, 77010.0f, 76980.0f, 77010.0f, 77010.0f, 77010.0f, 77040.0f, 77010.0f, 77010.0f, 77040.0f, 77040.0f, 77040.0f, 77040.0f, 77040.0f, 77040.0f, 77070.0f, 77070.0f, 77070.0f, 77070.0f, 77100.0f, 77070.0f, 77100.0f, 77100.0f, 77130.0f, 77100.0f, 77100.0f, 77100.0f, 77100.0f, 77130.0f, 77130.0f, 77130.0f, 77130.0f, 77130.0f, 77160.0f, 77160.0f, 77160.0f, 77160.0f, 77160.0f, 77160.0f, 77160.0f, -77190.0f, 77190.0f, 77190.0f, 77190.0f, 77220.0f, 77220.0f, 77190.0f, 77220.0f, 77220.0f, 77250.0f, 77220.0f, 77250.0f, 77250.0f, 77250.0f, 77250.0f, 77250.0f, 77280.0f, 77280.0f, 77280.0f, 77280.0f, 77280.0f, 77280.0f, 77280.0f, 77310.0f, 77310.0f, 77310.0f, 77310.0f, 77310.0f, 77310.0f, 77310.0f, 77340.0f, 77370.0f, 77340.0f, 77340.0f, 77340.0f, 77370.0f, 77370.0f, 77370.0f, 77370.0f, 77370.0f, 77400.0f, 77400.0f, 77400.0f, 77400.0f, 77400.0f, 77400.0f, 77400.0f, 77430.0f, 77430.0f, 77430.0f, -77430.0f, 77430.0f, 77460.0f, 77460.0f, 77460.0f, 77460.0f, 77460.0f, 77460.0f, 77490.0f, 77490.0f, 77490.0f, 77490.0f, 77490.0f, 77490.0f, 77490.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77550.0f, 77550.0f, 77550.0f, 77580.0f, 77580.0f, 77580.0f, 77580.0f, 77580.0f, 77610.0f, 77610.0f, 77610.0f, 77610.0f, 77610.0f, 77610.0f, 77640.0f, 77610.0f, 77610.0f, 77640.0f, 77640.0f, 77640.0f, 77640.0f, 77670.0f, 77670.0f, 77670.0f, 77670.0f, 77670.0f, 77670.0f, -77670.0f, 77670.0f, 77670.0f, 77700.0f, 77700.0f, 77700.0f, 77700.0f, 77700.0f, 77730.0f, 77730.0f, 77730.0f, 77730.0f, 77730.0f, 77730.0f, 77760.0f, 77760.0f, 77760.0f, 77760.0f, 77760.0f, 77760.0f, 77790.0f, 77790.0f, 77790.0f, 77790.0f, 77790.0f, 77790.0f, 77820.0f, 77820.0f, 77820.0f, 77820.0f, 77820.0f, 77820.0f, 77850.0f, 77850.0f, 77850.0f, 77850.0f, 77850.0f, 77880.0f, 77880.0f, 77850.0f, 77880.0f, 77880.0f, 77880.0f, 77910.0f, 77880.0f, 77910.0f, 77910.0f, 77910.0f, 77910.0f, 77940.0f, -77940.0f, 77940.0f, 77940.0f, 77940.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 78000.0f, 78000.0f, 78000.0f, 78000.0f, 78030.0f, 78030.0f, 78000.0f, 78060.0f, 78030.0f, 78030.0f, 78030.0f, 78030.0f, 78030.0f, 78060.0f, 78060.0f, 78060.0f, 78060.0f, 78060.0f, 78090.0f, 78090.0f, 78090.0f, 78090.0f, 78090.0f, 78120.0f, 78120.0f, 78120.0f, 78120.0f, 78120.0f, 78150.0f, 78150.0f, 78150.0f, 78150.0f, 78150.0f, 78150.0f, 78180.0f, 78180.0f, 78180.0f, -78180.0f, 78180.0f, 78180.0f, 78210.0f, 78210.0f, 78210.0f, 78210.0f, 78210.0f, 78210.0f, 78210.0f, 78240.0f, 78240.0f, 78240.0f, 78240.0f, 78240.0f, 78270.0f, 78270.0f, 78270.0f, 78270.0f, 78270.0f, 78300.0f, 78300.0f, 78270.0f, 78300.0f, 78300.0f, 78300.0f, 78300.0f, 78300.0f, 78300.0f, 78300.0f, 78330.0f, 78330.0f, 78330.0f, 78330.0f, 78360.0f, 78330.0f, 78360.0f, 78360.0f, 78360.0f, 78360.0f, 78390.0f, 78390.0f, 78390.0f, 78390.0f, 78390.0f, 78390.0f, 78420.0f, 78390.0f, 78420.0f, 78420.0f, -78420.0f, 78450.0f, 78420.0f, 78450.0f, 78450.0f, 78450.0f, 78450.0f, 78450.0f, 78450.0f, 78450.0f, 78480.0f, 78480.0f, 78480.0f, 78480.0f, 78480.0f, 78510.0f, 78510.0f, 78510.0f, 78510.0f, 78510.0f, 78510.0f, 78510.0f, 78540.0f, 78540.0f, 78570.0f, 78540.0f, 78570.0f, 78570.0f, 78570.0f, 78570.0f, 78570.0f, 78600.0f, 78570.0f, 78600.0f, 78600.0f, 78600.0f, 78600.0f, 78600.0f, 78600.0f, 78630.0f, 78630.0f, 78630.0f, 78630.0f, 78630.0f, 78630.0f, 78660.0f, 78660.0f, 78660.0f, 78660.0f, 78660.0f, -78660.0f, 78660.0f, 78690.0f, 78690.0f, 78690.0f, 78690.0f, 78720.0f, 78720.0f, 78690.0f, 78720.0f, 78720.0f, 78720.0f, 78720.0f, 78750.0f, 78720.0f, 78750.0f, 78750.0f, 78750.0f, 78750.0f, 78780.0f, 78780.0f, 78780.0f, 78780.0f, 78780.0f, 78780.0f, 78810.0f, 78810.0f, 78810.0f, 78810.0f, 78810.0f, 78840.0f, 78810.0f, 78840.0f, 78840.0f, 78840.0f, 78840.0f, 78870.0f, 78870.0f, 78840.0f, 78870.0f, 78870.0f, 78870.0f, 78900.0f, 78900.0f, 78900.0f, 78900.0f, 78900.0f, 78900.0f, 78930.0f, 78900.0f, -78930.0f, 78930.0f, 78930.0f, 78930.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, 78990.0f, 78990.0f, 78990.0f, 78990.0f, 78990.0f, 78990.0f, 79020.0f, 79020.0f, 79020.0f, 79020.0f, 79020.0f, 79020.0f, 79050.0f, 79050.0f, 79050.0f, 79050.0f, 79050.0f, 79050.0f, 79080.0f, 79080.0f, 79080.0f, 79080.0f, 79080.0f, 79110.0f, 79110.0f, 79110.0f, 79110.0f, 79110.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79170.0f, 79170.0f, 79170.0f, -79170.0f, 79200.0f, 79200.0f, 79170.0f, 79170.0f, 79200.0f, 79200.0f, 79200.0f, 79200.0f, 79200.0f, 79200.0f, 79230.0f, 79230.0f, 79230.0f, 79260.0f, 79230.0f, 79260.0f, 79260.0f, 79260.0f, 79260.0f, 79260.0f, 79290.0f, 79290.0f, 79290.0f, 79290.0f, 79320.0f, 79320.0f, 79290.0f, 79320.0f, 79320.0f, 79320.0f, 79320.0f, 79320.0f, 79350.0f, 79350.0f, 79350.0f, 79350.0f, 79350.0f, 79350.0f, 79380.0f, 79350.0f, 79380.0f, 79380.0f, 79380.0f, 79380.0f, 79410.0f, 79410.0f, 79410.0f, 79410.0f, 79410.0f, -79410.0f, 79410.0f, 79440.0f, 79440.0f, 79440.0f, 79440.0f, 79440.0f, 79470.0f, 79470.0f, 79470.0f, 79470.0f, 79470.0f, 79470.0f, 79500.0f, 79500.0f, 79500.0f, 79500.0f, 79500.0f, 79500.0f, 79500.0f, 79530.0f, 79500.0f, 79530.0f, 79530.0f, 79530.0f, 79530.0f, 79560.0f, 79560.0f, 79560.0f, 79560.0f, 79560.0f, 79560.0f, 79590.0f, 79590.0f, 79590.0f, 79590.0f, 79590.0f, 79590.0f, 79620.0f, 79620.0f, 79620.0f, 79620.0f, 79620.0f, 79620.0f, 79650.0f, 79650.0f, 79650.0f, 79650.0f, 79680.0f, 79680.0f, -79680.0f, 79680.0f, 79680.0f, 79710.0f, 79680.0f, 79680.0f, 79710.0f, 79710.0f, 79710.0f, 79710.0f, 79740.0f, 79740.0f, 79740.0f, 79740.0f, 79740.0f, 79740.0f, 79740.0f, 79770.0f, 79770.0f, 79770.0f, 79770.0f, 79770.0f, 79770.0f, 79800.0f, 79800.0f, 79800.0f, 79800.0f, 79800.0f, 79800.0f, 79830.0f, 79830.0f, 79830.0f, 79830.0f, 79830.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79890.0f, 79890.0f, 79890.0f, 79890.0f, 79890.0f, 79890.0f, 79920.0f, 79920.0f, -79920.0f, 79950.0f, 79920.0f, 79950.0f, 79950.0f, 79950.0f, 79950.0f, 79950.0f, 79980.0f, 79980.0f, 79950.0f, 79980.0f, 79980.0f, 79980.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, 80040.0f, 80040.0f, 80040.0f, 80040.0f, 80040.0f, 80040.0f, 80070.0f, 80070.0f, 80070.0f, 80070.0f, 80070.0f, 80100.0f, 80100.0f, 80100.0f, 80100.0f, 80100.0f, 80100.0f, 80130.0f, 80130.0f, 80130.0f, 80130.0f, 80130.0f, 80160.0f, 80130.0f, 80130.0f, 80130.0f, 80160.0f, 80160.0f, 80160.0f, -80190.0f, 80190.0f, 80190.0f, 80190.0f, 80190.0f, 80190.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80250.0f, 80250.0f, 80250.0f, 80250.0f, 80280.0f, 80250.0f, 80280.0f, 80280.0f, 80280.0f, 80280.0f, 80280.0f, 80310.0f, 80310.0f, 80310.0f, 80310.0f, 80310.0f, 80340.0f, 80310.0f, 80340.0f, 80340.0f, 80340.0f, 80340.0f, 80370.0f, 80370.0f, 80370.0f, 80370.0f, 80400.0f, 80370.0f, 80370.0f, 80370.0f, 80400.0f, 80400.0f, 80400.0f, 80400.0f, 80430.0f, 80430.0f, -80430.0f, 80430.0f, 80430.0f, 80430.0f, 80430.0f, 80460.0f, 80460.0f, 80460.0f, 80460.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80520.0f, 80520.0f, 80520.0f, 80520.0f, 80520.0f, 80550.0f, 80550.0f, 80550.0f, 80550.0f, 80550.0f, 80580.0f, 80550.0f, 80580.0f, 80580.0f, 80580.0f, 80580.0f, 80580.0f, 80610.0f, 80610.0f, 80610.0f, 80610.0f, 80610.0f, 80610.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, 80670.0f, 80670.0f, 80670.0f, -80670.0f, 80670.0f, 80700.0f, 80700.0f, 80700.0f, 80700.0f, 80700.0f, 80700.0f, 80700.0f, 80730.0f, 80730.0f, 80730.0f, 80760.0f, 80730.0f, 80760.0f, 80760.0f, 80760.0f, 80760.0f, 80760.0f, 80760.0f, 80760.0f, 80790.0f, 80790.0f, 80790.0f, 80790.0f, 80790.0f, 80820.0f, 80820.0f, 80820.0f, 80820.0f, 80820.0f, 80820.0f, 80820.0f, 80850.0f, 80850.0f, 80850.0f, 80850.0f, 80880.0f, 80880.0f, 80850.0f, 80880.0f, 80880.0f, 80880.0f, 80880.0f, 80910.0f, 80910.0f, 80910.0f, 80910.0f, 80910.0f, 80910.0f, -80940.0f, 80940.0f, 80940.0f, 80940.0f, 80940.0f, 80940.0f, 80970.0f, 80970.0f, 80970.0f, 80970.0f, 80970.0f, 80970.0f, 81000.0f, 81000.0f, 81000.0f, 81000.0f, 81000.0f, 81000.0f, 81030.0f, 81030.0f, 81030.0f, 81030.0f, 81060.0f, 81060.0f, 81060.0f, 81060.0f, 81060.0f, 81060.0f, 81090.0f, 81060.0f, 81090.0f, 81090.0f, 81090.0f, 81090.0f, 81090.0f, 81120.0f, 81120.0f, 81120.0f, 81090.0f, 81120.0f, 81120.0f, 81120.0f, 81120.0f, 81150.0f, 81150.0f, 81150.0f, 81150.0f, 81180.0f, 81180.0f, 81180.0f, -81180.0f, 81180.0f, 81180.0f, 81180.0f, 81180.0f, 81210.0f, 81210.0f, 81210.0f, 81210.0f, 81240.0f, 81210.0f, 81240.0f, 81240.0f, 81240.0f, 81240.0f, 81240.0f, 81270.0f, 81270.0f, 81270.0f, 81270.0f, 81270.0f, 81300.0f, 81300.0f, 81300.0f, 81300.0f, 81300.0f, 81300.0f, 81300.0f, 81330.0f, 81330.0f, 81330.0f, 81330.0f, 81330.0f, 81360.0f, 81360.0f, 81360.0f, 81360.0f, 81360.0f, 81390.0f, 81360.0f, 81390.0f, 81390.0f, 81390.0f, 81390.0f, 81390.0f, 81420.0f, 81420.0f, 81420.0f, 81420.0f, 81420.0f, -81420.0f, 81450.0f, 81420.0f, 81450.0f, 81450.0f, 81450.0f, 81450.0f, 81480.0f, 81480.0f, 81480.0f, 81480.0f, 81510.0f, 81510.0f, 81510.0f, 81510.0f, 81510.0f, 81510.0f, 81510.0f, 81510.0f, 81540.0f, 81540.0f, 81540.0f, 81540.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, 81600.0f, 81600.0f, 81600.0f, 81600.0f, 81600.0f, 81630.0f, 81600.0f, 81630.0f, 81630.0f, 81630.0f, 81660.0f, 81630.0f, 81660.0f, 81660.0f, 81660.0f, 81660.0f, 81690.0f, 81690.0f, 81660.0f, -81690.0f, 81690.0f, 81690.0f, 81690.0f, 81720.0f, 81720.0f, 81720.0f, 81720.0f, 81720.0f, 81720.0f, 81750.0f, 81750.0f, 81750.0f, 81750.0f, 81750.0f, 81780.0f, 81780.0f, 81780.0f, 81780.0f, 81780.0f, 81780.0f, 81810.0f, 81810.0f, 81780.0f, 81810.0f, 81810.0f, 81810.0f, 81840.0f, 81810.0f, 81840.0f, 81840.0f, 81840.0f, 81840.0f, 81840.0f, 81870.0f, 81870.0f, 81870.0f, 81870.0f, 81870.0f, 81870.0f, 81900.0f, 81900.0f, 81900.0f, 81900.0f, 81900.0f, 81930.0f, 81930.0f, 81900.0f, 81930.0f, 81930.0f, -81930.0f, 81960.0f, 81960.0f, 81960.0f, 81960.0f, 81960.0f, 81960.0f, 81990.0f, 81960.0f, 81990.0f, 81990.0f, 81990.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, 82050.0f, 82020.0f, 82050.0f, 82050.0f, 82050.0f, 82050.0f, 82080.0f, 82050.0f, 82080.0f, 82080.0f, 82080.0f, 82110.0f, 82110.0f, 82110.0f, 82110.0f, 82110.0f, 82110.0f, 82140.0f, 82110.0f, 82140.0f, 82140.0f, 82140.0f, 82140.0f, 82140.0f, 82170.0f, 82140.0f, 82170.0f, 82200.0f, 82200.0f, 82200.0f, 82200.0f, -82200.0f, 82200.0f, 82200.0f, 82200.0f, 82200.0f, 82200.0f, 82230.0f, 82230.0f, 82230.0f, 82230.0f, 82230.0f, 82230.0f, 82260.0f, 82260.0f, 82260.0f, 82260.0f, 82260.0f, 82290.0f, 82290.0f, 82290.0f, 82290.0f, 82290.0f, 82290.0f, 82320.0f, 82320.0f, 82320.0f, 82320.0f, 82320.0f, 82350.0f, 82350.0f, 82350.0f, 82350.0f, 82350.0f, 82350.0f, 82350.0f, 82380.0f, 82380.0f, 82380.0f, 82380.0f, 82380.0f, 82380.0f, 82410.0f, 82410.0f, 82410.0f, 82410.0f, 82410.0f, 82410.0f, 82440.0f, 82440.0f, 82440.0f, -82440.0f, 82470.0f, 82470.0f, 82470.0f, 82440.0f, 82470.0f, 82500.0f, 82470.0f, 82500.0f, 82470.0f, 82500.0f, 82500.0f, 82500.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82560.0f, 82560.0f, 82560.0f, 82560.0f, 82590.0f, 82590.0f, 82590.0f, 82590.0f, 82590.0f, 82590.0f, 82620.0f, 82620.0f, 82620.0f, 82620.0f, 82620.0f, 82650.0f, 82620.0f, 82650.0f, 82650.0f, 82650.0f, 82650.0f, 82650.0f, 82680.0f, 82650.0f, 82680.0f, 82680.0f, 82710.0f, 82710.0f, -82710.0f, 82710.0f, 82710.0f, 82710.0f, 82710.0f, 82710.0f, 82710.0f, 82740.0f, 82740.0f, 82740.0f, 82740.0f, 82740.0f, 82740.0f, 82770.0f, 82770.0f, 82770.0f, 82770.0f, 82800.0f, 82800.0f, 82800.0f, 82800.0f, 82800.0f, 82800.0f, 82830.0f, 82830.0f, 82830.0f, 82860.0f, 82830.0f, 82830.0f, 82860.0f, 82860.0f, 82860.0f, 82860.0f, 82860.0f, 82890.0f, 82860.0f, 82890.0f, 82890.0f, 82890.0f, 82890.0f, 82920.0f, 82890.0f, 82920.0f, 82920.0f, 82920.0f, 82950.0f, 82920.0f, 82950.0f, 82950.0f, 82950.0f, -82950.0f, 82950.0f, 82980.0f, 82980.0f, 82980.0f, 82980.0f, 82980.0f, 82980.0f, 83010.0f, 83010.0f, 83010.0f, 83010.0f, 83010.0f, 83010.0f, 83040.0f, 83040.0f, 83040.0f, 83040.0f, 83070.0f, 83040.0f, 83040.0f, 83070.0f, 83070.0f, 83070.0f, 83070.0f, 83070.0f, 83100.0f, 83100.0f, 83100.0f, 83100.0f, 83100.0f, 83100.0f, 83100.0f, 83130.0f, 83130.0f, 83130.0f, 83160.0f, 83130.0f, 83160.0f, 83160.0f, 83160.0f, 83190.0f, 83160.0f, 83160.0f, 83190.0f, 83190.0f, 83190.0f, 83190.0f, 83190.0f, 83220.0f, -83220.0f, 83220.0f, 83220.0f, 83220.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83310.0f, 83280.0f, 83310.0f, 83310.0f, 83280.0f, -83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83280.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83340.0f, 83310.0f, 83340.0f, 83310.0f, 83310.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83310.0f, 83340.0f, 83340.0f, 83310.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83310.0f, 83340.0f, 83340.0f, -83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83370.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83370.0f, 83340.0f, 83340.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83340.0f, 83340.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83400.0f, 83370.0f, 83370.0f, 83400.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83400.0f, 83400.0f, 83370.0f, 83370.0f, -83400.0f, 83370.0f, 83370.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83370.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83370.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83430.0f, 83400.0f, 83400.0f, 83400.0f, 83430.0f, 83400.0f, 83400.0f, 83430.0f, 83400.0f, 83400.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, -83400.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83460.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83460.0f, 83430.0f, 83460.0f, 83460.0f, 83430.0f, 83430.0f, 83460.0f, 83430.0f, 83430.0f, 83460.0f, 83460.0f, 83430.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, -83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83490.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83490.0f, 83490.0f, 83460.0f, 83490.0f, 83460.0f, 83460.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83520.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83520.0f, 83520.0f, 83520.0f, 83490.0f, 83520.0f, 83520.0f, 83490.0f, 83490.0f, 83490.0f, 83520.0f, 83520.0f, 83490.0f, 83520.0f, -83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83490.0f, 83520.0f, 83490.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83550.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83550.0f, 83550.0f, 83520.0f, 83520.0f, 83550.0f, 83520.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83520.0f, 83550.0f, 83550.0f, 83520.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, -83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83580.0f, 83580.0f, 83580.0f, 83550.0f, 83550.0f, 83550.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83550.0f, 83580.0f, 83550.0f, 83580.0f, 83550.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83610.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83610.0f, 83610.0f, 83580.0f, -83580.0f, 83610.0f, 83580.0f, 83580.0f, 83610.0f, 83610.0f, 83580.0f, 83580.0f, 83610.0f, 83610.0f, 83580.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83580.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83640.0f, 83640.0f, 83640.0f, 83610.0f, 83640.0f, 83610.0f, 83610.0f, 83640.0f, 83640.0f, 83640.0f, 83610.0f, 83640.0f, 83610.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83610.0f, 83640.0f, 83610.0f, -83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83670.0f, 83640.0f, 83670.0f, 83670.0f, 83670.0f, 83640.0f, 83670.0f, 83670.0f, 83640.0f, 83670.0f, 83670.0f, 83640.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83700.0f, 83670.0f, 83670.0f, 83700.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, -83670.0f, 83700.0f, 83670.0f, 83670.0f, 83700.0f, 83700.0f, 83670.0f, 83700.0f, 83700.0f, 83700.0f, 83670.0f, 83700.0f, 83700.0f, 83670.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83730.0f, 83700.0f, 83730.0f, 83700.0f, 83700.0f, 83730.0f, 83700.0f, 83700.0f, 83730.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83730.0f, 83730.0f, 83700.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, -83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83760.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83760.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83760.0f, 83760.0f, 83730.0f, 83760.0f, 83760.0f, 83730.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, -83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83790.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83790.0f, 83790.0f, 83760.0f, 83760.0f, 83760.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83820.0f, 83790.0f, 83820.0f, 83790.0f, 83790.0f, 83820.0f, 83790.0f, 83820.0f, -83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83850.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83850.0f, 83820.0f, 83820.0f, 83820.0f, 83850.0f, 83850.0f, 83820.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83820.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, -83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83880.0f, 83850.0f, 83880.0f, 83880.0f, 83850.0f, 83850.0f, 83880.0f, 83850.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83850.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83910.0f, 83880.0f, 83910.0f, 83880.0f, 83880.0f, 83910.0f, 83910.0f, -83880.0f, 83910.0f, 83880.0f, 83880.0f, 83880.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83880.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83940.0f, 83910.0f, 83940.0f, 83910.0f, 83910.0f, 83940.0f, 83910.0f, 83940.0f, 83910.0f, 83910.0f, 83910.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, -83970.0f, 83940.0f, 83940.0f, 83970.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83940.0f, 83970.0f, 83970.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83970.0f, 83940.0f, 83940.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 84000.0f, -83970.0f, 83970.0f, 83970.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 83970.0f, 83970.0f, 84000.0f, 84000.0f, 84000.0f, 83970.0f, 83970.0f, 84000.0f, 84000.0f, 83970.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84030.0f, 84000.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, -84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84060.0f, 84060.0f, 84030.0f, 84060.0f, 84030.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, -84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84060.0f, 84090.0f, 84090.0f, 84060.0f, 84090.0f, 84090.0f, 84060.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84120.0f, 84090.0f, 84090.0f, 84090.0f, 84120.0f, 84090.0f, 84090.0f, 84090.0f, 84120.0f, 84120.0f, 84090.0f, 84120.0f, -84090.0f, 84120.0f, 84120.0f, 84090.0f, 84120.0f, 84090.0f, 84120.0f, 84090.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84150.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84150.0f, 84150.0f, 84120.0f, 84120.0f, 84120.0f, 84150.0f, 84120.0f, 84120.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, -84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84180.0f, 84180.0f, 84150.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84150.0f, 84150.0f, 84180.0f, 84180.0f, 84150.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84150.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84210.0f, 84180.0f, 84210.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84210.0f, 84180.0f, -84180.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84180.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84180.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84240.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84240.0f, 84240.0f, 84210.0f, 84210.0f, 84240.0f, 84210.0f, 84210.0f, 84240.0f, 84210.0f, 84210.0f, 84240.0f, 84240.0f, 84210.0f, 84210.0f, 84240.0f, 84210.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, -84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84270.0f, 84240.0f, 84240.0f, 84270.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84270.0f, 84270.0f, 84270.0f, 84240.0f, 84270.0f, 84240.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84300.0f, 84300.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84300.0f, -84270.0f, 84300.0f, 84270.0f, 84270.0f, 84300.0f, 84270.0f, 84270.0f, 84270.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84330.0f, 84300.0f, 84330.0f, 84330.0f, 84300.0f, 84300.0f, 84330.0f, 84330.0f, 84300.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84300.0f, 84330.0f, 84330.0f, 84330.0f, -84300.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84360.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84360.0f, 84330.0f, 84360.0f, 84330.0f, 84360.0f, 84360.0f, 84360.0f, 84330.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84390.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, -84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84390.0f, 84360.0f, 84390.0f, 84360.0f, 84390.0f, 84360.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84360.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84420.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84420.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84420.0f, 84420.0f, 84390.0f, -84390.0f, 84390.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84390.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84450.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84450.0f, 84420.0f, 84450.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84450.0f, 84450.0f, 84420.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, -84450.0f, 84480.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84510.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84510.0f, 84480.0f, 84480.0f, 84480.0f, 84510.0f, 84510.0f, -84510.0f, 84510.0f, 84480.0f, 84480.0f, 84480.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84480.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84540.0f, 84510.0f, 84510.0f, 84510.0f, 84540.0f, 84510.0f, 84540.0f, 84510.0f, 84510.0f, 84510.0f, 84540.0f, 84510.0f, 84540.0f, 84540.0f, 84540.0f, 84510.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, -84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84570.0f, 84540.0f, 84540.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84540.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, -84570.0f, 84570.0f, 84570.0f, 84600.0f, 84570.0f, 84570.0f, 84600.0f, 84600.0f, 84570.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84570.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84630.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84630.0f, 84600.0f, 84630.0f, 84600.0f, 84600.0f, 84600.0f}; +const float ADA_SIMULATED_NOISY_PRESSURE[DATA_SIZE] = {85440.0f, 85440.0f, +85410.0f, 85440.0f, 85440.0f, 85410.0f, 85410.0f, 85410.0f, 85380.0f, +85380.0f, 85350.0f, 85350.0f, 85320.0f, 85290.0f, 85260.0f, 85230.0f, +85200.0f, 85170.0f, 85140.0f, 85110.0f, 85050.0f, 85020.0f, 84960.0f, +84930.0f, 84870.0f, 84810.0f, 84750.0f, 84690.0f, 84630.0f, 84570.0f, +84510.0f, 84450.0f, 84390.0f, 84300.0f, 84240.0f, 84150.0f, 84090.0f, +84030.0f, 83940.0f, 83850.0f, 83790.0f, 83700.0f, 83610.0f, 83520.0f, +83460.0f, 83370.0f, 83280.0f, 83190.0f, 83100.0f, 83010.0f, 82920.0f, +82860.0f, 82740.0f, 82650.0f, 82560.0f, 82470.0f, 82380.0f, 82290.0f, +82200.0f, 82110.0f, 82020.0f, 81930.0f, 81840.0f, 81750.0f, 81660.0f, +81570.0f, 81510.0f, 81390.0f, 81300.0f, 81240.0f, 81120.0f, 81060.0f, +80970.0f, 80880.0f, 80790.0f, 80700.0f, 80640.0f, 80550.0f, 80460.0f, +80370.0f, 80280.0f, 80220.0f, 80130.0f, 80040.0f, 79980.0f, 79890.0f, +79800.0f, 79710.0f, 79650.0f, 79560.0f, 79500.0f, 79410.0f, 79320.0f, +79260.0f, 79170.0f, 79110.0f, 79020.0f, 78930.0f, 78870.0f, 78810.0f, +78720.0f, 78630.0f, 78570.0f, 78510.0f, 78420.0f, 78360.0f, 78300.0f, +78210.0f, 78120.0f, 78060.0f, 78000.0f, 77940.0f, 77850.0f, 77790.0f, +77700.0f, 77640.0f, 77580.0f, 77490.0f, 77430.0f, 77370.0f, 77310.0f, +77250.0f, 77160.0f, 77100.0f, 77040.0f, 76950.0f, 76920.0f, 76830.0f, +76770.0f, 76710.0f, 76650.0f, 76590.0f, 76530.0f, 76440.0f, 76380.0f, +76320.0f, 76260.0f, 76200.0f, 76140.0f, 76080.0f, 76020.0f, 75960.0f, +75900.0f, 75840.0f, 75780.0f, 75720.0f, 75660.0f, 75600.0f, 75540.0f, +75480.0f, 75420.0f, 75360.0f, 75330.0f, 75270.0f, 75210.0f, 75150.0f, +75090.0f, 75030.0f, 74970.0f, 74910.0f, 74880.0f, 74820.0f, 74760.0f, +74700.0f, 74670.0f, 74610.0f, 74550.0f, 74490.0f, 74460.0f, 74400.0f, +74340.0f, 74280.0f, 74220.0f, 74190.0f, 74130.0f, 74070.0f, 74040.0f, +73980.0f, 73920.0f, 73890.0f, 73830.0f, 73800.0f, 73740.0f, 73710.0f, +73650.0f, 73590.0f, 73560.0f, 73500.0f, 73470.0f, 73410.0f, 73350.0f, +73320.0f, 73260.0f, 73230.0f, 73200.0f, 73140.0f, 73110.0f, 73050.0f, +73020.0f, 72990.0f, 72930.0f, 72900.0f, 72840.0f, 72810.0f, 72780.0f, +72720.0f, 72690.0f, 72630.0f, 72600.0f, 72570.0f, 72510.0f, 72480.0f, +72420.0f, 72390.0f, 72360.0f, 72300.0f, 72270.0f, 72240.0f, 72210.0f, +72180.0f, 72150.0f, 72090.0f, 72060.0f, 72030.0f, 71970.0f, 71940.0f, +71910.0f, 71880.0f, 71850.0f, 71820.0f, 71790.0f, 71730.0f, 71700.0f, +71670.0f, 71640.0f, 71610.0f, 71550.0f, 71550.0f, 71490.0f, 71460.0f, +71430.0f, 71400.0f, 71370.0f, 71340.0f, 71310.0f, 71280.0f, 71250.0f, +71220.0f, 71190.0f, 71160.0f, 71130.0f, 71100.0f, 71070.0f, 71010.0f, +71010.0f, 70980.0f, 70950.0f, 70920.0f, 70890.0f, 70860.0f, 70830.0f, +70800.0f, 70770.0f, 70740.0f, 70740.0f, 70710.0f, 70680.0f, 70620.0f, +70620.0f, 70590.0f, 70590.0f, 70560.0f, 70530.0f, 70500.0f, 70500.0f, +70440.0f, 70410.0f, 70410.0f, 70380.0f, 70380.0f, 70350.0f, 70320.0f, +70290.0f, 70290.0f, 70260.0f, 70230.0f, 70200.0f, 70200.0f, 70140.0f, +70170.0f, 70140.0f, 70110.0f, 70080.0f, 70080.0f, 70050.0f, 70020.0f, +69990.0f, 69990.0f, 69960.0f, 69960.0f, 69930.0f, 69930.0f, 69900.0f, +69870.0f, 69870.0f, 69840.0f, 69840.0f, 69810.0f, 69810.0f, 69810.0f, +69780.0f, 69750.0f, 69750.0f, 69720.0f, 69720.0f, 69720.0f, 69690.0f, +69660.0f, 69660.0f, 69630.0f, 69630.0f, 69600.0f, 69600.0f, 69600.0f, +69570.0f, 69570.0f, 69570.0f, 69540.0f, 69510.0f, 69510.0f, 69510.0f, +69480.0f, 69480.0f, 69480.0f, 69480.0f, 69450.0f, 69450.0f, 69420.0f, +69420.0f, 69420.0f, 69390.0f, 69420.0f, 69390.0f, 69390.0f, 69390.0f, +69360.0f, 69360.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, +69300.0f, 69300.0f, 69300.0f, 69300.0f, 69270.0f, 69270.0f, 69270.0f, +69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69240.0f, 69240.0f, +69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, +69210.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69210.0f, 69210.0f, +69210.0f, 69210.0f, 69210.0f, 69210.0f, 69240.0f, 69210.0f, 69210.0f, +69210.0f, 69210.0f, 69210.0f, 69210.0f, 69240.0f, 69240.0f, 69240.0f, +69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, 69240.0f, +69240.0f, 69240.0f, 69240.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, +69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69270.0f, 69300.0f, +69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, 69300.0f, +69300.0f, 69300.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, 69330.0f, +69330.0f, 69360.0f, 69360.0f, 69360.0f, 69360.0f, 69390.0f, 69360.0f, +69360.0f, 69390.0f, 69390.0f, 69390.0f, 69390.0f, 69390.0f, 69390.0f, +69420.0f, 69420.0f, 69420.0f, 69420.0f, 69420.0f, 69420.0f, 69450.0f, +69450.0f, 69450.0f, 69450.0f, 69450.0f, 69480.0f, 69480.0f, 69450.0f, +69480.0f, 69480.0f, 69480.0f, 69480.0f, 69480.0f, 69510.0f, 69510.0f, +69510.0f, 69540.0f, 69510.0f, 69510.0f, 69540.0f, 69510.0f, 69540.0f, +69540.0f, 69540.0f, 69540.0f, 69540.0f, 69540.0f, 69570.0f, 69570.0f, +69570.0f, 69570.0f, 69570.0f, 69600.0f, 69600.0f, 69600.0f, 69600.0f, +69600.0f, 69630.0f, 69600.0f, 69630.0f, 69630.0f, 69630.0f, 69660.0f, +69630.0f, 69630.0f, 69630.0f, 69660.0f, 69660.0f, 69660.0f, 69660.0f, +69690.0f, 69660.0f, 69690.0f, 69690.0f, 69690.0f, 69690.0f, 69690.0f, +69690.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, 69720.0f, +69720.0f, 69720.0f, 69750.0f, 69750.0f, 69750.0f, 69780.0f, 69780.0f, +69750.0f, 69780.0f, 69780.0f, 69780.0f, 69780.0f, 69810.0f, 69810.0f, +69810.0f, 69810.0f, 69810.0f, 69840.0f, 69810.0f, 69840.0f, 69840.0f, +69840.0f, 69840.0f, 69840.0f, 69840.0f, 69840.0f, 69870.0f, 69870.0f, +69870.0f, 69900.0f, 69870.0f, 69900.0f, 69900.0f, 69900.0f, 69900.0f, +69900.0f, 69900.0f, 69900.0f, 69930.0f, 69930.0f, 69930.0f, 69930.0f, +69930.0f, 69930.0f, 69930.0f, 69960.0f, 69960.0f, 69960.0f, 69960.0f, +69990.0f, 69990.0f, 69990.0f, 69990.0f, 69990.0f, 69990.0f, 70020.0f, +70020.0f, 70020.0f, 70020.0f, 70020.0f, 70020.0f, 70020.0f, 70050.0f, +70020.0f, 70050.0f, 70050.0f, 70050.0f, 70050.0f, 70050.0f, 70050.0f, +70080.0f, 70080.0f, 70080.0f, 70080.0f, 70080.0f, 70110.0f, 70110.0f, +70110.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70110.0f, 70140.0f, +70140.0f, 70140.0f, 70170.0f, 70140.0f, 70140.0f, 70140.0f, 70200.0f, +70200.0f, 70170.0f, 70200.0f, 70200.0f, 70200.0f, 70200.0f, 70200.0f, +70200.0f, 70200.0f, 70230.0f, 70230.0f, 70200.0f, 70230.0f, 70230.0f, +70230.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, 70260.0f, +70260.0f, 70260.0f, 70290.0f, 70290.0f, 70290.0f, 70290.0f, 70290.0f, +70320.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, 70320.0f, +70350.0f, 70350.0f, 70350.0f, 70350.0f, 70350.0f, 70380.0f, 70380.0f, +70350.0f, 70380.0f, 70380.0f, 70380.0f, 70380.0f, 70410.0f, 70410.0f, +70410.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70410.0f, 70440.0f, +70440.0f, 70440.0f, 70440.0f, 70440.0f, 70470.0f, 70440.0f, 70470.0f, +70470.0f, 70470.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, 70500.0f, +70500.0f, 70500.0f, 70500.0f, 70530.0f, 70530.0f, 70530.0f, 70530.0f, +70530.0f, 70560.0f, 70560.0f, 70530.0f, 70560.0f, 70560.0f, 70590.0f, +70560.0f, 70590.0f, 70590.0f, 70590.0f, 70590.0f, 70590.0f, 70590.0f, +70620.0f, 70590.0f, 70620.0f, 70620.0f, 70620.0f, 70620.0f, 70620.0f, +70620.0f, 70650.0f, 70650.0f, 70650.0f, 70650.0f, 70650.0f, 70680.0f, +70650.0f, 70680.0f, 70680.0f, 70680.0f, 70680.0f, 70710.0f, 70680.0f, +70710.0f, 70710.0f, 70740.0f, 70710.0f, 70710.0f, 70710.0f, 70740.0f, +70740.0f, 70740.0f, 70740.0f, 70740.0f, 70770.0f, 70770.0f, 70770.0f, +70800.0f, 70770.0f, 70770.0f, 70770.0f, 70800.0f, 70800.0f, 70800.0f, +70800.0f, 70800.0f, 70800.0f, 70830.0f, 70830.0f, 70830.0f, 70830.0f, +70830.0f, 70830.0f, 70860.0f, 70860.0f, 70860.0f, 70860.0f, 70860.0f, +70860.0f, 70890.0f, 70890.0f, 70860.0f, 70890.0f, 70920.0f, 70890.0f, +70890.0f, 70890.0f, 70920.0f, 70920.0f, 70920.0f, 70950.0f, 70920.0f, +70920.0f, 70920.0f, 70950.0f, 70950.0f, 70950.0f, 70950.0f, 70950.0f, +70980.0f, 70980.0f, 70980.0f, 70980.0f, 70980.0f, 71010.0f, 71010.0f, +71010.0f, 71010.0f, 71010.0f, 71010.0f, 71010.0f, 71040.0f, 71040.0f, +71040.0f, 71040.0f, 71070.0f, 71040.0f, 71070.0f, 71070.0f, 71070.0f, +71070.0f, 71100.0f, 71070.0f, 71100.0f, 71100.0f, 71100.0f, 71100.0f, +71100.0f, 71100.0f, 71100.0f, 71130.0f, 71130.0f, 71130.0f, 71130.0f, +71130.0f, 71130.0f, 71130.0f, 71160.0f, 71160.0f, 71160.0f, 71160.0f, +71160.0f, 71190.0f, 71160.0f, 71190.0f, 71190.0f, 71220.0f, 71190.0f, +71220.0f, 71220.0f, 71220.0f, 71220.0f, 71250.0f, 71220.0f, 71220.0f, +71220.0f, 71220.0f, 71250.0f, 71250.0f, 71250.0f, 71280.0f, 71250.0f, +71280.0f, 71280.0f, 71280.0f, 71280.0f, 71310.0f, 71310.0f, 71310.0f, +71310.0f, 71310.0f, 71310.0f, 71310.0f, 71340.0f, 71340.0f, 71310.0f, +71340.0f, 71340.0f, 71340.0f, 71340.0f, 71370.0f, 71340.0f, 71370.0f, +71370.0f, 71370.0f, 71370.0f, 71370.0f, 71400.0f, 71400.0f, 71400.0f, +71400.0f, 71400.0f, 71430.0f, 71430.0f, 71430.0f, 71430.0f, 71430.0f, +71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, 71460.0f, +71460.0f, 71490.0f, 71490.0f, 71490.0f, 71520.0f, 71490.0f, 71490.0f, +71490.0f, 71520.0f, 71520.0f, 71520.0f, 71520.0f, 71520.0f, 71550.0f, +71550.0f, 71550.0f, 71550.0f, 71550.0f, 71580.0f, 71580.0f, 71580.0f, +71580.0f, 71580.0f, 71580.0f, 71580.0f, 71580.0f, 71610.0f, 71610.0f, +71610.0f, 71610.0f, 71610.0f, 71610.0f, 71640.0f, 71640.0f, 71640.0f, +71640.0f, 71640.0f, 71640.0f, 71640.0f, 71670.0f, 71670.0f, 71670.0f, +71670.0f, 71670.0f, 71700.0f, 71700.0f, 71700.0f, 71700.0f, 71700.0f, +71730.0f, 71700.0f, 71730.0f, 71730.0f, 71730.0f, 71730.0f, 71730.0f, +71760.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, 71760.0f, +71790.0f, 71760.0f, 71790.0f, 71790.0f, 71790.0f, 71790.0f, 71790.0f, +71820.0f, 71820.0f, 71820.0f, 71820.0f, 71820.0f, 71850.0f, 71820.0f, +71850.0f, 71850.0f, 71850.0f, 71850.0f, 71850.0f, 71850.0f, 71880.0f, +71880.0f, 71880.0f, 71880.0f, 71880.0f, 71910.0f, 71910.0f, 71910.0f, +71910.0f, 71940.0f, 71910.0f, 71910.0f, 71910.0f, 71940.0f, 71940.0f, +71940.0f, 71940.0f, 71970.0f, 71970.0f, 71970.0f, 71970.0f, 71970.0f, +71970.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, 72000.0f, +72000.0f, 72030.0f, 72030.0f, 72030.0f, 72030.0f, 72030.0f, 72060.0f, +72060.0f, 72060.0f, 72030.0f, 72060.0f, 72060.0f, 72090.0f, 72090.0f, +72090.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72090.0f, 72120.0f, +72120.0f, 72120.0f, 72120.0f, 72120.0f, 72120.0f, 72150.0f, 72120.0f, +72150.0f, 72150.0f, 72150.0f, 72150.0f, 72180.0f, 72180.0f, 72180.0f, +72180.0f, 72180.0f, 72180.0f, 72210.0f, 72210.0f, 72210.0f, 72210.0f, +72210.0f, 72210.0f, 72210.0f, 72240.0f, 72240.0f, 72270.0f, 72240.0f, +72240.0f, 72240.0f, 72240.0f, 72270.0f, 72270.0f, 72300.0f, 72270.0f, +72270.0f, 72270.0f, 72270.0f, 72300.0f, 72300.0f, 72330.0f, 72300.0f, +72300.0f, 72330.0f, 72330.0f, 72330.0f, 72330.0f, 72330.0f, 72330.0f, +72360.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, 72360.0f, +72390.0f, 72390.0f, 72390.0f, 72390.0f, 72390.0f, 72420.0f, 72390.0f, +72420.0f, 72420.0f, 72420.0f, 72420.0f, 72420.0f, 72450.0f, 72420.0f, +72450.0f, 72450.0f, 72450.0f, 72450.0f, 72480.0f, 72480.0f, 72510.0f, +72450.0f, 72480.0f, 72480.0f, 72510.0f, 72510.0f, 72510.0f, 72510.0f, +72510.0f, 72540.0f, 72510.0f, 72510.0f, 72540.0f, 72540.0f, 72540.0f, +72540.0f, 72540.0f, 72570.0f, 72570.0f, 72570.0f, 72570.0f, 72570.0f, +72600.0f, 72600.0f, 72600.0f, 72600.0f, 72600.0f, 72630.0f, 72630.0f, +72630.0f, 72630.0f, 72630.0f, 72630.0f, 72630.0f, 72660.0f, 72630.0f, +72660.0f, 72630.0f, 72660.0f, 72660.0f, 72660.0f, 72660.0f, 72690.0f, +72690.0f, 72690.0f, 72690.0f, 72720.0f, 72690.0f, 72720.0f, 72720.0f, +72720.0f, 72720.0f, 72750.0f, 72720.0f, 72720.0f, 72720.0f, 72750.0f, +72750.0f, 72750.0f, 72780.0f, 72750.0f, 72780.0f, 72780.0f, 72780.0f, +72780.0f, 72780.0f, 72780.0f, 72780.0f, 72810.0f, 72810.0f, 72810.0f, +72810.0f, 72810.0f, 72810.0f, 72840.0f, 72840.0f, 72840.0f, 72840.0f, +72870.0f, 72840.0f, 72870.0f, 72870.0f, 72870.0f, 72870.0f, 72900.0f, +72900.0f, 72900.0f, 72900.0f, 72900.0f, 72930.0f, 72900.0f, 72930.0f, +72930.0f, 72930.0f, 72930.0f, 72930.0f, 72960.0f, 72930.0f, 72960.0f, +72960.0f, 72960.0f, 72960.0f, 72960.0f, 72960.0f, 72990.0f, 72990.0f, +72990.0f, 72990.0f, 72990.0f, 72990.0f, 72990.0f, 73020.0f, 73020.0f, +73020.0f, 73050.0f, 73050.0f, 73050.0f, 73050.0f, 73050.0f, 73050.0f, +73080.0f, 73080.0f, 73050.0f, 73080.0f, 73080.0f, 73080.0f, 73080.0f, +73110.0f, 73080.0f, 73110.0f, 73110.0f, 73110.0f, 73110.0f, 73140.0f, +73140.0f, 73110.0f, 73140.0f, 73140.0f, 73140.0f, 73140.0f, 73140.0f, +73170.0f, 73170.0f, 73170.0f, 73170.0f, 73170.0f, 73200.0f, 73200.0f, +73200.0f, 73200.0f, 73200.0f, 73200.0f, 73230.0f, 73230.0f, 73200.0f, +73230.0f, 73230.0f, 73230.0f, 73230.0f, 73230.0f, 73260.0f, 73260.0f, +73260.0f, 73260.0f, 73290.0f, 73260.0f, 73290.0f, 73260.0f, 73290.0f, +73290.0f, 73320.0f, 73290.0f, 73290.0f, 73320.0f, 73320.0f, 73320.0f, +73320.0f, 73320.0f, 73350.0f, 73350.0f, 73350.0f, 73350.0f, 73350.0f, +73380.0f, 73380.0f, 73380.0f, 73380.0f, 73380.0f, 73380.0f, 73410.0f, +73380.0f, 73410.0f, 73410.0f, 73410.0f, 73410.0f, 73410.0f, 73440.0f, +73440.0f, 73440.0f, 73440.0f, 73440.0f, 73470.0f, 73440.0f, 73470.0f, +73440.0f, 73470.0f, 73470.0f, 73470.0f, 73470.0f, 73500.0f, 73500.0f, +73500.0f, 73500.0f, 73530.0f, 73530.0f, 73530.0f, 73530.0f, 73530.0f, +73530.0f, 73530.0f, 73530.0f, 73560.0f, 73560.0f, 73560.0f, 73560.0f, +73560.0f, 73560.0f, 73590.0f, 73590.0f, 73590.0f, 73590.0f, 73590.0f, +73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, 73620.0f, +73620.0f, 73620.0f, 73650.0f, 73650.0f, 73680.0f, 73680.0f, 73650.0f, +73650.0f, 73680.0f, 73680.0f, 73680.0f, 73680.0f, 73680.0f, 73710.0f, +73710.0f, 73710.0f, 73710.0f, 73710.0f, 73710.0f, 73740.0f, 73740.0f, +73740.0f, 73740.0f, 73770.0f, 73740.0f, 73740.0f, 73770.0f, 73770.0f, +73770.0f, 73770.0f, 73770.0f, 73770.0f, 73800.0f, 73800.0f, 73830.0f, +73800.0f, 73800.0f, 73830.0f, 73830.0f, 73830.0f, 73830.0f, 73830.0f, +73860.0f, 73860.0f, 73860.0f, 73830.0f, 73860.0f, 73860.0f, 73860.0f, +73860.0f, 73890.0f, 73890.0f, 73890.0f, 73890.0f, 73890.0f, 73920.0f, +73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, 73920.0f, +73950.0f, 73950.0f, 73950.0f, 73950.0f, 73950.0f, 73950.0f, 73980.0f, +73980.0f, 73980.0f, 73980.0f, 73980.0f, 74010.0f, 73980.0f, 74010.0f, +74010.0f, 74010.0f, 74010.0f, 74040.0f, 74010.0f, 74040.0f, 74040.0f, +74040.0f, 74040.0f, 74040.0f, 74040.0f, 74070.0f, 74070.0f, 74070.0f, +74070.0f, 74100.0f, 74070.0f, 74070.0f, 74100.0f, 74100.0f, 74100.0f, +74130.0f, 74130.0f, 74130.0f, 74100.0f, 74130.0f, 74130.0f, 74130.0f, +74130.0f, 74130.0f, 74160.0f, 74160.0f, 74160.0f, 74160.0f, 74160.0f, +74190.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, 74190.0f, +74220.0f, 74220.0f, 74220.0f, 74220.0f, 74220.0f, 74250.0f, 74220.0f, +74250.0f, 74250.0f, 74250.0f, 74280.0f, 74250.0f, 74280.0f, 74280.0f, +74280.0f, 74280.0f, 74280.0f, 74310.0f, 74280.0f, 74310.0f, 74310.0f, +74310.0f, 74310.0f, 74310.0f, 74340.0f, 74340.0f, 74340.0f, 74340.0f, +74340.0f, 74340.0f, 74370.0f, 74370.0f, 74370.0f, 74370.0f, 74370.0f, +74370.0f, 74400.0f, 74400.0f, 74400.0f, 74400.0f, 74430.0f, 74400.0f, +74430.0f, 74400.0f, 74430.0f, 74430.0f, 74430.0f, 74460.0f, 74430.0f, +74460.0f, 74460.0f, 74460.0f, 74460.0f, 74490.0f, 74460.0f, 74460.0f, +74490.0f, 74490.0f, 74490.0f, 74490.0f, 74520.0f, 74520.0f, 74520.0f, +74520.0f, 74520.0f, 74550.0f, 74520.0f, 74550.0f, 74550.0f, 74550.0f, +74550.0f, 74550.0f, 74550.0f, 74550.0f, 74580.0f, 74580.0f, 74580.0f, +74610.0f, 74580.0f, 74610.0f, 74610.0f, 74610.0f, 74610.0f, 74610.0f, +74610.0f, 74610.0f, 74640.0f, 74640.0f, 74640.0f, 74640.0f, 74640.0f, +74670.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, 74670.0f, +74700.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, 74700.0f, +74730.0f, 74730.0f, 74730.0f, 74730.0f, 74760.0f, 74760.0f, 74760.0f, +74760.0f, 74790.0f, 74760.0f, 74790.0f, 74790.0f, 74790.0f, 74790.0f, +74790.0f, 74790.0f, 74790.0f, 74790.0f, 74820.0f, 74820.0f, 74820.0f, +74820.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, 74850.0f, +74850.0f, 74850.0f, 74880.0f, 74880.0f, 74880.0f, 74880.0f, 74910.0f, +74880.0f, 74910.0f, 74910.0f, 74910.0f, 74910.0f, 74910.0f, 74940.0f, +74910.0f, 74940.0f, 74940.0f, 74940.0f, 74940.0f, 74940.0f, 74970.0f, +74970.0f, 74970.0f, 74970.0f, 74970.0f, 74970.0f, 75000.0f, 75000.0f, +75000.0f, 75000.0f, 75000.0f, 75030.0f, 75030.0f, 75030.0f, 75030.0f, +75030.0f, 75030.0f, 75030.0f, 75060.0f, 75060.0f, 75060.0f, 75060.0f, +75060.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, 75090.0f, +75090.0f, 75090.0f, 75120.0f, 75090.0f, 75120.0f, 75120.0f, 75120.0f, +75150.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, 75150.0f, +75180.0f, 75180.0f, 75180.0f, 75180.0f, 75180.0f, 75180.0f, 75210.0f, +75210.0f, 75210.0f, 75210.0f, 75210.0f, 75240.0f, 75210.0f, 75240.0f, +75240.0f, 75240.0f, 75240.0f, 75240.0f, 75270.0f, 75270.0f, 75240.0f, +75270.0f, 75270.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, +75300.0f, 75300.0f, 75300.0f, 75300.0f, 75300.0f, 75330.0f, 75360.0f, +75330.0f, 75330.0f, 75360.0f, 75360.0f, 75360.0f, 75360.0f, 75360.0f, +75390.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, 75390.0f, +75420.0f, 75420.0f, 75420.0f, 75450.0f, 75450.0f, 75420.0f, 75450.0f, +75450.0f, 75450.0f, 75450.0f, 75450.0f, 75480.0f, 75480.0f, 75480.0f, +75480.0f, 75480.0f, 75480.0f, 75510.0f, 75510.0f, 75510.0f, 75510.0f, +75510.0f, 75510.0f, 75510.0f, 75540.0f, 75540.0f, 75540.0f, 75540.0f, +75540.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, 75570.0f, +75570.0f, 75570.0f, 75600.0f, 75600.0f, 75600.0f, 75600.0f, 75600.0f, +75600.0f, 75600.0f, 75630.0f, 75630.0f, 75630.0f, 75630.0f, 75660.0f, +75660.0f, 75630.0f, 75660.0f, 75660.0f, 75690.0f, 75660.0f, 75660.0f, +75690.0f, 75690.0f, 75690.0f, 75690.0f, 75690.0f, 75720.0f, 75720.0f, +75720.0f, 75720.0f, 75750.0f, 75750.0f, 75750.0f, 75720.0f, 75750.0f, +75750.0f, 75780.0f, 75750.0f, 75780.0f, 75750.0f, 75780.0f, 75810.0f, +75780.0f, 75810.0f, 75810.0f, 75780.0f, 75810.0f, 75810.0f, 75810.0f, +75810.0f, 75810.0f, 75810.0f, 75840.0f, 75840.0f, 75840.0f, 75840.0f, +75870.0f, 75870.0f, 75870.0f, 75870.0f, 75870.0f, 75900.0f, 75870.0f, +75900.0f, 75900.0f, 75900.0f, 75930.0f, 75900.0f, 75900.0f, 75930.0f, +75930.0f, 75930.0f, 75930.0f, 75930.0f, 75930.0f, 75960.0f, 75960.0f, +75960.0f, 75960.0f, 75960.0f, 75960.0f, 75990.0f, 75990.0f, 75990.0f, +75990.0f, 76020.0f, 75990.0f, 76020.0f, 76020.0f, 76020.0f, 76020.0f, +76020.0f, 76050.0f, 76050.0f, 76020.0f, 76050.0f, 76050.0f, 76050.0f, +76050.0f, 76080.0f, 76080.0f, 76080.0f, 76080.0f, 76080.0f, 76080.0f, +76110.0f, 76110.0f, 76110.0f, 76110.0f, 76110.0f, 76110.0f, 76140.0f, +76140.0f, 76140.0f, 76140.0f, 76140.0f, 76140.0f, 76140.0f, 76170.0f, +76170.0f, 76170.0f, 76170.0f, 76170.0f, 76170.0f, 76200.0f, 76200.0f, +76200.0f, 76200.0f, 76200.0f, 76230.0f, 76230.0f, 76230.0f, 76230.0f, +76230.0f, 76230.0f, 76230.0f, 76230.0f, 76260.0f, 76260.0f, 76260.0f, +76260.0f, 76260.0f, 76290.0f, 76290.0f, 76290.0f, 76290.0f, 76320.0f, +76290.0f, 76320.0f, 76320.0f, 76290.0f, 76320.0f, 76320.0f, 76350.0f, +76350.0f, 76350.0f, 76350.0f, 76350.0f, 76350.0f, 76380.0f, 76380.0f, +76350.0f, 76380.0f, 76380.0f, 76380.0f, 76380.0f, 76410.0f, 76410.0f, +76410.0f, 76410.0f, 76410.0f, 76440.0f, 76440.0f, 76410.0f, 76440.0f, +76440.0f, 76440.0f, 76440.0f, 76440.0f, 76440.0f, 76470.0f, 76470.0f, +76470.0f, 76470.0f, 76470.0f, 76500.0f, 76500.0f, 76500.0f, 76500.0f, +76500.0f, 76530.0f, 76500.0f, 76530.0f, 76530.0f, 76530.0f, 76530.0f, +76530.0f, 76530.0f, 76560.0f, 76560.0f, 76560.0f, 76560.0f, 76590.0f, +76560.0f, 76590.0f, 76590.0f, 76590.0f, 76620.0f, 76620.0f, 76590.0f, +76590.0f, 76590.0f, 76620.0f, 76620.0f, 76650.0f, 76620.0f, 76620.0f, +76650.0f, 76650.0f, 76650.0f, 76650.0f, 76650.0f, 76680.0f, 76680.0f, +76680.0f, 76680.0f, 76680.0f, 76710.0f, 76680.0f, 76710.0f, 76710.0f, +76740.0f, 76710.0f, 76710.0f, 76740.0f, 76710.0f, 76740.0f, 76740.0f, +76770.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, 76770.0f, +76800.0f, 76800.0f, 76800.0f, 76800.0f, 76800.0f, 76800.0f, 76830.0f, +76830.0f, 76830.0f, 76830.0f, 76830.0f, 76830.0f, 76860.0f, 76860.0f, +76860.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76860.0f, 76890.0f, +76890.0f, 76890.0f, 76890.0f, 76920.0f, 76920.0f, 76920.0f, 76920.0f, +76920.0f, 76920.0f, 76920.0f, 76950.0f, 76950.0f, 76950.0f, 76950.0f, +76950.0f, 76980.0f, 76980.0f, 76980.0f, 76980.0f, 76980.0f, 76980.0f, +77010.0f, 76980.0f, 77010.0f, 77010.0f, 77010.0f, 77040.0f, 77010.0f, +77010.0f, 77040.0f, 77040.0f, 77040.0f, 77040.0f, 77040.0f, 77040.0f, +77070.0f, 77070.0f, 77070.0f, 77070.0f, 77100.0f, 77070.0f, 77100.0f, +77100.0f, 77130.0f, 77100.0f, 77100.0f, 77100.0f, 77100.0f, 77130.0f, +77130.0f, 77130.0f, 77130.0f, 77130.0f, 77160.0f, 77160.0f, 77160.0f, +77160.0f, 77160.0f, 77160.0f, 77160.0f, 77190.0f, 77190.0f, 77190.0f, +77190.0f, 77220.0f, 77220.0f, 77190.0f, 77220.0f, 77220.0f, 77250.0f, +77220.0f, 77250.0f, 77250.0f, 77250.0f, 77250.0f, 77250.0f, 77280.0f, +77280.0f, 77280.0f, 77280.0f, 77280.0f, 77280.0f, 77280.0f, 77310.0f, +77310.0f, 77310.0f, 77310.0f, 77310.0f, 77310.0f, 77310.0f, 77340.0f, +77370.0f, 77340.0f, 77340.0f, 77340.0f, 77370.0f, 77370.0f, 77370.0f, +77370.0f, 77370.0f, 77400.0f, 77400.0f, 77400.0f, 77400.0f, 77400.0f, +77400.0f, 77400.0f, 77430.0f, 77430.0f, 77430.0f, 77430.0f, 77430.0f, +77460.0f, 77460.0f, 77460.0f, 77460.0f, 77460.0f, 77460.0f, 77490.0f, +77490.0f, 77490.0f, 77490.0f, 77490.0f, 77490.0f, 77490.0f, 77520.0f, +77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, 77520.0f, +77550.0f, 77550.0f, 77550.0f, 77580.0f, 77580.0f, 77580.0f, 77580.0f, +77580.0f, 77610.0f, 77610.0f, 77610.0f, 77610.0f, 77610.0f, 77610.0f, +77640.0f, 77610.0f, 77610.0f, 77640.0f, 77640.0f, 77640.0f, 77640.0f, +77670.0f, 77670.0f, 77670.0f, 77670.0f, 77670.0f, 77670.0f, 77670.0f, +77670.0f, 77670.0f, 77700.0f, 77700.0f, 77700.0f, 77700.0f, 77700.0f, +77730.0f, 77730.0f, 77730.0f, 77730.0f, 77730.0f, 77730.0f, 77760.0f, +77760.0f, 77760.0f, 77760.0f, 77760.0f, 77760.0f, 77790.0f, 77790.0f, +77790.0f, 77790.0f, 77790.0f, 77790.0f, 77820.0f, 77820.0f, 77820.0f, +77820.0f, 77820.0f, 77820.0f, 77850.0f, 77850.0f, 77850.0f, 77850.0f, +77850.0f, 77880.0f, 77880.0f, 77850.0f, 77880.0f, 77880.0f, 77880.0f, +77910.0f, 77880.0f, 77910.0f, 77910.0f, 77910.0f, 77910.0f, 77940.0f, +77940.0f, 77940.0f, 77940.0f, 77940.0f, 77970.0f, 77970.0f, 77970.0f, +77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 77970.0f, 78000.0f, +78000.0f, 78000.0f, 78000.0f, 78030.0f, 78030.0f, 78000.0f, 78060.0f, +78030.0f, 78030.0f, 78030.0f, 78030.0f, 78030.0f, 78060.0f, 78060.0f, +78060.0f, 78060.0f, 78060.0f, 78090.0f, 78090.0f, 78090.0f, 78090.0f, +78090.0f, 78120.0f, 78120.0f, 78120.0f, 78120.0f, 78120.0f, 78150.0f, +78150.0f, 78150.0f, 78150.0f, 78150.0f, 78150.0f, 78180.0f, 78180.0f, +78180.0f, 78180.0f, 78180.0f, 78180.0f, 78210.0f, 78210.0f, 78210.0f, +78210.0f, 78210.0f, 78210.0f, 78210.0f, 78240.0f, 78240.0f, 78240.0f, +78240.0f, 78240.0f, 78270.0f, 78270.0f, 78270.0f, 78270.0f, 78270.0f, +78300.0f, 78300.0f, 78270.0f, 78300.0f, 78300.0f, 78300.0f, 78300.0f, +78300.0f, 78300.0f, 78300.0f, 78330.0f, 78330.0f, 78330.0f, 78330.0f, +78360.0f, 78330.0f, 78360.0f, 78360.0f, 78360.0f, 78360.0f, 78390.0f, +78390.0f, 78390.0f, 78390.0f, 78390.0f, 78390.0f, 78420.0f, 78390.0f, +78420.0f, 78420.0f, 78420.0f, 78450.0f, 78420.0f, 78450.0f, 78450.0f, +78450.0f, 78450.0f, 78450.0f, 78450.0f, 78450.0f, 78480.0f, 78480.0f, +78480.0f, 78480.0f, 78480.0f, 78510.0f, 78510.0f, 78510.0f, 78510.0f, +78510.0f, 78510.0f, 78510.0f, 78540.0f, 78540.0f, 78570.0f, 78540.0f, +78570.0f, 78570.0f, 78570.0f, 78570.0f, 78570.0f, 78600.0f, 78570.0f, +78600.0f, 78600.0f, 78600.0f, 78600.0f, 78600.0f, 78600.0f, 78630.0f, +78630.0f, 78630.0f, 78630.0f, 78630.0f, 78630.0f, 78660.0f, 78660.0f, +78660.0f, 78660.0f, 78660.0f, 78660.0f, 78660.0f, 78690.0f, 78690.0f, +78690.0f, 78690.0f, 78720.0f, 78720.0f, 78690.0f, 78720.0f, 78720.0f, +78720.0f, 78720.0f, 78750.0f, 78720.0f, 78750.0f, 78750.0f, 78750.0f, +78750.0f, 78780.0f, 78780.0f, 78780.0f, 78780.0f, 78780.0f, 78780.0f, +78810.0f, 78810.0f, 78810.0f, 78810.0f, 78810.0f, 78840.0f, 78810.0f, +78840.0f, 78840.0f, 78840.0f, 78840.0f, 78870.0f, 78870.0f, 78840.0f, +78870.0f, 78870.0f, 78870.0f, 78900.0f, 78900.0f, 78900.0f, 78900.0f, +78900.0f, 78900.0f, 78930.0f, 78900.0f, 78930.0f, 78930.0f, 78930.0f, +78930.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, 78960.0f, +78960.0f, 78990.0f, 78990.0f, 78990.0f, 78990.0f, 78990.0f, 78990.0f, +79020.0f, 79020.0f, 79020.0f, 79020.0f, 79020.0f, 79020.0f, 79050.0f, +79050.0f, 79050.0f, 79050.0f, 79050.0f, 79050.0f, 79080.0f, 79080.0f, +79080.0f, 79080.0f, 79080.0f, 79110.0f, 79110.0f, 79110.0f, 79110.0f, +79110.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, 79140.0f, +79140.0f, 79140.0f, 79170.0f, 79170.0f, 79170.0f, 79170.0f, 79200.0f, +79200.0f, 79170.0f, 79170.0f, 79200.0f, 79200.0f, 79200.0f, 79200.0f, +79200.0f, 79200.0f, 79230.0f, 79230.0f, 79230.0f, 79260.0f, 79230.0f, +79260.0f, 79260.0f, 79260.0f, 79260.0f, 79260.0f, 79290.0f, 79290.0f, +79290.0f, 79290.0f, 79320.0f, 79320.0f, 79290.0f, 79320.0f, 79320.0f, +79320.0f, 79320.0f, 79320.0f, 79350.0f, 79350.0f, 79350.0f, 79350.0f, +79350.0f, 79350.0f, 79380.0f, 79350.0f, 79380.0f, 79380.0f, 79380.0f, +79380.0f, 79410.0f, 79410.0f, 79410.0f, 79410.0f, 79410.0f, 79410.0f, +79410.0f, 79440.0f, 79440.0f, 79440.0f, 79440.0f, 79440.0f, 79470.0f, +79470.0f, 79470.0f, 79470.0f, 79470.0f, 79470.0f, 79500.0f, 79500.0f, +79500.0f, 79500.0f, 79500.0f, 79500.0f, 79500.0f, 79530.0f, 79500.0f, +79530.0f, 79530.0f, 79530.0f, 79530.0f, 79560.0f, 79560.0f, 79560.0f, +79560.0f, 79560.0f, 79560.0f, 79590.0f, 79590.0f, 79590.0f, 79590.0f, +79590.0f, 79590.0f, 79620.0f, 79620.0f, 79620.0f, 79620.0f, 79620.0f, +79620.0f, 79650.0f, 79650.0f, 79650.0f, 79650.0f, 79680.0f, 79680.0f, +79680.0f, 79680.0f, 79680.0f, 79710.0f, 79680.0f, 79680.0f, 79710.0f, +79710.0f, 79710.0f, 79710.0f, 79740.0f, 79740.0f, 79740.0f, 79740.0f, +79740.0f, 79740.0f, 79740.0f, 79770.0f, 79770.0f, 79770.0f, 79770.0f, +79770.0f, 79770.0f, 79800.0f, 79800.0f, 79800.0f, 79800.0f, 79800.0f, +79800.0f, 79830.0f, 79830.0f, 79830.0f, 79830.0f, 79830.0f, 79860.0f, +79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, 79860.0f, +79890.0f, 79890.0f, 79890.0f, 79890.0f, 79890.0f, 79890.0f, 79920.0f, +79920.0f, 79920.0f, 79950.0f, 79920.0f, 79950.0f, 79950.0f, 79950.0f, +79950.0f, 79950.0f, 79980.0f, 79980.0f, 79950.0f, 79980.0f, 79980.0f, +79980.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, 80010.0f, +80010.0f, 80040.0f, 80040.0f, 80040.0f, 80040.0f, 80040.0f, 80040.0f, +80070.0f, 80070.0f, 80070.0f, 80070.0f, 80070.0f, 80100.0f, 80100.0f, +80100.0f, 80100.0f, 80100.0f, 80100.0f, 80130.0f, 80130.0f, 80130.0f, +80130.0f, 80130.0f, 80160.0f, 80130.0f, 80130.0f, 80130.0f, 80160.0f, +80160.0f, 80160.0f, 80190.0f, 80190.0f, 80190.0f, 80190.0f, 80190.0f, +80190.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, 80220.0f, +80220.0f, 80220.0f, 80250.0f, 80250.0f, 80250.0f, 80250.0f, 80280.0f, +80250.0f, 80280.0f, 80280.0f, 80280.0f, 80280.0f, 80280.0f, 80310.0f, +80310.0f, 80310.0f, 80310.0f, 80310.0f, 80340.0f, 80310.0f, 80340.0f, +80340.0f, 80340.0f, 80340.0f, 80370.0f, 80370.0f, 80370.0f, 80370.0f, +80400.0f, 80370.0f, 80370.0f, 80370.0f, 80400.0f, 80400.0f, 80400.0f, +80400.0f, 80430.0f, 80430.0f, 80430.0f, 80430.0f, 80430.0f, 80430.0f, +80430.0f, 80460.0f, 80460.0f, 80460.0f, 80460.0f, 80490.0f, 80490.0f, +80490.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80490.0f, 80520.0f, +80520.0f, 80520.0f, 80520.0f, 80520.0f, 80550.0f, 80550.0f, 80550.0f, +80550.0f, 80550.0f, 80580.0f, 80550.0f, 80580.0f, 80580.0f, 80580.0f, +80580.0f, 80580.0f, 80610.0f, 80610.0f, 80610.0f, 80610.0f, 80610.0f, +80610.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, 80640.0f, +80640.0f, 80670.0f, 80670.0f, 80670.0f, 80670.0f, 80670.0f, 80700.0f, +80700.0f, 80700.0f, 80700.0f, 80700.0f, 80700.0f, 80700.0f, 80730.0f, +80730.0f, 80730.0f, 80760.0f, 80730.0f, 80760.0f, 80760.0f, 80760.0f, +80760.0f, 80760.0f, 80760.0f, 80760.0f, 80790.0f, 80790.0f, 80790.0f, +80790.0f, 80790.0f, 80820.0f, 80820.0f, 80820.0f, 80820.0f, 80820.0f, +80820.0f, 80820.0f, 80850.0f, 80850.0f, 80850.0f, 80850.0f, 80880.0f, +80880.0f, 80850.0f, 80880.0f, 80880.0f, 80880.0f, 80880.0f, 80910.0f, +80910.0f, 80910.0f, 80910.0f, 80910.0f, 80910.0f, 80940.0f, 80940.0f, +80940.0f, 80940.0f, 80940.0f, 80940.0f, 80970.0f, 80970.0f, 80970.0f, +80970.0f, 80970.0f, 80970.0f, 81000.0f, 81000.0f, 81000.0f, 81000.0f, +81000.0f, 81000.0f, 81030.0f, 81030.0f, 81030.0f, 81030.0f, 81060.0f, +81060.0f, 81060.0f, 81060.0f, 81060.0f, 81060.0f, 81090.0f, 81060.0f, +81090.0f, 81090.0f, 81090.0f, 81090.0f, 81090.0f, 81120.0f, 81120.0f, +81120.0f, 81090.0f, 81120.0f, 81120.0f, 81120.0f, 81120.0f, 81150.0f, +81150.0f, 81150.0f, 81150.0f, 81180.0f, 81180.0f, 81180.0f, 81180.0f, +81180.0f, 81180.0f, 81180.0f, 81180.0f, 81210.0f, 81210.0f, 81210.0f, +81210.0f, 81240.0f, 81210.0f, 81240.0f, 81240.0f, 81240.0f, 81240.0f, +81240.0f, 81270.0f, 81270.0f, 81270.0f, 81270.0f, 81270.0f, 81300.0f, +81300.0f, 81300.0f, 81300.0f, 81300.0f, 81300.0f, 81300.0f, 81330.0f, +81330.0f, 81330.0f, 81330.0f, 81330.0f, 81360.0f, 81360.0f, 81360.0f, +81360.0f, 81360.0f, 81390.0f, 81360.0f, 81390.0f, 81390.0f, 81390.0f, +81390.0f, 81390.0f, 81420.0f, 81420.0f, 81420.0f, 81420.0f, 81420.0f, +81420.0f, 81450.0f, 81420.0f, 81450.0f, 81450.0f, 81450.0f, 81450.0f, +81480.0f, 81480.0f, 81480.0f, 81480.0f, 81510.0f, 81510.0f, 81510.0f, +81510.0f, 81510.0f, 81510.0f, 81510.0f, 81510.0f, 81540.0f, 81540.0f, +81540.0f, 81540.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, 81570.0f, +81570.0f, 81570.0f, 81570.0f, 81600.0f, 81600.0f, 81600.0f, 81600.0f, +81600.0f, 81630.0f, 81600.0f, 81630.0f, 81630.0f, 81630.0f, 81660.0f, +81630.0f, 81660.0f, 81660.0f, 81660.0f, 81660.0f, 81690.0f, 81690.0f, +81660.0f, 81690.0f, 81690.0f, 81690.0f, 81690.0f, 81720.0f, 81720.0f, +81720.0f, 81720.0f, 81720.0f, 81720.0f, 81750.0f, 81750.0f, 81750.0f, +81750.0f, 81750.0f, 81780.0f, 81780.0f, 81780.0f, 81780.0f, 81780.0f, +81780.0f, 81810.0f, 81810.0f, 81780.0f, 81810.0f, 81810.0f, 81810.0f, +81840.0f, 81810.0f, 81840.0f, 81840.0f, 81840.0f, 81840.0f, 81840.0f, +81870.0f, 81870.0f, 81870.0f, 81870.0f, 81870.0f, 81870.0f, 81900.0f, +81900.0f, 81900.0f, 81900.0f, 81900.0f, 81930.0f, 81930.0f, 81900.0f, +81930.0f, 81930.0f, 81930.0f, 81960.0f, 81960.0f, 81960.0f, 81960.0f, +81960.0f, 81960.0f, 81990.0f, 81960.0f, 81990.0f, 81990.0f, 81990.0f, +82020.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, 82020.0f, +82050.0f, 82020.0f, 82050.0f, 82050.0f, 82050.0f, 82050.0f, 82080.0f, +82050.0f, 82080.0f, 82080.0f, 82080.0f, 82110.0f, 82110.0f, 82110.0f, +82110.0f, 82110.0f, 82110.0f, 82140.0f, 82110.0f, 82140.0f, 82140.0f, +82140.0f, 82140.0f, 82140.0f, 82170.0f, 82140.0f, 82170.0f, 82200.0f, +82200.0f, 82200.0f, 82200.0f, 82200.0f, 82200.0f, 82200.0f, 82200.0f, +82200.0f, 82200.0f, 82230.0f, 82230.0f, 82230.0f, 82230.0f, 82230.0f, +82230.0f, 82260.0f, 82260.0f, 82260.0f, 82260.0f, 82260.0f, 82290.0f, +82290.0f, 82290.0f, 82290.0f, 82290.0f, 82290.0f, 82320.0f, 82320.0f, +82320.0f, 82320.0f, 82320.0f, 82350.0f, 82350.0f, 82350.0f, 82350.0f, +82350.0f, 82350.0f, 82350.0f, 82380.0f, 82380.0f, 82380.0f, 82380.0f, +82380.0f, 82380.0f, 82410.0f, 82410.0f, 82410.0f, 82410.0f, 82410.0f, +82410.0f, 82440.0f, 82440.0f, 82440.0f, 82440.0f, 82470.0f, 82470.0f, +82470.0f, 82440.0f, 82470.0f, 82500.0f, 82470.0f, 82500.0f, 82470.0f, +82500.0f, 82500.0f, 82500.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, +82530.0f, 82530.0f, 82530.0f, 82530.0f, 82530.0f, 82560.0f, 82560.0f, +82560.0f, 82560.0f, 82590.0f, 82590.0f, 82590.0f, 82590.0f, 82590.0f, +82590.0f, 82620.0f, 82620.0f, 82620.0f, 82620.0f, 82620.0f, 82650.0f, +82620.0f, 82650.0f, 82650.0f, 82650.0f, 82650.0f, 82650.0f, 82680.0f, +82650.0f, 82680.0f, 82680.0f, 82710.0f, 82710.0f, 82710.0f, 82710.0f, +82710.0f, 82710.0f, 82710.0f, 82710.0f, 82710.0f, 82740.0f, 82740.0f, +82740.0f, 82740.0f, 82740.0f, 82740.0f, 82770.0f, 82770.0f, 82770.0f, +82770.0f, 82800.0f, 82800.0f, 82800.0f, 82800.0f, 82800.0f, 82800.0f, +82830.0f, 82830.0f, 82830.0f, 82860.0f, 82830.0f, 82830.0f, 82860.0f, +82860.0f, 82860.0f, 82860.0f, 82860.0f, 82890.0f, 82860.0f, 82890.0f, +82890.0f, 82890.0f, 82890.0f, 82920.0f, 82890.0f, 82920.0f, 82920.0f, +82920.0f, 82950.0f, 82920.0f, 82950.0f, 82950.0f, 82950.0f, 82950.0f, +82950.0f, 82980.0f, 82980.0f, 82980.0f, 82980.0f, 82980.0f, 82980.0f, +83010.0f, 83010.0f, 83010.0f, 83010.0f, 83010.0f, 83010.0f, 83040.0f, +83040.0f, 83040.0f, 83040.0f, 83070.0f, 83040.0f, 83040.0f, 83070.0f, +83070.0f, 83070.0f, 83070.0f, 83070.0f, 83100.0f, 83100.0f, 83100.0f, +83100.0f, 83100.0f, 83100.0f, 83100.0f, 83130.0f, 83130.0f, 83130.0f, +83160.0f, 83130.0f, 83160.0f, 83160.0f, 83160.0f, 83190.0f, 83160.0f, +83160.0f, 83190.0f, 83190.0f, 83190.0f, 83190.0f, 83190.0f, 83220.0f, +83220.0f, 83220.0f, 83220.0f, 83220.0f, 83250.0f, 83250.0f, 83250.0f, +83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83250.0f, 83280.0f, +83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, +83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, +83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, +83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, 83280.0f, +83280.0f, 83280.0f, 83280.0f, 83310.0f, 83280.0f, 83310.0f, 83310.0f, +83280.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83280.0f, +83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, +83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, +83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, 83310.0f, +83310.0f, 83340.0f, 83310.0f, 83340.0f, 83310.0f, 83310.0f, 83340.0f, +83340.0f, 83340.0f, 83340.0f, 83310.0f, 83340.0f, 83340.0f, 83310.0f, +83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83310.0f, +83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, +83340.0f, 83340.0f, 83340.0f, 83370.0f, 83340.0f, 83340.0f, 83340.0f, +83340.0f, 83340.0f, 83340.0f, 83340.0f, 83340.0f, 83370.0f, 83340.0f, +83340.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, +83370.0f, 83370.0f, 83340.0f, 83340.0f, 83370.0f, 83370.0f, 83370.0f, +83370.0f, 83370.0f, 83370.0f, 83370.0f, 83400.0f, 83370.0f, 83370.0f, +83400.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83370.0f, 83400.0f, +83400.0f, 83370.0f, 83370.0f, 83400.0f, 83370.0f, 83370.0f, 83400.0f, +83400.0f, 83400.0f, 83400.0f, 83370.0f, 83400.0f, 83400.0f, 83400.0f, +83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, 83400.0f, +83400.0f, 83400.0f, 83400.0f, 83400.0f, 83370.0f, 83400.0f, 83400.0f, +83400.0f, 83400.0f, 83400.0f, 83400.0f, 83430.0f, 83400.0f, 83400.0f, +83400.0f, 83430.0f, 83400.0f, 83400.0f, 83430.0f, 83400.0f, 83400.0f, +83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, +83430.0f, 83430.0f, 83430.0f, 83430.0f, 83400.0f, 83430.0f, 83430.0f, +83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, 83430.0f, +83430.0f, 83430.0f, 83430.0f, 83460.0f, 83430.0f, 83430.0f, 83430.0f, +83430.0f, 83460.0f, 83430.0f, 83460.0f, 83460.0f, 83430.0f, 83430.0f, +83460.0f, 83430.0f, 83430.0f, 83460.0f, 83460.0f, 83430.0f, 83460.0f, +83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, +83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, +83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, 83460.0f, +83460.0f, 83460.0f, 83460.0f, 83490.0f, 83460.0f, 83460.0f, 83460.0f, +83460.0f, 83490.0f, 83490.0f, 83460.0f, 83490.0f, 83460.0f, 83460.0f, +83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, +83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, +83490.0f, 83490.0f, 83520.0f, 83490.0f, 83490.0f, 83490.0f, 83490.0f, +83520.0f, 83520.0f, 83520.0f, 83490.0f, 83520.0f, 83520.0f, 83490.0f, +83490.0f, 83490.0f, 83520.0f, 83520.0f, 83490.0f, 83520.0f, 83520.0f, +83520.0f, 83520.0f, 83520.0f, 83520.0f, 83490.0f, 83520.0f, 83490.0f, +83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, +83550.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, 83520.0f, +83520.0f, 83550.0f, 83550.0f, 83520.0f, 83520.0f, 83550.0f, 83520.0f, +83550.0f, 83550.0f, 83550.0f, 83550.0f, 83520.0f, 83550.0f, 83550.0f, +83520.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, +83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, +83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, +83550.0f, 83550.0f, 83550.0f, 83550.0f, 83550.0f, 83580.0f, 83580.0f, +83580.0f, 83550.0f, 83550.0f, 83550.0f, 83580.0f, 83580.0f, 83580.0f, +83580.0f, 83580.0f, 83550.0f, 83580.0f, 83550.0f, 83580.0f, 83550.0f, +83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, +83580.0f, 83610.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, +83580.0f, 83580.0f, 83580.0f, 83580.0f, 83580.0f, 83610.0f, 83610.0f, +83580.0f, 83580.0f, 83610.0f, 83580.0f, 83580.0f, 83610.0f, 83610.0f, +83580.0f, 83580.0f, 83610.0f, 83610.0f, 83580.0f, 83610.0f, 83610.0f, +83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, 83610.0f, +83610.0f, 83610.0f, 83610.0f, 83580.0f, 83610.0f, 83610.0f, 83610.0f, +83610.0f, 83610.0f, 83640.0f, 83640.0f, 83640.0f, 83610.0f, 83640.0f, +83610.0f, 83610.0f, 83640.0f, 83640.0f, 83640.0f, 83610.0f, 83640.0f, +83610.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83610.0f, +83640.0f, 83610.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, +83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, +83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83640.0f, 83670.0f, +83640.0f, 83670.0f, 83670.0f, 83670.0f, 83640.0f, 83670.0f, 83670.0f, +83640.0f, 83670.0f, 83670.0f, 83640.0f, 83670.0f, 83670.0f, 83670.0f, +83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, 83670.0f, +83670.0f, 83700.0f, 83670.0f, 83670.0f, 83700.0f, 83670.0f, 83670.0f, +83670.0f, 83670.0f, 83670.0f, 83670.0f, 83700.0f, 83670.0f, 83670.0f, +83700.0f, 83700.0f, 83670.0f, 83700.0f, 83700.0f, 83700.0f, 83670.0f, +83700.0f, 83700.0f, 83670.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, +83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, 83700.0f, +83700.0f, 83700.0f, 83700.0f, 83730.0f, 83700.0f, 83730.0f, 83700.0f, +83700.0f, 83730.0f, 83700.0f, 83700.0f, 83730.0f, 83700.0f, 83700.0f, +83700.0f, 83700.0f, 83730.0f, 83730.0f, 83700.0f, 83730.0f, 83730.0f, +83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, +83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, +83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83760.0f, 83730.0f, +83730.0f, 83730.0f, 83730.0f, 83730.0f, 83730.0f, 83760.0f, 83730.0f, +83730.0f, 83730.0f, 83730.0f, 83760.0f, 83760.0f, 83730.0f, 83760.0f, +83760.0f, 83730.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, +83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, +83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, 83760.0f, +83760.0f, 83760.0f, 83760.0f, 83760.0f, 83790.0f, 83760.0f, 83760.0f, +83760.0f, 83760.0f, 83760.0f, 83790.0f, 83790.0f, 83760.0f, 83760.0f, +83760.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, +83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, +83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, +83790.0f, 83790.0f, 83790.0f, 83790.0f, 83790.0f, 83820.0f, 83790.0f, +83820.0f, 83790.0f, 83790.0f, 83820.0f, 83790.0f, 83820.0f, 83820.0f, +83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, +83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, +83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83850.0f, 83820.0f, +83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, 83820.0f, +83850.0f, 83820.0f, 83820.0f, 83820.0f, 83850.0f, 83850.0f, 83820.0f, +83850.0f, 83850.0f, 83850.0f, 83850.0f, 83820.0f, 83850.0f, 83850.0f, +83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, +83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, 83850.0f, +83850.0f, 83850.0f, 83880.0f, 83850.0f, 83880.0f, 83880.0f, 83850.0f, +83850.0f, 83880.0f, 83850.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, +83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, +83880.0f, 83880.0f, 83880.0f, 83850.0f, 83880.0f, 83880.0f, 83880.0f, +83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, 83880.0f, +83880.0f, 83910.0f, 83880.0f, 83910.0f, 83880.0f, 83880.0f, 83910.0f, +83910.0f, 83880.0f, 83910.0f, 83880.0f, 83880.0f, 83880.0f, 83910.0f, +83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, +83880.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, +83910.0f, 83910.0f, 83910.0f, 83910.0f, 83910.0f, 83940.0f, 83910.0f, +83940.0f, 83910.0f, 83910.0f, 83940.0f, 83910.0f, 83940.0f, 83910.0f, +83910.0f, 83910.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, +83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, +83940.0f, 83940.0f, 83970.0f, 83940.0f, 83940.0f, 83970.0f, 83940.0f, +83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83940.0f, 83970.0f, +83970.0f, 83970.0f, 83970.0f, 83940.0f, 83970.0f, 83970.0f, 83940.0f, +83940.0f, 83940.0f, 83940.0f, 83970.0f, 83940.0f, 83940.0f, 83970.0f, +83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, +83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, +83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, 83970.0f, +83970.0f, 83970.0f, 84000.0f, 83970.0f, 83970.0f, 83970.0f, 84000.0f, +84000.0f, 84000.0f, 84000.0f, 83970.0f, 83970.0f, 84000.0f, 84000.0f, +84000.0f, 83970.0f, 83970.0f, 84000.0f, 84000.0f, 83970.0f, 84000.0f, +84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, +84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, +84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, 84000.0f, +84030.0f, 84000.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, +84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, +84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, +84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, +84030.0f, 84030.0f, 84030.0f, 84030.0f, 84030.0f, 84060.0f, 84060.0f, +84030.0f, 84060.0f, 84030.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, +84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, +84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, +84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, +84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, 84060.0f, +84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84060.0f, 84090.0f, +84090.0f, 84060.0f, 84090.0f, 84090.0f, 84060.0f, 84090.0f, 84090.0f, +84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, +84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, 84090.0f, +84090.0f, 84120.0f, 84090.0f, 84090.0f, 84090.0f, 84120.0f, 84090.0f, +84090.0f, 84090.0f, 84120.0f, 84120.0f, 84090.0f, 84120.0f, 84090.0f, +84120.0f, 84120.0f, 84090.0f, 84120.0f, 84090.0f, 84120.0f, 84090.0f, +84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, +84150.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, 84120.0f, +84120.0f, 84120.0f, 84120.0f, 84120.0f, 84150.0f, 84150.0f, 84120.0f, +84120.0f, 84120.0f, 84150.0f, 84120.0f, 84120.0f, 84150.0f, 84150.0f, +84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, +84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, +84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, +84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84150.0f, 84180.0f, +84180.0f, 84150.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84150.0f, +84150.0f, 84180.0f, 84180.0f, 84150.0f, 84180.0f, 84180.0f, 84180.0f, +84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84150.0f, +84180.0f, 84180.0f, 84180.0f, 84180.0f, 84210.0f, 84180.0f, 84210.0f, +84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84180.0f, 84210.0f, +84180.0f, 84180.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84180.0f, +84210.0f, 84210.0f, 84210.0f, 84210.0f, 84180.0f, 84210.0f, 84210.0f, +84210.0f, 84210.0f, 84210.0f, 84210.0f, 84240.0f, 84210.0f, 84210.0f, +84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, 84210.0f, +84210.0f, 84240.0f, 84240.0f, 84210.0f, 84210.0f, 84240.0f, 84210.0f, +84210.0f, 84240.0f, 84210.0f, 84210.0f, 84240.0f, 84240.0f, 84210.0f, +84210.0f, 84240.0f, 84210.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, +84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, +84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, 84240.0f, +84240.0f, 84240.0f, 84270.0f, 84240.0f, 84240.0f, 84270.0f, 84240.0f, +84240.0f, 84240.0f, 84240.0f, 84270.0f, 84270.0f, 84270.0f, 84240.0f, +84270.0f, 84240.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, +84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, 84270.0f, +84270.0f, 84270.0f, 84270.0f, 84300.0f, 84300.0f, 84270.0f, 84270.0f, +84270.0f, 84270.0f, 84300.0f, 84270.0f, 84300.0f, 84270.0f, 84270.0f, +84300.0f, 84270.0f, 84270.0f, 84270.0f, 84300.0f, 84300.0f, 84300.0f, +84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, +84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, +84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, 84300.0f, +84300.0f, 84330.0f, 84300.0f, 84330.0f, 84330.0f, 84300.0f, 84300.0f, +84330.0f, 84330.0f, 84300.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, +84300.0f, 84330.0f, 84330.0f, 84330.0f, 84300.0f, 84330.0f, 84330.0f, +84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, +84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, +84330.0f, 84330.0f, 84330.0f, 84360.0f, 84330.0f, 84330.0f, 84330.0f, +84330.0f, 84330.0f, 84330.0f, 84330.0f, 84330.0f, 84360.0f, 84330.0f, +84360.0f, 84330.0f, 84360.0f, 84360.0f, 84360.0f, 84330.0f, 84360.0f, +84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84390.0f, +84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, +84360.0f, 84360.0f, 84360.0f, 84360.0f, 84360.0f, 84390.0f, 84360.0f, +84390.0f, 84360.0f, 84390.0f, 84360.0f, 84390.0f, 84390.0f, 84390.0f, +84390.0f, 84360.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, +84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, +84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84420.0f, +84390.0f, 84390.0f, 84390.0f, 84390.0f, 84390.0f, 84420.0f, 84390.0f, +84390.0f, 84390.0f, 84390.0f, 84420.0f, 84420.0f, 84390.0f, 84390.0f, +84390.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84390.0f, +84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, +84450.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, 84420.0f, +84420.0f, 84420.0f, 84450.0f, 84420.0f, 84450.0f, 84420.0f, 84420.0f, +84420.0f, 84420.0f, 84420.0f, 84450.0f, 84450.0f, 84420.0f, 84450.0f, +84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, +84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, +84450.0f, 84480.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, +84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, 84450.0f, +84450.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, 84450.0f, 84480.0f, +84480.0f, 84480.0f, 84480.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, +84480.0f, 84450.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, 84480.0f, +84480.0f, 84480.0f, 84480.0f, 84510.0f, 84480.0f, 84480.0f, 84480.0f, +84480.0f, 84480.0f, 84510.0f, 84480.0f, 84480.0f, 84480.0f, 84510.0f, +84510.0f, 84510.0f, 84510.0f, 84480.0f, 84480.0f, 84480.0f, 84510.0f, +84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84480.0f, 84510.0f, +84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, +84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, 84510.0f, +84510.0f, 84510.0f, 84510.0f, 84540.0f, 84510.0f, 84510.0f, 84510.0f, +84540.0f, 84510.0f, 84540.0f, 84510.0f, 84510.0f, 84510.0f, 84540.0f, +84510.0f, 84540.0f, 84540.0f, 84540.0f, 84510.0f, 84540.0f, 84540.0f, +84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, +84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, 84540.0f, +84540.0f, 84540.0f, 84540.0f, 84540.0f, 84570.0f, 84540.0f, 84540.0f, +84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84540.0f, 84570.0f, +84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, +84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, +84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, +84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84570.0f, 84600.0f, +84570.0f, 84570.0f, 84600.0f, 84600.0f, 84570.0f, 84600.0f, 84600.0f, +84600.0f, 84600.0f, 84600.0f, 84600.0f, 84570.0f, 84600.0f, 84600.0f, +84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84630.0f, 84600.0f, +84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, 84600.0f, +84600.0f, 84600.0f, 84630.0f, 84600.0f, 84630.0f, 84600.0f, 84600.0f, +84600.0f}; */ \ No newline at end of file diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-data.h b/src/tests/catch/ada/ada_kalman_p/test-ada-data.h index 69d8d8a238651aa0f375ecd68e1fa676af675af1..5f83f99198f60227199a6b6c342f70eb559e6589 100644 --- a/src/tests/catch/ada/ada_kalman_p/test-ada-data.h +++ b/src/tests/catch/ada/ada_kalman_p/test-ada-data.h @@ -1,20 +1,19 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -24,6 +23,6 @@ #pragma once static const unsigned DATA_SIZE = 4840; -extern const float SIMULATED_PRESSURE[DATA_SIZE]; -extern const float SIMULATED_PRESSURE_SPEED[DATA_SIZE]; -// extern const float SIMULATED_NOISY_PRESSURE[DATA_SIZE]; \ No newline at end of file +extern const float ADA_SIMULATED_PRESSURE[DATA_SIZE]; +extern const float ADA_SIMULATED_PRESSURE_SPEED[DATA_SIZE]; +// extern const float ADA_SIMULATED_NOISY_PRESSURE[DATA_SIZE]; \ No newline at end of file diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp index f09110dc2f4dbe34ae1f4b163d51fd0f976380a5..a2f7897ffd075988fb3b13fd064a829b3ba321f5 100644 --- a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp +++ b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli, Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,24 +13,31 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#define private public -#define protected public - #ifdef STANDALONE_CATCH1_TEST #include "catch/catch-tests-entry.cpp" #endif +#define EIGEN_NO_MALLOC + + +#include <utils/testutils/catch.hpp> + +#define private public +#define protected public + +#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> +#include <DeathStack.h> #include <Common.h> -#include <DeathStack/ADA/ADAController.h> -#include <DeathStack/events/Events.h> #include <events/EventBroker.h> +#include <events/Events.h> #include <events/FSM.h> #include <events/utils/EventCounter.h> #include <algorithm> @@ -39,132 +45,214 @@ #include <iostream> #include <random> #include <sstream> -#include <utils/testutils/catch.hpp> + #include "test-ada-data.h" using namespace DeathStackBoard; +using namespace ADAConfigs; -constexpr float NOISE_STD_DEV = 5; // Noise varaince +constexpr float NOISE_STD_DEV = 5; // Noise variance constexpr float LSB = 28; constexpr unsigned int SHADOW_MODE_END_INDEX = 30; -ADAController *ada_controller; -unsigned seed = 1234567; // Seed for noise generation +constexpr unsigned int APOGEE_SAMPLE = 382; -float addNoise(float sample); // Function to add noise -float quantization(float sample); -void checkState(unsigned int i, KalmanState state); +// Mock sensors for testing purposes +class MockPressureSensor : public Sensor<PressureData> +{ +public: + MockPressureSensor() {} -std::default_random_engine generator(seed); // Noise generator -std::normal_distribution<float> distribution( - 0.0, NOISE_STD_DEV); // Noise generator distribution + bool init() override { return true; } -typedef miosix::Gpio<GPIOG_BASE, 13> greenLed; + bool selfTest() override { return true; } -TEST_CASE("Testing ada_controller from calibration to first descent phase") + PressureData sampleImpl() override + { + float press = 0.0; + + if (before_liftoff) + { + press = addNoise(ADA_SIMULATED_PRESSURE[0]); + } + else + { + if (i < DATA_SIZE) + { + press = addNoise(ADA_SIMULATED_PRESSURE[i++]); + } + else + { + press = addNoise(ADA_SIMULATED_PRESSURE[DATA_SIZE - 1]); + } + } + + return PressureData{TimestampTimer::getTimestamp(), press}; + } + + void signalLiftoff() { before_liftoff = false; } + +private: + volatile bool before_liftoff = true; + volatile unsigned int i = 0; // Last index + std::default_random_engine generator{1234567}; + std::normal_distribution<float> distribution{0.0f, NOISE_STD_DEV}; + + float addNoise(float sample) + { + return quantization(sample + distribution(generator)); + } + + float quantization(float sample) { return round(sample / LSB) * LSB; } +}; + +class MockGPSSensor : public Sensor<GPSData> +{ +public: + bool init() { return true; } + bool selfTest() { return true; } + GPSData sampleImpl() { return GPSData{}; } +}; + +MockPressureSensor mock_baro; +MockGPSSensor mock_gps; + +using ADACtrl = ADAController<PressureData, GPSData>; +ADACtrl *ada_controller; + +void checkState(unsigned int i, ADAKalmanState state) { - // Setting pin mode for signaling ada_controller status + if (i > 200) { - miosix::FastInterruptDisableLock dLock; - greenLed::mode(miosix::Mode::OUTPUT); + if (state.x0 == Approx(ADA_SIMULATED_PRESSURE[i]).margin(70)) + SUCCEED(); + else + FAIL("i = " << i << "\t\t" << state.x0 + << " != " << ADA_SIMULATED_PRESSURE[i]); + + // Flying under the chutes the speed estimation is not very precise + if (i < 3000) + { + if (state.x1 == Approx(ADA_SIMULATED_PRESSURE_SPEED[i]).margin(80)) + SUCCEED(); + else + FAIL("i = " << i << "\t\t" << state.x1 + << " != " << ADA_SIMULATED_PRESSURE_SPEED[i]); + } } +} - ada_controller = new ADAController(); +TEST_CASE("Testing ada_controller from calibration to first descent phase") +{ + TimestampTimer::enableTimestampTimer(); + + ada_controller = new ADACtrl(mock_baro, mock_gps); + TRACE("ADA init : %d \n", ada_controller->start()); // Start event broker and ada_controller sEventBroker->start(); - ada_controller->start(); EventCounter counter{*sEventBroker}; counter.subscribe(TOPIC_ADA); // Startup: we should be in idle Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateIdle)); + REQUIRE(ada_controller->testState(&ADACtrl::state_idle)); // Enter Calibrating and REQUIRE - sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_TC); + sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_ADA); Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateCalibrating)); + REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating)); // Send baro calibration samples for (unsigned i = 0; i < CALIBRATION_BARO_N_SAMPLES + 5; i++) { - ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0])); + mock_baro.sample(); + Thread::sleep(10); + ada_controller->update(); } - // float mean = ada_controller->calibrator - // if (mean == Approx(SIMULATED_PRESSURE[0])) - // FAIL("Calibration value"); - // else - // SUCCEED(); + float mean = ada_controller->calibrator.getReferenceValues().ref_pressure; + if (mean == Approx(ADA_SIMULATED_PRESSURE[0])) + FAIL("Calibration value"); + else + SUCCEED(); // Should still be in calibrating Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateCalibrating)); + REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating)); // Send set deployment altitude ada_controller->setDeploymentAltitude(100); - ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0])); + mock_baro.sample(); + Thread::sleep(10); + ada_controller->update(); // Should still be in calibrating Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateCalibrating)); + REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating)); // Send set altitude ref ada_controller->setReferenceAltitude(1300); - ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0])); - + mock_baro.sample(); + Thread::sleep(10); + ada_controller->update(); // Should still be in calibrating Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateCalibrating)); + REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating)); // Send set temperature ref ada_controller->setReferenceTemperature(15); - ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0])); + mock_baro.sample(); + Thread::sleep(10); + ada_controller->update(); // Now we should be in ready Thread::sleep(100); - ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0])); + mock_baro.sample(); + Thread::sleep(10); + ada_controller->update(); Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateReady)); - - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + REQUIRE(ada_controller->testState(&ADACtrl::state_ready)); // Send liftoff event: should be in shadow mode + sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + mock_baro.signalLiftoff(); Thread::sleep(100); - REQUIRE(ada_controller->testState(&ADAController::stateShadowMode)); + REQUIRE(ada_controller->testState(&ADACtrl::state_shadowMode)); long long shadow_mode_start = miosix::getTick(); - // Perform some checks while in shadow mode (to avoid triggering a false - // apogee) + // Perform some chcmathecks while in shadow mode (to avoid triggering a + // false apogee) for (unsigned i = 0; i < SHADOW_MODE_END_INDEX; i++) { - greenLed::high(); - float noisy_p = addNoise(SIMULATED_PRESSURE[i]); - ada_controller->updateBaro(noisy_p); + // float noisy_p = addNoise(ADA_SIMULATED_PRESSURE[i]); + mock_baro.sample(); + Thread::sleep(5); + ada_controller->update(); + float noisy_p = mock_baro.getLastSample().press; // Thread::sleep(100); - KalmanState state = ada_controller->ada.getKalmanState(); + ADAKalmanState state = ada_controller->ada.getKalmanState(); printf("%d,%f,%f,%f\n", (int)i, noisy_p, state.x0, ada_controller->ada.getVerticalSpeed()); checkState(i, state); - greenLed::low(); } + // Wait timeout Thread::sleepUntil(shadow_mode_start + TIMEOUT_ADA_SHADOW_MODE); // Should be active now - REQUIRE(ada_controller->testState(&ADAController::stateActive)); + REQUIRE(ada_controller->testState(&ADACtrl::state_active)); Thread::sleep(100); // Send samples - - printf("%d\n", DATA_SIZE); bool apogee_checked = false; for (unsigned i = SHADOW_MODE_END_INDEX; i < DATA_SIZE; i++) { - greenLed::high(); - float noisy_p = addNoise(SIMULATED_PRESSURE[i]); - ada_controller->updateBaro(noisy_p); + // float noisy_p = addNoise(ADA_SIMULATED_PRESSURE[i]); + mock_baro.sample(); + Thread::sleep(5); + ada_controller->update(); + float noisy_p = mock_baro.getLastSample().press; // Thread::sleep(100); - KalmanState state = ada_controller->ada.getKalmanState(); + ADAKalmanState state = ada_controller->ada.getKalmanState(); printf("%d,%f,%f,%f\n", (int)i, noisy_p, state.x0, ada_controller->ada.getVerticalSpeed()); checkState(i, state); @@ -173,51 +261,21 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") !apogee_checked) { apogee_checked = true; - if (abs(i - 382) > 10) + if (fabs(i - APOGEE_SAMPLE) > 10) { - FAIL("Apogee error: " << (int)i - 382 << " samples"); + FAIL("Apogee error: " << (int)i - APOGEE_SAMPLE << " samples"); } else { - printf("Apogee error: %d samples\n", (int)(i - 382)); + printf("Apogee error: %d samples\n", (int)(i - APOGEE_SAMPLE)); SUCCEED(); } - - REQUIRE(ada_controller->testState(&ADAController::statePressureStabilization)); - Thread::sleep(EV_TIMEOUT_PRESS_STABILIZATION + 1000); - REQUIRE(ada_controller->testState(&ADAController::stateFirstDescentPhase)); - } - - greenLed::low(); - } -} -void checkState(unsigned int i, KalmanState state) -{ - if (i > 200) - { - if (state.x0 == Approx(SIMULATED_PRESSURE[i]).margin(70)) - SUCCEED(); - else - FAIL("i = " << i << "\t\t" << state.x0 - << " != " << SIMULATED_PRESSURE[i]); - - // Flying under the chutes the speed estimation is not very precise - if (i < 3000) - { - if (state.x1 == Approx(SIMULATED_PRESSURE_SPEED[i]).margin(80)) - SUCCEED(); - else - FAIL("i = " << i << "\t\t" << state.x1 - << " != " << SIMULATED_PRESSURE_SPEED[i]); + Thread::sleep(1000); + REQUIRE(ada_controller->testState( + &ADACtrl::state_pressureStabilization)); + Thread::sleep(EV_TIMEOUT_PRESS_STABILIZATION + 1000); + REQUIRE(ada_controller->testState(&ADACtrl::state_drogueDescent)); } } -} - -float addNoise(float sample) -{ - float noise = distribution(generator); - return quantization(sample + noise); -} - -float quantization(float sample) { return round(sample / LSB) * LSB; } \ No newline at end of file +} \ No newline at end of file diff --git a/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp b/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp index 60c64838d3690d80bfef58e98580fd4ce1d9c406..abb8e7fafec12802b48b7e91e078d6cd9c67ca2f 100644 --- a/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp +++ b/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp @@ -1,26 +1,25 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli * - * Permission is hereby grantedf, free of chargef, to any person obtaining a - * copy of this software and associated documentation files (the "Software")f, - * to deal in the Software without restrictionf, including without limitation - * the rights to usef, copyf, modifyf, mergef, publishf, distributef, - * sublicensef, and/or sell copies of the Softwaref, and to permit persons to - * whom the Software is furnished to do sof, subject to the following - * conditions: + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * - * THE SOFTWARE IS PROVIDED "AS IS"f, WITHOUT WARRANTY OF ANY KINDf, EXPRESS OR - * IMPLIEDf, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIMf, DAMAGES OR OTHER - * LIABILITYf, WHETHER IN AN ACTION OF CONTRACTf, TORT OR OTHERWISEf, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS - * IN THE SOFTWARE. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. */ + #include "test-kalman-acc-data.h" const float SIMULATED_AX[DATA_SIZE_AX] = { diff --git a/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.h b/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.h index ee104329c3aa41a9cb9f8fea0b987c4a1768baba..1128d56143c9a4d11566d775acd1769027d89597 100644 --- a/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.h +++ b/src/tests/catch/ada/kalman_acc/test-kalman-acc-data.h @@ -1,20 +1,19 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -31,5 +30,4 @@ extern const float SIMULATED_VZ[DATA_SIZE_P]; static const unsigned DATA_SIZE_AX = 17495; extern const float SIMULATED_AX[DATA_SIZE_AX]; - // extern const float SIMULATED_NOISY_PRESSURE[DATA_SIZE]; \ No newline at end of file diff --git a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp index 4aafd250b43c6c9366f7559d375a8b0228c5680f..883cd1fd46d1db641417d69b5675a505675a94c4 100644 --- a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp +++ b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp @@ -1,3 +1,24 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ #ifdef STANDALONE_CATCH1_TEST #include "catch/catch-tests-entry.cpp" @@ -5,15 +26,19 @@ #define private public +#include <ADA/ADA.h> #include <Common.h> -#include <DeathStack/ADA/ADA.h> +#include <configs/ADAConfig.h> + #include <iostream> #include <random> #include <sstream> #include <utils/testutils/catch.hpp> + #include "test-kalman-acc-data.h" using namespace DeathStackBoard; +using namespace ADAConfigs; constexpr float NOISE_STD_DEV_P = 5; // Noise varaince constexpr float LSB_P = 28; @@ -34,7 +59,7 @@ float addNoise_p(float sample); float quantization_p(float sample); std::normal_distribution<float> distribution_p(0.0, NOISE_STD_DEV_P); -typedef miosix::Gpio<GPIOG_BASE, 13> greenLed; +typedef miosix::Gpio<GPIOA_BASE, 5> greenLed; TEST_CASE("Testing Kalman with accelerometer") { @@ -43,9 +68,9 @@ TEST_CASE("Testing Kalman with accelerometer") ref_values.ref_altitude = 0; ref_values.msl_pressure = SIMULATED_PRESSURE[1]; - ada = new ADA(ref_values); + ada = new ADA(ref_values, getKalmanConfig(), getKalmanAccConfig()); - KalmanState state; + ADAKalmanState state; unsigned int j = 0; for (unsigned int i = 0; i < DATA_SIZE_AX; i++) { @@ -58,7 +83,7 @@ TEST_CASE("Testing Kalman with accelerometer") ada->updateBaro(addNoise_p(SIMULATED_PRESSURE[j])); j++; - state = ada->getKalmanState(); + state = ada->getADAKalmanState(); std::cout << state.x0_acc << ", " << state.x1_acc << ", " << state.x2_acc << ", " << SIMULATED_PRESSURE[i] << ", " << ada->last_acc_average << "\n"; diff --git a/src/tests/catch/ada/test-rogallo-dts.cpp b/src/tests/catch/ada/test-rogallo-dts.cpp index 70a2c12991b389594bac36f80efe7d0fe4c69637..86dbb8ee5391ccbac27bc98dd727a819fefcd847 100644 --- a/src/tests/catch/ada/test-rogallo-dts.cpp +++ b/src/tests/catch/ada/test-rogallo-dts.cpp @@ -1,8 +1,5 @@ -/** - * Rogallo Deployment and Termination System tests - * - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -16,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -26,19 +23,17 @@ #ifdef STANDALONE_CATCH1_TEST #include "catch/catch-tests-entry.cpp" #endif -#include <utils/testutils/catch.hpp> +#include <utils/EventCounter.h> #include <algorithm> +#include <utils/testutils/catch.hpp> -#include "DeathStack/ADA/RogalloDTS/ElevationMap.h" -#include "DeathStack/ADA/RogalloDTS/LHCircles.h" -#include "DeathStack/ADA/RogalloDTS/generated/tests/elevation_map_test_data.h" -#include "DeathStack/ADA/RogalloDTS/generated/tests/lh_circles_test_data.h" - -#include "DeathStack/ADA/RogalloDTS/RogalloDTS.h" - -#include <utils/EventCounter.h> -#include "DeathStack/events/Events.h" +#include "ADA/RogalloDTS/ElevationMap.h" +#include "ADA/RogalloDTS/LHCircles.h" +#include "ADA/RogalloDTS/RogalloDTS.h" +#include "ADA/RogalloDTS/generated/tests/elevation_map_test_data.h" +#include "ADA/RogalloDTS/generated/tests/lh_circles_test_data.h" +#include "events/Events.h" using namespace DeathStackBoard; diff --git a/src/tests/catch/catch-tests-entry.cpp b/src/tests/catch/catch-tests-entry.cpp index b1463cf3ea7d675327a21844ef861913d8d79648..88504833a92870610dd948d69d719055e6f0215f 100644 --- a/src/tests/catch/catch-tests-entry.cpp +++ b/src/tests/catch/catch-tests-entry.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -55,9 +54,10 @@ #define CATCH_CONFIG_NO_POSIX_SIGNALS #include <miosix.h> -#include <utils/testutils/catch.hpp> + #include <cstring> #include <string> +#include <utils/testutils/catch.hpp> #include <vector> using miosix::Thread; diff --git a/src/tests/catch/fsm/test-ada.cpp b/src/tests/catch/fsm/test-ada.cpp index 754e0e4922036b733666c881150e5759b6b410ed..7c303c5d2e85e9b09706f14b673b4b53c5102c93 100644 --- a/src/tests/catch/fsm/test-ada.cpp +++ b/src/tests/catch/fsm/test-ada.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Benedetta Margrethe Cattani & Luca Mozzarelli +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -27,71 +26,145 @@ // We need access to the handleEvent(...) function in state machines in order to // test them synchronously -#define protected public -#define private public #include <miosix.h> + #include <utils/testutils/catch.hpp> -#include <boards/CanInterfaces.h> -#include <boards/DeathStack/ADA/ADAController.h> -#include <boards/DeathStack/events/Events.h> -#include <boards/DeathStack/configs/ADA_config.h> -#include "utils/testutils/TestHelper.h" +#define private public +#define protected public + +#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <events/Events.h> +#include <mocksensors/MockGPS.h> +#include <mocksensors/MockPressureSensor.h> +#include <utils/testutils/TestHelper.h> using miosix::Thread; using namespace DeathStackBoard; -using namespace CanInterfaces; -class ADATestFixture +using ADACtrl = ADAController<PressureData, GPSData>; + +class ADAControllerFixture { public: - ADATestFixture() { ada = new ADAController(); } - ~ADATestFixture() + // This is called at the beginning of each test / section + ADAControllerFixture() { - sEventBroker->unsubscribe(ada); + sEventBroker->start(); + controller = new ADACtrl(mock_baro, mock_gps); + controller->start(); + } + + // This is called at the end of each test / section + ~ADAControllerFixture() + { + controller->stop(); + sEventBroker->unsubscribe(controller); sEventBroker->clearDelayedEvents(); - delete ada; + delete controller; } protected: - ADAController* ada; + MockPressureSensor mock_baro; + MockGPS mock_gps; + + ADACtrl* controller; }; -TEST_CASE_METHOD(ADATestFixture, "Testing all transitions") +TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from idle") +{ + controller->transition(&ADACtrl::state_idle); + + SECTION("EV_CALIBRATE_ADA -> CALIBRATING") + { + REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA}, + &ADACtrl::state_calibrating)); + } +} + +TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from calibrating") +{ + controller->transition(&ADACtrl::state_calibrating); + + SECTION("EV_CALIBRATE_ADA -> CALIBRATING") + { + REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA}, + &ADACtrl::state_calibrating)); + } + + SECTION("EV_ADA_READY -> READY") + { + REQUIRE(testFSMTransition(*controller, Event{EV_ADA_READY}, + &ADACtrl::state_ready)); + } +} + +TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from ready") +{ + controller->transition(&ADACtrl::state_ready); + + SECTION("EV_CALIBRATE_ADA -> CALIBRATING") + { + REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA}, + &ADACtrl::state_calibrating)); + } + + SECTION("EV_LIFTOFF -> SHADOW_MODE") + { + REQUIRE(testFSMTransition(*controller, Event{EV_LIFTOFF}, + &ADACtrl::state_shadowMode)); + } +} + +TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from shadow_mode") +{ + controller->transition(&ADACtrl::state_shadowMode); + + SECTION("EV_SHADOW_MODE_TIMEOUT -> ACTIVE") + { + REQUIRE(testFSMTransition(*controller, Event{EV_SHADOW_MODE_TIMEOUT}, + &ADACtrl::state_active)); + } +} + +TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from active") { - SECTION("Testing CALIBRATION transitions") + controller->transition(&ADACtrl::state_active); + + SECTION("EV_ADA_APOGEE_DETECTED -> PRESSURE_STABILIZATION") { - REQUIRE(testFSMTransition(*ada, Event{EV_ADA_READY}, &ADAController::stateIdle)); + REQUIRE(testFSMTransition(*controller, Event{EV_ADA_APOGEE_DETECTED}, + &ADACtrl::state_pressureStabilization)); } +} - SECTION("Testing IDLE transitions") +TEST_CASE_METHOD(ADAControllerFixture, + "Testing transitions from pressure_stabilization") +{ + controller->transition(&ADACtrl::state_pressureStabilization); + + SECTION("EV_TIMEOUT_PRESS_STABILIZATION -> DROGUE_DESCENT") { - REQUIRE(testFSMTransition(*ada, Event{EV_ADA_READY}, &ADAController::stateIdle)); - - SECTION("IDLE->CALIBRATION") - { - REQUIRE(testFSMTransition(*ada, Event{EV_TC_CALIBRATE_ADA}, - &ADAController::stateCalibrating)); - } - - SECTION("IDLE->SHADOW_MODE") - { - REQUIRE(testFSMTransition(*ada, Event{EV_LIFTOFF}, - &ADAController::stateShadowMode)); - } + REQUIRE(testFSMTransition(*controller, + Event{EV_TIMEOUT_PRESS_STABILIZATION}, + &ADACtrl::state_drogueDescent)); } +} + +TEST_CASE_METHOD(ADAControllerFixture, + "Testing transitions from drogue_descent") +{ + controller->transition(&ADACtrl::state_drogueDescent); - SECTION("Testing all the transition from SHADOW_MODE") + SECTION("EV_ADA_DPL_ALT_DETECTED -> END") { - REQUIRE(testFSMTransition(*ada, Event{EV_ADA_READY}, &ADAController::stateIdle)); - REQUIRE( - testFSMTransition(*ada, Event{EV_LIFTOFF}, &ADAController::stateShadowMode)); - REQUIRE(testFSMTransition(*ada, Event{EV_TIMEOUT_SHADOW_MODE}, - &ADAController::stateActive)); - REQUIRE(testFSMTransition(*ada, Event{EV_APOGEE}, - &ADAController::stateFirstDescentPhase)); - REQUIRE( - testFSMTransition(*ada, Event{EV_DPL_ALTITUDE}, &ADAController::stateEnd)); + REQUIRE(testFSMTransition(*controller, Event{EV_ADA_DPL_ALT_DETECTED}, + &ADACtrl::state_end)); } +} + +TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from end") +{ + controller->transition(&ADACtrl::state_end); } \ No newline at end of file diff --git a/src/tests/catch/fsm/test-airbrakes.cpp b/src/tests/catch/fsm/test-airbrakes.cpp new file mode 100644 index 0000000000000000000000000000000000000000..abaa3c1e090e9efd15c0cc84a49521f2ae3683c1 --- /dev/null +++ b/src/tests/catch/fsm/test-airbrakes.cpp @@ -0,0 +1,201 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch/catch-tests-entry.cpp" +#endif + +// We need access to the handleEvent(...) function in state machines in order to +// test them synchronously +#define protected public + +#include <AirBrakes/AirBrakesController.h> +#include <AirBrakes/AirBrakesServo.h> +#include <miosix.h> + +#include <utils/testutils/catch.hpp> + +#include "configs/AirBrakesConfig.h" +#include "events/Events.h" +#include "utils/testutils/TestHelper.h" + +using miosix::Thread; +using namespace DeathStackBoard; + +class InputClass +{ +public: + float z; + float vz; + float vMod; + uint64_t timestamp; +}; + +template <typename T> +class MockSensor : public Sensor<T> +{ +private: + int ts = 0; + +protected: + T sampleImpl() override + { + T data; + return data; + } + + bool init() override { return true; } + + bool selfTest() override { return true; } +}; + +class AirBrakesControllerFixture +{ +public: + // This is called at the beginning of each test / section + AirBrakesControllerFixture() + { + controller = new AirBrakesController<InputClass>(sensor); + sEventBroker->start(); + controller->start(); + } + + // This is called at the end of each test / section + ~AirBrakesControllerFixture() + { + controller->stop(); + sEventBroker->unsubscribe(controller); + sEventBroker->clearDelayedEvents(); + delete controller; + } + +protected: + AirBrakesController<InputClass>* controller; + + MockSensor<InputClass> sensor; +}; + +TEST_CASE_METHOD(AirBrakesControllerFixture, + "Testing transitions from initialization") +{ + controller->transition( + &AirBrakesController<InputClass>::state_initialization); + + REQUIRE( + controller->testState(&AirBrakesController<InputClass>::state_idle)); +} + +TEST_CASE_METHOD(AirBrakesControllerFixture, "Testing transitions from idle") +{ + controller->transition(&AirBrakesController<InputClass>::state_idle); + + SECTION("EV_LIFTOFF -> SHADOW_MODE") + { + REQUIRE(testFSMTransition( + *controller, {EV_LIFTOFF}, + &AirBrakesController<InputClass>::state_shadowMode)); + } + + SECTION("EV_WIGGLE_SERVO -> IDLE") + { + REQUIRE( + testFSMTransition(*controller, {EV_WIGGLE_SERVO}, + &AirBrakesController<InputClass>::state_idle)); + } + + SECTION("EV_RESET_SERVO -> IDLE") + { + REQUIRE( + testFSMTransition(*controller, {EV_RESET_SERVO}, + &AirBrakesController<InputClass>::state_idle)); + } + + SECTION("EV_TEST_ABK -> TEST_AEROBRAKES") + { + REQUIRE(testFSMTransition( + *controller, {EV_TEST_ABK}, + &AirBrakesController<InputClass>::state_testAirbrakes)); + } +} + +TEST_CASE_METHOD(AirBrakesControllerFixture, + "Testing transitions from shadow_mode") +{ + controller->transition(&AirBrakesController<InputClass>::state_shadowMode); + + SECTION("EV_SHADOW_MODE_TIMEOUT -> ENABLED") + { + REQUIRE( + testFSMTransition(*controller, {EV_SHADOW_MODE_TIMEOUT}, + &AirBrakesController<InputClass>::state_enabled)); + } + + SECTION("EV_DISABLE_ABK -> DISABLED") + { + REQUIRE(testFSMTransition( + *controller, {EV_DISABLE_ABK}, + &AirBrakesController<InputClass>::state_disabled)); + } +} + +TEST_CASE_METHOD(AirBrakesControllerFixture, "Testing transitions from enabled") +{ + controller->transition(&AirBrakesController<InputClass>::state_enabled); + + SECTION("EV_APOGEE -> END") + { + REQUIRE(testFSMTransition(*controller, {EV_APOGEE}, + &AirBrakesController<InputClass>::state_end)); + } + + SECTION("EV_DISABLE_ABK -> DISABLED") + { + REQUIRE(testFSMTransition( + *controller, {EV_DISABLE_ABK}, + &AirBrakesController<InputClass>::state_disabled)); + } +} + +TEST_CASE_METHOD(AirBrakesControllerFixture, "Testing transitions from end") +{ + controller->transition(&AirBrakesController<InputClass>::state_end); +} + +TEST_CASE_METHOD(AirBrakesControllerFixture, + "Testing transitions from disabled") +{ + controller->transition(&AirBrakesController<InputClass>::state_disabled); +} + +TEST_CASE_METHOD(AirBrakesControllerFixture, + "Testing transitions from test_airbrakes") +{ + controller->transition( + &AirBrakesController<InputClass>::state_testAirbrakes); + + SECTION("EV_TEST_TIMEOUT -> IDLE") + { + REQUIRE( + testFSMTransition(*controller, {EV_TEST_TIMEOUT}, + &AirBrakesController<InputClass>::state_idle)); + } +} \ No newline at end of file diff --git a/src/tests/catch/fsm/test-deployment.cpp b/src/tests/catch/fsm/test-deployment.cpp index db6f9f96d9aca4dfd333e8ceeffc95016b5d89a2..890be19a0b569b252ea20182e891bea0461f55f5 100644 --- a/src/tests/catch/fsm/test-deployment.cpp +++ b/src/tests/catch/fsm/test-deployment.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Alvise de'Faveri Tron, Luca Erbetta, Luca Mozzarelli * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -29,422 +28,100 @@ // test them synchronously #define protected public +#include <Deployment/DeploymentController.h> +#include <drivers/servo/servo.h> #include <miosix.h> + #include <utils/testutils/catch.hpp> -#include "DeathStack/DeploymentController/DeploymentController.h" -#include "DeathStack/events/Events.h" -#include "utils/PinObserver.h" +#include "configs/DeploymentConfig.h" +#include "events/Events.h" #include "utils/testutils/TestHelper.h" using miosix::Thread; using namespace DeathStackBoard; -/** - * @brief Ensure cleanup in every test using RAIII - * - */ class DeploymentControllerFixture { public: // This is called at the beginning of each test / section DeploymentControllerFixture() { - dpl = new DeploymentController(); + controller = new DeploymentController(&ejection_servo); sEventBroker->start(); - dpl->start(); + controller->start(); } // This is called at the end of each test / section ~DeploymentControllerFixture() { - dpl->stop(); - sEventBroker->unsubscribe(dpl); + controller->stop(); + sEventBroker->unsubscribe(controller); sEventBroker->clearDelayedEvents(); - delete dpl; + delete controller; } protected: - DeploymentController* dpl; + DeploymentController* controller; + DeploymentServo ejection_servo; }; -/** - * TEST_CASE_METHOD(Foo, "...") can access all protected members of Foo. See the - * catch framework reference on Github. - */ -TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from IDLE") +TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from idle") { - SECTION("IDLE -> Opening Nosecone") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_OPEN}, - &DeploymentController::state_spinning)); - } + controller->transition(&DeploymentController::state_idle); - SECTION("IDLE -> Cutting main") + SECTION("DPL_IDLE -> EV_RESET_SERVO") { - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_cuttingMain)); + REQUIRE(testFSMTransition(*controller, Event{EV_RESET_SERVO}, + &DeploymentController::state_idle)); } - SECTION("IDLE -> Cutting drogue") + SECTION("DPL_IDLE -> EV_WIGGLE_SERVO") { - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_cuttingDrogue)); + REQUIRE(testFSMTransition(*controller, Event{EV_WIGGLE_SERVO}, + &DeploymentController::state_idle)); } -} - -TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from CUTTING MAIN") -{ - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_cuttingMain)); - - SECTION(" Cutting Main -> Idle") + SECTION("DPL_IDLE -> EV_NC_OPEN") { REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING}, - &DeploymentController::state_idle)); + testFSMTransition(*controller, Event{EV_NC_OPEN}, + &DeploymentController::state_noseconeEjection)); } - SECTION("Deferred event: EV_CUT_DROGUE") + SECTION("DPL_IDLE -> EV_CUT_DROGUE") { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_cuttingMain)); - // Send TIMEOUT_CUTTING: back in idle - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_DROGUE should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); + REQUIRE(testFSMTransition(*controller, Event{EV_CUT_DROGUE}, + &DeploymentController::state_cutting)); } } -TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from CUTTING DROGUE") +TEST_CASE_METHOD(DeploymentControllerFixture, + "Testing transitions from nosecone_ejection") { - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_cuttingDrogue)); + controller->transition(&DeploymentController::state_noseconeEjection); - SECTION("Cutting Drogue -> Idle") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING}, - &DeploymentController::state_idle)); - } - SECTION("Deferred event: EV_CUT_MAIN") + SECTION("DPL_NOSECONE_EJECTION -> EV_NC_DETACHED") { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_cuttingDrogue)); - // Send TIMEOUT_CUTTING: back in idle - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); + REQUIRE(testFSMTransition(*controller, Event{EV_NC_DETACHED}, + &DeploymentController::state_idle)); } -} -TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from SPINNING") -{ - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_OPEN}, - &DeploymentController::state_spinning)); - SECTION("SPINNING -> Waiting Detachment") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_awaitingDetachment)); - } - SECTION("SPINNING -> Waiting Min Open Time") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_awaitingOpenTime)); - } - SECTION("OPENING NOSECONE -> Idle") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - } - SECTION("Deferred event: EV_CUT_MAIN (spinning->idle)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_spinning)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_MAIN (spinning->waiting time->idle)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_spinning)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_awaitingOpenTime)); - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_MAIN (spinning->waiting detachement->idle)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_spinning)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_awaitingDetachment)); - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_DROGUE") - { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_spinning)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_DROGUE should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); - } - SECTION("Deferred event: EV_CUT_DROGUE (spinning->waiting time->idle)") - { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_spinning)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_awaitingOpenTime)); - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); - } - SECTION("Deferred event: EV_CUT_DROGUE (spinning->waiting detachement->idle)") + SECTION("DPL_NOSECONE_EJECTION -> EV_NC_OPEN_TIMEOUT") { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_spinning)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_awaitingDetachment)); - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); + REQUIRE(testFSMTransition(*controller, Event{EV_NC_OPEN_TIMEOUT}, + &DeploymentController::state_idle)); } } -TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from WAITING MIN OPEN TIME") +TEST_CASE_METHOD(DeploymentControllerFixture, + "Testing transitions from cutting") { - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_OPEN}, - &DeploymentController::state_spinning)); - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_awaitingOpenTime)); + controller->transition(&DeploymentController::state_cutting); - SECTION("WAITING MIN OPEN TIME -> Idle") + SECTION("DPL_CUTTING -> EV_CUTTING_TIMEOUT") { - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_idle)); - } - SECTION("OPENING NOSECONE -> Idle") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - } - SECTION("Deferred event: EV_CUT_MAIN (timeout)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_awaitingOpenTime)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_MAIN (min_open_time)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_awaitingOpenTime)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_DROGUE (timeout)") - { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_awaitingOpenTime)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_DROGUE should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); - } - SECTION("Deferred event: EV_CUT_DROGUE (min_open_time)") - { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_awaitingOpenTime)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_DROGUE should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); + REQUIRE(testFSMTransition(*controller, Event{EV_CUTTING_TIMEOUT}, + &DeploymentController::state_idle)); } } - -TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from WAITING DETACHEMENT") -{ - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_OPEN}, - &DeploymentController::state_spinning)); - REQUIRE( - testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME}, - &DeploymentController::state_awaitingDetachment)); - - SECTION("WAITING DETACHEMENT -> Idle") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_idle)); - } - SECTION("OPENING NOSECONE -> Idle") - { - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - } - SECTION("Deferred event: EV_CUT_MAIN (timeout)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_awaitingDetachment)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_DROGUE (timeout)") - { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_awaitingDetachment)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_DROGUE should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); - } - SECTION("Deferred event: EV_CUT_MAIN (nc_detached)") - { - // Send CUT_MAIN: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_MAIN}, - &DeploymentController::state_awaitingDetachment)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_MAIN should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain )); - } - SECTION("Deferred event: EV_CUT_DROGUE (nc_detached)") - { - // Send CUT_DROGUE: nothing should happen - REQUIRE( - testHSMTransition(*dpl, Event{EV_CUT_DROGUE}, - &DeploymentController::state_awaitingDetachment)); - // Go back in IDLE - REQUIRE( - testHSMTransition(*dpl, Event{EV_NC_DETACHED}, - &DeploymentController::state_idle)); - // Wait a bit to allow EV_ENTRY handling - Thread::sleep(10); - // CUT_DROGUE should now be processed - REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue )); - } -} \ No newline at end of file diff --git a/src/tests/catch/fsm/test-flightstatsrecorder.cpp b/src/tests/catch/fsm/test-flightstatsrecorder.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8d3d632cd5224b59b9ff8326006f592755b828fc --- /dev/null +++ b/src/tests/catch/fsm/test-flightstatsrecorder.cpp @@ -0,0 +1,132 @@ +/* + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch/catch-tests-entry.cpp" +#endif + +// We need access to the handleEvent(...) function in state machines in order to +// test them synchronously + +#include <miosix.h> + +#include <utils/testutils/catch.hpp> + +#define private public +#define protected public + +#include <FlightStatsRecorder/FSRController.h> + +#include "events/Events.h" +#include "utils/testutils/TestHelper.h" + +using miosix::Thread; +using namespace DeathStackBoard; + +class FlightStatsFixture +{ +public: + // This is called at the beginning of each test / section + FlightStatsFixture() + { + sEventBroker->start(); + fsm = new FlightStatsRecorder(); + fsm->start(); + } + + // This is called at the end of each test / section + ~FlightStatsFixture() + { + fsm->stop(); + sEventBroker->unsubscribe(fsm); + sEventBroker->clearDelayedEvents(); + delete fsm; + } + +protected: + FlightStatsRecorder* fsm; +}; + +TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from idle") +{ + SECTION("EV_LIFTOFF -> liftoff") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_LIFTOFF}, + &FlightStatsRecorder::state_liftOff)); + } + + SECTION("EV_DPL_ALTITUDE -> mainDPL") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_DPL_ALTITUDE}, + &FlightStatsRecorder::state_mainDeployment)); + } +} + +TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from liftoff") +{ + fsm->transition(&FlightStatsRecorder::state_liftOff); + Thread::sleep(100); + + SECTION("EV_STATS_TIMEOUT -> ascending") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + &FlightStatsRecorder::state_ascending)); + } +} + +TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from ascending") +{ + fsm->transition(&FlightStatsRecorder::state_ascending); + Thread::sleep(100); + + SECTION("EV_APOGEE -> drogueDPL") + { + REQUIRE( + testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + &FlightStatsRecorder::state_drogueDeployment)); + } +} + +TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from drogueDPL") +{ + fsm->transition(&FlightStatsRecorder::state_drogueDeployment); + Thread::sleep(100); + + SECTION("EV_STATS_TIMEOUT -> idle") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + &FlightStatsRecorder::state_idle)); + } +} + +TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from mainDPL") +{ + fsm->transition(&FlightStatsRecorder::state_mainDeployment); + Thread::sleep(100); + + SECTION("EV_STATS_TIMEOUT -> idle") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + &FlightStatsRecorder::state_idle)); + } +} \ No newline at end of file diff --git a/src/tests/catch/fsm/test-fmm.cpp b/src/tests/catch/fsm/test-fmm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..143651cab6ee1f1b156318ca5670b444451aec46 --- /dev/null +++ b/src/tests/catch/fsm/test-fmm.cpp @@ -0,0 +1,350 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch/catch-tests-entry.cpp" +#endif + +// We need access to the handleEvent(...) function in state machines in order to +// test them synchronously + +#include <miosix.h> + +#include <utils/testutils/catch.hpp> + +#include "events/Events.h" + +#define private public +#define protected public + +#include <FlightModeManager/FMMController.h> + +#include "utils/testutils/TestHelper.h" + +using miosix::Thread; +using namespace DeathStackBoard; + +class FMMFixture +{ +public: + // This is called at the beginning of each test / section + FMMFixture() + { + sEventBroker->start(); + fsm = new FMMController(); + fsm->start(); + } + + // This is called at the end of each test / section + ~FMMFixture() + { + fsm->stop(); + sEventBroker->unsubscribe(fsm); + sEventBroker->clearDelayedEvents(); + delete fsm; + } + +protected: + FMMController* fsm; +}; + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_onGround") +{ + SECTION("EV_TC_RESET_BOARD -> ON_GROUND (INIT)") + { + // If you decomment this the board will reset continuously, + // since the FMM receives the event that triggers a reboot... + // I needed one hour of debugging to figure it out :') + /* + REQUIRE(testHSMTransition( + *fsm, Event{EV_TC_RESET_BOARD}, + &FMMController::state_init)); // initial state of + // state_onGround + */ + } + + SECTION("EV_TC_LAUNCH -> FLYING (ASCENDING)") + { + REQUIRE(testHSMTransition( + *fsm, Event{EV_TC_LAUNCH}, + &FMMController::state_ascending)); // initial state of + // state_flying + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_flying") +{ + // move to state_flying (state_ascending) + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(50); + fsm->postEvent(Event{EV_SENSORS_READY}); + Thread::sleep(50); + fsm->postEvent(Event{EV_CALIBRATION_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_ARM}); + Thread::sleep(50); + fsm->postEvent(Event{EV_UMBILICAL_DETACHED}); + Thread::sleep(100); + + SECTION("EV_TC_END_MISSION -> LANDED") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_END_MISSION}, + &FMMController::state_landed)); + } + + SECTION("EV_TIMEOUT_END_MISSION -> LANDED") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TIMEOUT_END_MISSION}, + &FMMController::state_landed)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_init") +{ + SECTION("EV_INIT_OK -> INIT_DONE") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_INIT_OK}, + &FMMController::state_initDone)); + } + + SECTION("EV_INIT_ERROR -> INIT_ERROR") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_INIT_ERROR}, + &FMMController::state_initError)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_initError") +{ + // move to state_initError + fsm->postEvent(Event{EV_INIT_ERROR}); + Thread::sleep(50); + + SECTION("EV_TC_FORCE_INIT -> INIT_DONE") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_FORCE_INIT}, + &FMMController::state_initDone)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_initDone") +{ + // move to state_initDone + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + + SECTION("EV_TC_TEST_MODE -> TEST_MODE") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_TEST_MODE}, + &FMMController::state_testMode)); + } + + SECTION("EV_TC_CALIBRATE_SENSORS -> SENSORS_CALIBRATION") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_SENSORS}, + &FMMController::state_sensorsCalibration)); + } +} + +/* +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_testMode") +{ + +} +*/ + +TEST_CASE_METHOD(FMMFixture, + "Testing transitions from state_sensorsCalibration") +{ + // move to state_calibrating + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(100); + + SECTION("EV_TC_CALIBRATE_SENSORS -> SENSORS_CALIBRATION") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_SENSORS}, + &FMMController::state_sensorsCalibration)); + } + + SECTION("EV_SENSORS_READY -> ALGOS_CALIBRATION") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_SENSORS_READY}, + &FMMController::state_algosCalibration)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_algosCalibration") +{ + // move to state_calibrating + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(50); + fsm->postEvent(Event{EV_SENSORS_READY}); + Thread::sleep(100); + + SECTION("EV_TC_CALIBRATE_ALGOS -> ALGOS_CALIBRATION") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_ALGOS}, + &FMMController::state_algosCalibration)); + } + + SECTION("EV_CALIBRATION_OK -> DISARMED") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_CALIBRATION_OK}, + &FMMController::state_disarmed)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_disarmed") +{ + // move to state_disarmed + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(50); + fsm->postEvent(Event{EV_SENSORS_READY}); + Thread::sleep(50); + fsm->postEvent(Event{EV_CALIBRATION_OK}); + Thread::sleep(100); + + SECTION("EV_TC_CALIBRATE_ALGOS -> ALGOS_CALIBRATION") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_ALGOS}, + &FMMController::state_algosCalibration)); + } + + SECTION("EV_TC_CALIBRATE_SENSORS -> SENSORS_CALIBRATION") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_SENSORS}, + &FMMController::state_sensorsCalibration)); + } + + SECTION("EV_TC_ARM -> ARMED") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_ARM}, + &FMMController::state_armed)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_armed") +{ + // move to state_armed + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(50); + fsm->postEvent(Event{EV_SENSORS_READY}); + Thread::sleep(50); + fsm->postEvent(Event{EV_CALIBRATION_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_ARM}); + Thread::sleep(100); + + SECTION("EV_TC_DISARM -> DISARMED") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_DISARM}, + &FMMController::state_disarmed)); + } + + SECTION("EV_TC_LAUNCH -> ASCENDING") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_LAUNCH}, + &FMMController::state_ascending)); + } + + SECTION("EV_UMBILICAL_DETACHED -> ASCENDING") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_UMBILICAL_DETACHED}, + &FMMController::state_ascending)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_ascending") +{ + // move to state_ascending + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(50); + fsm->postEvent(Event{EV_SENSORS_READY}); + Thread::sleep(50); + fsm->postEvent(Event{EV_CALIBRATION_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_ARM}); + Thread::sleep(50); + fsm->postEvent(Event{EV_UMBILICAL_DETACHED}); + Thread::sleep(100); + + SECTION("EV_ADA_APOGEE_DETECTED -> DROGUE_DESCENT") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_ADA_APOGEE_DETECTED}, + &FMMController::state_drogueDescent)); + } + + SECTION("EV_ADA_DISABLE_ABK -> ASCENDING") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_ADA_DISABLE_ABK}, + &FMMController::state_ascending)); + } + + SECTION("EV_TC_NC_OPEN -> DROGUE_DESCENT") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_NC_OPEN}, + &FMMController::state_drogueDescent)); + } +} + +TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_drogueDescent") +{ + // move to state_drogueDescent + fsm->postEvent(Event{EV_INIT_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + Thread::sleep(50); + fsm->postEvent(Event{EV_SENSORS_READY}); + Thread::sleep(50); + fsm->postEvent(Event{EV_CALIBRATION_OK}); + Thread::sleep(50); + fsm->postEvent(Event{EV_TC_ARM}); + Thread::sleep(50); + fsm->postEvent(Event{EV_UMBILICAL_DETACHED}); + Thread::sleep(50); + fsm->postEvent(Event{EV_ADA_APOGEE_DETECTED}); + Thread::sleep(100); + + SECTION("EV_ADA_DPL_ALT_DETECTED -> TERMINAL_DESCENT") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_ADA_DPL_ALT_DETECTED}, + &FMMController::state_terminalDescent)); + } + + SECTION("EV_TC_CUT_DROGUE -> TERMINAL_DESCENT") + { + REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CUT_DROGUE}, + &FMMController::state_terminalDescent)); + } +} \ No newline at end of file diff --git a/src/tests/catch/fsm/test-ignition.cpp b/src/tests/catch/fsm/test-ignition.cpp index c910afbe8780b8fe2ed15efc7f2b44803e619c66..fbda2c9275f402fec7c04fa3cd1f31256b935b12 100644 --- a/src/tests/catch/fsm/test-ignition.cpp +++ b/src/tests/catch/fsm/test-ignition.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Benedetta Margrethe Cattani +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Benedetta Margrethe Cattani * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -30,14 +29,15 @@ #define protected public #define private public +#include <CanInterfaces.h> +#include <EventClasses.h> +#include <IgnitionController/IgnitionController.h> +#include <configs/IgnitionConfig.h> +#include <events/Events.h> #include <miosix.h> + #include <utils/testutils/catch.hpp> -#include <boards/CanInterfaces.h> -#include <boards/DeathStack/EventClasses.h> -#include <boards/DeathStack/events/Events.h> -#include <boards/DeathStack/IgnitionController/IgnitionController.h> -#include <boards/DeathStack/configs/IgnitionConfig.h> #include "utils/testutils/TestHelper.h" using miosix::Thread; @@ -193,7 +193,7 @@ TEST_CASE_METHOD(IgnitionTestFixture2, "Igntiion: Testing IDLE functions") long long start = miosix::getTick(); REQUIRE(expectEvent(EV_IGN_OFFLINE, TOPIC_FLIGHT_EVENTS, - start + TIMEOUT_IGN_OFFLINE)); + start + TIMEOUT_IGN_OFFLINE)); } SECTION("IGNITION OFFLINE 2") // Wait for EV_IGN_OFFLINE after we post diff --git a/src/tests/catch/fsm/test-nas.cpp b/src/tests/catch/fsm/test-nas.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ee2667fa03347e185ac4ad7e45db4bce735a452d --- /dev/null +++ b/src/tests/catch/fsm/test-nas.cpp @@ -0,0 +1,129 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch/catch-tests-entry.cpp" +#endif + +#include <utils/testutils/catch.hpp> + +// We need access to the handleEvent(...) function in state machines in order to +// test them synchronously +#define private public +#define protected public + +#include <NavigationAttitudeSystem/NASController.h> +#include <NavigationAttitudeSystem/NASCalibrator.h> +#include "events/Events.h" +#include <mocksensors/MockSensors.h> +#include "utils/testutils/TestHelper.h" + +using namespace DeathStackBoard; + +using NASCtrl = NASController<MockIMUData, PressureData, GPSData>; + +class NASControllerFixture +{ +public: + // This is called at the beginning of each test / section + NASControllerFixture() : controller(mock_imu, mock_baro, mock_gps) + { + sEventBroker->start(); + controller.start(); + } + + // This is called at the end of each test / section + ~NASControllerFixture() + { + controller.stop(); + sEventBroker->unsubscribe(&controller); + sEventBroker->clearDelayedEvents(); + } + +protected: + MockIMU mock_imu; + MockPressureSensor mock_baro; + MockGPS mock_gps; + + NASCtrl controller; +}; + +TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from idle") +{ + // controller.transition(&NASCtrl::state_idle); + + SECTION("EV_CALIBRATE_NAS -> CALIBRATING") + { + REQUIRE(testFSMTransition(controller, Event{EV_CALIBRATE_NAS}, + &NASCtrl::state_calibrating)); + } +} + +TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from calibrating") +{ + controller.transition(&NASCtrl::state_calibrating); + + SECTION("EV_CALIBRATE_NAS -> CALIBRATING") + { + REQUIRE(testFSMTransition(controller, Event{EV_CALIBRATE_NAS}, + &NASCtrl::state_calibrating)); + } + + SECTION("EV_NAS_READY -> READY") + { + REQUIRE(testFSMTransition(controller, Event{EV_NAS_READY}, + &NASCtrl::state_ready)); + } +} + +TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from ready") +{ + controller.transition(&NASCtrl::state_ready); + + SECTION("EV_LIFTOFF -> ACTIVE") + { + REQUIRE(testFSMTransition(controller, Event{EV_LIFTOFF}, + &NASCtrl::state_active)); + } + + SECTION("EV_CALIBRATE_NAS -> CALIBRATING") + { + REQUIRE(testFSMTransition(controller, Event{EV_CALIBRATE_NAS}, + &NASCtrl::state_calibrating)); + } +} + +TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from active") +{ + controller.transition(&NASCtrl::state_active); + + SECTION("EV_LANDED -> END") + { + REQUIRE(testFSMTransition(controller, Event{EV_LANDED}, + &NASCtrl::state_end)); + } +} + +TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from end") +{ + controller.transition(&NASCtrl::state_end); +} \ No newline at end of file diff --git a/src/tests/catch/fsm/test-tmtc.cpp b/src/tests/catch/fsm/test-tmtc.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9cfb17ac86c2af960f0a559095170c0ee194aae9 --- /dev/null +++ b/src/tests/catch/fsm/test-tmtc.cpp @@ -0,0 +1,131 @@ +/* + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch/catch-tests-entry.cpp" +#endif + +// We need access to the handleEvent(...) function in state machines in order to +// test them synchronously + +#include <miosix.h> + +#include <utils/testutils/catch.hpp> + +#define private public +#define protected public + +#include <TelemetriesTelecommands/TMTCController.h> + +#include "events/Events.h" +#include "utils/testutils/TestHelper.h" + +using miosix::Thread; +using namespace DeathStackBoard; + +class TMTCFixture +{ +public: + // This is called at the beginning of each test / section + TMTCFixture() + { + sEventBroker->start(); + fsm = new TMTCController(); + fsm->start(); + } + + // This is called at the end of each test / section + ~TMTCFixture() + { + fsm->stop(); + sEventBroker->unsubscribe(fsm); + sEventBroker->clearDelayedEvents(); + delete fsm; + } + +protected: + TMTCController* fsm; +}; + +TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateGroundTM") +{ + SECTION("EV_TC_START_SENSOR_TM -> stateSensorTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_TC_START_SENSOR_TM}, + &TMTCController::stateSensorTM)); + } + + SECTION("EV_ARMED -> stateSensorTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_ARMED}, + &TMTCController::stateFlightTM)); + } + + SECTION("EV_LIFTOFF -> stateFlightTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_LIFTOFF}, + &TMTCController::stateFlightTM)); + } + + SECTION("EV_TC_SERIAL_TM -> stateSerialDebugTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_TC_SERIAL_TM}, + &TMTCController::stateSerialDebugTM)); + } +} + +TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateSensorTM") +{ + fsm->transition(&TMTCController::stateSensorTM); + Thread::sleep(100); + + SECTION("EV_TC_STOP_SENSOR_TM -> stateGroundTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_TC_STOP_SENSOR_TM}, + &TMTCController::stateGroundTM)); + } +} + +TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateFlightTM") +{ + fsm->transition(&TMTCController::stateFlightTM); + Thread::sleep(100); + + SECTION("EV_DISARMED -> stateGroundTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_DISARMED}, + &TMTCController::stateGroundTM)); + } +} + +TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateSerialDebugTM") +{ + fsm->transition(&TMTCController::stateSerialDebugTM); + Thread::sleep(100); + + SECTION("EV_TC_RADIO_TM -> stateSerialDebugTM") + { + REQUIRE(testFSMTransition(*fsm, Event{EV_TC_RADIO_TM}, + &TMTCController::stateGroundTM)); + } +} \ No newline at end of file diff --git a/src/tests/catch/nas/test-nas-simulation.cpp b/src/tests/catch/nas/test-nas-simulation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..38b39bcd83f7fdff9283ee33a2787d52a7485fdd --- /dev/null +++ b/src/tests/catch/nas/test-nas-simulation.cpp @@ -0,0 +1,160 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch/catch-tests-entry.cpp" +#endif + +#include <utils/testutils/catch.hpp> + +#define private public + +#define EIGEN_NO_MALLOC // enable eigen malloc usage assert + +#include <NavigationAttitudeSystem/NASController.h> +#include <miosix.h> +#include <mocksensors/MockSensors.h> + +#include "events/Events.h" +#include "utils/testutils/TestHelper.h" + +using namespace DeathStackBoard; +using namespace NASConfigs; + +MockIMU mock_imu; +MockPressureSensor mock_baro; +MockGPS mock_gps; + +using NASCtrl = NASController<MockIMUData, PressureData, GPSData>; + +NASCtrl controller(mock_imu, mock_baro, mock_gps); + +void sampleSensors() +{ + mock_imu.sample(); + mock_baro.sample(); + mock_gps.sample(); +} + +void signalLiftoff() +{ + mock_baro.signalLiftoff(); + mock_imu.signalLiftoff(); + mock_gps.signalLiftoff(); +} + +TEST_CASE("Testing Navigation System Controller") +{ + TimestampTimer::enableTimestampTimer(); + + sEventBroker->start(); + controller.start(); + + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_NAS); + + // Startup: we should be in idle + Thread::sleep(100); + REQUIRE(controller.testState(&NASCtrl::state_idle)); + + // Enter Calibrating and REQUIRE + sEventBroker->post(Event{EV_CALIBRATE_NAS}, TOPIC_NAS); + Thread::sleep(100); + REQUIRE(controller.testState(&NASCtrl::state_calibrating)); + + Thread::sleep(100); + + // sample some data for calibration + for (uint32_t i = 0; i < CALIBRATION_N_SAMPLES; i++) + { + sampleSensors(); + Thread::sleep(10); + controller.update(); + } + + // check calibrator values : + // they should be equal to the last sample of each sensor + // i.e. the first element of the arrays from which the sensors get the data + PressureData press_data = + mock_baro.getLastSample(); // still before liftoff + // (same as SIMULATED_PRESSURE[0]) + GPSData gps_data = mock_gps.getLastSample(); // still before liftoff ... + MockIMUData imu_data = + mock_imu.getLastSample(); // still before liftoff ... + NASReferenceValues ref_values{ + press_data.press, gps_data.latitude, gps_data.longitude, + imu_data.accel_x, imu_data.accel_y, imu_data.accel_z, + imu_data.mag_x, imu_data.mag_y, imu_data.mag_z}; + REQUIRE(ref_values == controller.calibrator.getReferenceValues()); + + // Now we should be in ready + sampleSensors(); + controller.update(); + Thread::sleep(100); + REQUIRE(controller.testState(&NASCtrl::state_ready)); + + Thread::sleep(100); + + // retry the calibration phase + sEventBroker->post({EV_CALIBRATE_NAS}, TOPIC_NAS); + Thread::sleep(100); + REQUIRE(controller.testState(&NASCtrl::state_calibrating)); + // sample some data for calibration + for (uint32_t i = 0; i < CALIBRATION_N_SAMPLES + 10; i++) + { + sampleSensors(); + Thread::sleep(10); + controller.update(); + } + // check calibrator values : + // they should be equal to the last sample of each sensor + // i.e. the first element of the arrays from which the sensors get the data + press_data = mock_baro.getLastSample(); // still before liftoff + // (same as SIMULATED_PRESSURE[0]) + gps_data = mock_gps.getLastSample(); // still before liftoff ... + imu_data = mock_imu.getLastSample(); // still before liftoff ... + ref_values = NASReferenceValues{press_data.press, gps_data.latitude, gps_data.longitude, + imu_data.accel_x, imu_data.accel_y, imu_data.accel_z, + imu_data.mag_x, imu_data.mag_y, imu_data.mag_z}; + REQUIRE(ref_values == controller.calibrator.getReferenceValues()); + + // Now we should be in ready + sampleSensors(); + controller.update(); + Thread::sleep(100); + REQUIRE(controller.testState(&NASCtrl::state_ready)); + + // Send liftoff event: should be in state active + sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + signalLiftoff(); + Thread::sleep(100); + REQUIRE(controller.testState(&NASCtrl::state_active)); + + // test the NAS on the mock sensors' data + for (uint32_t i = 0; i < 2570; i++) + { + TRACE("Iteration: %u \n", i); + sampleSensors(); + // Thread::sleep(10); + controller.update(); + } +} diff --git a/src/tests/deathstack-boards/test-analog-board.cpp b/src/tests/deathstack-boards/test-analog-board.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bc8dcd0643104de41ef2084c0c5f74c6e324fc44 --- /dev/null +++ b/src/tests/deathstack-boards/test-analog-board.cpp @@ -0,0 +1,267 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/** + * This code allows testing of the whole analog board of the DeathStackX. + * + * When sampling all sensors the analog pressure sensors are sampled at 1/4 of + * the configured frequency (due to the ads1118). + * + * Components on the analog board: + * + * Analog sensors: + * MPXHZ6130A (x2): Absolute pressure sensor + * SSCDANN030PAA: Absolute pressure sensor with a 0-206kPa range + * SSCDRRN015PDA: Differential pressure sensor with a ±103kPa range + * MS5803: Digital absolute pressure sensor + * Deatachment pins + */ + +#include <Common.h> +#include <drivers/adc/ADS1118/ADS1118.h> +#include <interfaces-impl/hwmapping.h> +#include <miosix.h> +#include <sensors/MS5803/MS5803.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h> + +#include <iostream> +#include <sstream> + +#include "PinHandler/PinHandler.h" + +namespace PinHandlerTest +{ +#include "../test-pinhandler.cpp" +} + +// Pressure sensors mappings +constexpr ADS1118::ADS1118Mux NXP_1 = ADS1118::ADS1118Mux::MUX_AIN0_GND; +constexpr ADS1118::ADS1118Mux NXP_2 = ADS1118::ADS1118Mux::MUX_AIN3_GND; +constexpr ADS1118::ADS1118Mux HONEYWELL_1 = ADS1118::ADS1118Mux::MUX_AIN2_GND; +constexpr ADS1118::ADS1118Mux HONEYWELL_2 = ADS1118::ADS1118Mux::MUX_AIN1_GND; + +// Sample frequency +constexpr int SAMPLING_FREQUENCY = 50; + +// Voltage supplied to the analog sensors +constexpr int SUPPLIED_VOLTAGE = 5; // Measure and change! + +int menu(); +int askSeconds(); +template <typename PressureSensor> +void sampleAnalogPressureSensor(ADS1118::ADS1118Mux channel); +void sampleMS5803(); +void sampleAll(); + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + switch (menu()) + { + case 1: + sampleAnalogPressureSensor<MPXHZ6130A>(NXP_1); + break; + case 2: + sampleAnalogPressureSensor<MPXHZ6130A>(NXP_2); + break; + case 3: + sampleAnalogPressureSensor<SSCDANN030PAA>(HONEYWELL_1); + break; + case 4: + sampleAnalogPressureSensor<SSCDRRN015PDA>(HONEYWELL_2); + break; + case 5: + sampleMS5803(); + break; + case 6: + sampleAll(); + break; + case 7: + PinHandlerTest::main(); + break; + + default: + break; + } + + return 0; +} + +int menu() +{ + int choice; + + printf("\n\nWhat do you want to do?\n"); + printf("1. Sample NXP 1 (absolute)\n"); + printf("2. Sample NXP 2 (not installed)\n"); + printf("3. Sample HONEYWELL 1 (absolute)\n"); + printf("4. Sample HONEYWELL 2 (differential)\n"); + printf("5. Sample MS5803\n"); + printf("6. Sample all the above\n"); + printf("7. Test detachment pins\n"); + printf("\n>> "); + scanf("%d", &choice); + + return choice; +} + +int askSeconds() +{ + int seconds; + + printf("How many seconds the test should run?\n"); + printf("\n>> "); + scanf("%d", &seconds); + + return seconds; +} + +template <typename PressureSensor> +void sampleAnalogPressureSensor(ADS1118::ADS1118Mux channel) +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + SPIBus spiBus(SPI1); + SPIBusConfig spiConfig = ADS1118::getDefaultSPIConfig(); + spiConfig.clock_div = SPIClockDivider::DIV64; + SPISlave spiSlave(spiBus, miosix::sensors::ads1118::cs::getPin(), + spiConfig); + + ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; + ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; + ADS1118 ads1118 = ADS1118(spiSlave, ads1118Config); + ads1118.init(); + ads1118.enableInput(channel, ADS1118::ADS1118DataRate::DR_860, + ADS1118::ADS1118Pga::FSR_6_144); + + std::function<ADCData()> get_voltage_function = + std::bind(&ADS1118::getVoltage, &ads1118, channel); + + PressureSensor pressureSensor(get_voltage_function, SUPPLIED_VOLTAGE); + + // Sampling + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + ads1118.sample(); + pressureSensor.sample(); + printf("%llu,%.2f\n", pressureSensor.getLastSample().press_timestamp, + pressureSensor.getLastSample().press); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} + +void sampleMS5803() +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + SPIBusConfig spiCfg{}; + spiCfg.clock_div = SPIClockDivider::DIV16; + SPIBus spiBus(SPI1); + SPISlave spiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), spiCfg); + MS5803 ms5803 = MS5803(spiSlave); + ms5803.init(); + + // Sampling + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + ms5803.sample(); + printf("%.2f\n", ms5803.getLastSample().press); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} + +void sampleAll() +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + SPIBus spiBus(SPI1); + SPIBusConfig adsSpiConfig = ADS1118::getDefaultSPIConfig(); + adsSpiConfig.clock_div = SPIClockDivider::DIV64; + SPISlave adsSpiSlave(spiBus, miosix::sensors::ads1118::cs::getPin(), + adsSpiConfig); + + ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; + ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; + ADS1118 ads1118 = ADS1118(adsSpiSlave, ads1118Config); + ads1118.init(); + ads1118.enableInput(NXP_1, ADS1118::ADS1118DataRate::DR_860, + ADS1118::ADS1118Pga::FSR_6_144); + ads1118.enableInput(NXP_2, ADS1118::ADS1118DataRate::DR_860, + ADS1118::ADS1118Pga::FSR_6_144); + ads1118.enableInput(HONEYWELL_1, ADS1118::ADS1118DataRate::DR_860, + ADS1118::ADS1118Pga::FSR_6_144); + ads1118.enableInput(HONEYWELL_2, ADS1118::ADS1118DataRate::DR_860, + ADS1118::ADS1118Pga::FSR_6_144); + + std::function<ADCData()> get_voltage_function_nxp_1 = + std::bind(&ADS1118::getVoltage, &ads1118, NXP_1); + std::function<ADCData()> get_voltage_function_nxp_2 = + std::bind(&ADS1118::getVoltage, &ads1118, NXP_2); + std::function<ADCData()> get_voltage_function_honeywell_1 = + std::bind(&ADS1118::getVoltage, &ads1118, HONEYWELL_1); + std::function<ADCData()> get_voltage_function_honeywell_2 = + std::bind(&ADS1118::getVoltage, &ads1118, HONEYWELL_2); + + MPXHZ6130A npx1(get_voltage_function_nxp_1, SUPPLIED_VOLTAGE); + MPXHZ6130A npx2(get_voltage_function_nxp_2, SUPPLIED_VOLTAGE); + SSCDANN030PAA honeywell1(get_voltage_function_honeywell_1, + SUPPLIED_VOLTAGE); + SSCDRRN015PDA honeywell2(get_voltage_function_honeywell_2, + SUPPLIED_VOLTAGE); + + SPIBusConfig ms5803SpiCfg{}; + ms5803SpiCfg.clock_div = SPIClockDivider::DIV16; + SPISlave ms5803SpiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), ms5803SpiCfg); + MS5803 ms5803 = MS5803(ms5803SpiSlave); + ms5803.init(); + + // Sampling + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + ads1118.sample(); + npx1.sample(); + npx2.sample(); + honeywell1.sample(); + honeywell2.sample(); + ms5803.sample(); + printf("%llu,%.2f,%.2f,%.2f,%.2f,%.2f\n", + ads1118.getLastSample().adc_timestamp, + npx1.getLastSample().press, npx2.getLastSample().press, + honeywell1.getLastSample().press, + honeywell2.getLastSample().press, ms5803.getLastSample().press); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} diff --git a/src/tests/deathstack-boards/test-power-board.cpp b/src/tests/deathstack-boards/test-power-board.cpp new file mode 100644 index 0000000000000000000000000000000000000000..acc36ed2f7349e3fce6f96a19ab367a2692ec70b --- /dev/null +++ b/src/tests/deathstack-boards/test-power-board.cpp @@ -0,0 +1,148 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/** + * Components on the power board: + * + * Analog input: Battery meter + * HBridge (x2): Cutters (primary & backup) + * Servo (x2): Deployment & airbrakes + */ + +#include <AirBrakes/AirBrakesServo.h> +#include <Common.h> +#include <Deployment/DeploymentServo.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/hbridge/HBridge.h> +#include <drivers/servo/servo.h> +#include <sensors/analog/battery/BatteryVoltageSensor.h> + +#include <cstdio> +#include <iostream> +#include <sstream> +#include <string> + +namespace CutterTest +{ +#include "../drivers/test-cutter.cpp" +} + +namespace ServoTest +{ +#include "../drivers/test-servo.cpp" +} + +using namespace std; + +// Sample frequency +constexpr int SAMPLING_FREQUENCY = 20; + +int menu(); +int askSeconds(); +void sampleBatteryVoltage(); + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + switch (menu()) + { + case 1: + sampleBatteryVoltage(); + break; + case 2: + CutterTest::main(); + break; + case 3: + ServoTest::main(); + break; + + default: + break; + } + + return 0; +} + +int menu() +{ + string temp; + int choice; + + printf("\n\nWhat do you want to do?\n"); + printf("1. Sample battery voltage\n"); + printf("2. Test cutters\n"); + printf("3. Test servos\n"); + printf("\n>> "); + getline(cin, temp); + stringstream(temp) >> choice; + + return choice; +} + +int askSeconds() +{ + int seconds; + + printf("How many seconds the test should run?\n"); + printf("\n>> "); + scanf("%d", &seconds); + + return seconds; +} + +void sampleBatteryVoltage() +{ + // Set the clock divider for the analog circuitry (/8) + ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1; + + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + InternalADC internalADC = InternalADC(*ADC3, 3.3); + internalADC.enableChannel(InternalADC::CH5); + internalADC.init(); + + std::function<ADCData()> get_voltage_function = + std::bind(&InternalADC::getVoltage, &internalADC, InternalADC::CH5); + + BatteryVoltageSensor batterySensor(get_voltage_function, 5.98); + + // Sampling + printf("adc_timestamp,gpio_voltage,bat_voltage,battery_percentage\n"); + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + internalADC.sample(); + batterySensor.sample(); + BatteryVoltageSensorData data = batterySensor.getLastSample(); + + // Calculate a simple linear battery percentage + float batteryPercentage = 100 * (data.bat_voltage - 9.6) / (12.6 - 9.6); + + printf("%llu,%.3f,%.3f,%.1f\n", data.adc_timestamp, data.voltage, + data.bat_voltage, batteryPercentage); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} diff --git a/src/tests/deathstack-boards/test-rf-board.cpp b/src/tests/deathstack-boards/test-rf-board.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6448e6d7a9158fd18b5d5216b33c21e0561de407 --- /dev/null +++ b/src/tests/deathstack-boards/test-rf-board.cpp @@ -0,0 +1,260 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/** + * Components on the rf board: + * + * LIS3MDL: Magnetometer + * ASM330LHH: Accelerometer and gyroscope + * LSM9DS1: Accelerometer, gyroscope and magnetometer + * BMX160: Accelerometer and gyroscope + * GPS + * SD card + */ + +#include <Common.h> +#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/spi/SPIDriver.h> +#include <interfaces-impl/hwmapping.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/LIS3MDL/LIS3MDL.h> + +#include <ctime> +#include <iostream> +#include <sstream> +#include <vector> + +#include "math/Stats.h" + +namespace SDCardBenchmark +{ +#include "../../skyward-boardcore/src/entrypoints/sdcard-benchmark.cpp" +} + +// Sample frequency +constexpr int SAMPLING_FREQUENCY = 100; + +int menu(); +int askSeconds(); +void sampleLIS3MDL(); +// void sampleASM330LHH(); +// void sampleLSM9DSI(); +void sampleBMX160(); +void sampleAll(); +void sampleGPS(); + +int main() +{ + switch (menu()) + { + case 1: + sampleLIS3MDL(); + break; + case 2: + printf("Component not currently installed!\n"); + // sampleASM330LHH(); + break; + case 3: + printf("Component not currently installed!\n"); + // sampleLSM9DSI(); + break; + case 4: + sampleAll(); + break; + case 5: + sampleBMX160(); + break; + case 6: + SDCardBenchmark::main(); + break; + + default: + break; + } + + return 0; +} + +int menu() +{ + int choice; + + printf("\n\nWhat do you want to do?\n"); + printf("1. Sample LIS3MDL\n"); + printf("2. Sample ASM330LHH (not installed)\n"); + printf("3. Sample LSM9DSI (not installed)\n"); + printf("4. Sample all the above\n"); + printf("5. Sample BMX160 (gps)\n"); + printf("6. SD Card benchmark\n"); + printf("\n>> "); + scanf("%d", &choice); + + return choice; +} + +int askSeconds() +{ + int seconds; + + printf("How many seconds the test should run?\n"); + printf("\n>> "); + scanf("%d", &seconds); + + return seconds; +} + +void sampleLIS3MDL() +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + SPIBus spiBus(SPI1); + SPIBusConfig spiConfig; + spiConfig.clock_div = SPIClockDivider::DIV32; + spiConfig.mode = SPIMode::MODE1; + + LIS3MDL::Config lis3mdlConfig; + lis3mdlConfig.odr = LIS3MDL::ODR_560_HZ; + lis3mdlConfig.scale = LIS3MDL::FS_16_GAUSS; + lis3mdlConfig.temperatureDivider = 5; + + LIS3MDL lis3mdl(spiBus, miosix::sensors::lis3mdl::cs::getPin(), spiConfig, + lis3mdlConfig); + lis3mdl.init(); + + // Sampling + std::cout << LIS3MDLData::header(); + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + lis3mdl.sample(); + LIS3MDLData data = lis3mdl.getLastSample(); + + data.print(std::cout); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} + +void sampleBMX160() +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + SPIBus spiBus(SPI1); + + BMX160Config bmx160Config; + bmx160Config.fifo_mode = BMX160Config::FifoMode::DISABLED; + bmx160Config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx160Config.temp_divider = 1; + + BMX160 bmx160 = + BMX160(spiBus, miosix::sensors::bmx160::cs::getPin(), bmx160Config); + bmx160.init(); + + // Sampling + std::cout << BMX160Data::header(); + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + bmx160.sample(); + BMX160Data data = bmx160.getLastSample(); + + data.print(std::cout); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} + +void sampleAll() +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + SPIBus spiBus(SPI1); + SPIBusConfig spiConfig; + spiConfig.clock_div = SPIClockDivider::DIV32; + spiConfig.mode = SPIMode::MODE1; + + LIS3MDL::Config lis3mdlConfig; + lis3mdlConfig.odr = LIS3MDL::ODR_560_HZ; + lis3mdlConfig.scale = LIS3MDL::FS_16_GAUSS; + lis3mdlConfig.temperatureDivider = 5; + + LIS3MDL lis3mdl(spiBus, miosix::sensors::lis3mdl::cs::getPin(), spiConfig, + lis3mdlConfig); + lis3mdl.init(); + + BMX160Config bmx160Config; + bmx160Config.fifo_mode = BMX160Config::FifoMode::DISABLED; + bmx160Config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx160Config.temp_divider = 1; + + BMX160 bmx160 = + BMX160(spiBus, miosix::sensors::bmx160::cs::getPin(), bmx160Config); + bmx160.init(); + + // Sampling + for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) + { + lis3mdl.sample(); + LIS3MDLData lis3mdlData = lis3mdl.getLastSample(); + bmx160.sample(); + BMX160Data bmx160Data = bmx160.getLastSample(); + + lis3mdlData.print(std::cout); + bmx160Data.print(std::cout); + + miosix::delayMs(1000 / SAMPLING_FREQUENCY); + } +} + +void sampleGPS() +{ + // Ask the user how many second the test should be perfomed + int seconds = askSeconds(); + + // Sensor setup + + UbloxGPS gps(38400); + gps.init(); + miosix::delayMs(200); + gps.start(); + // gps.sendSBASMessage(); + + // Sampling + std::cout << UbloxGPSData::header(); + for (int i = 0; i < seconds; i++) + { + gps.sample(); + UbloxGPSData data = gps.getLastSample(); + + data.print(std::cout); + + miosix::delayMs(1000); + } +} diff --git a/src/tests/deathstack-boards/test-stm-board.cpp b/src/tests/deathstack-boards/test-stm-board.cpp new file mode 100644 index 0000000000000000000000000000000000000000..053619d503859e4b12084b130a24f3d77c3a30a0 --- /dev/null +++ b/src/tests/deathstack-boards/test-stm-board.cpp @@ -0,0 +1,98 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/** + * Components on the stm board: + * + * STM32F429: + * Led wave + * External oscillator + */ + +#include <Common.h> + +#include <iostream> +#include <sstream> + +using namespace std; + +namespace LedWave +{ +#include "../ledwave.cpp" +} + +namespace HSE +{ +#include "../test-hse.cpp" +} + +// Sample frequency +constexpr int SAMPLING_FREQUENCY = 100; + +int menu(); +int askSeconds(); + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + switch (menu()) + { + case 1: + LedWave::main(); + break; + case 2: + HSE::main(); + break; + + default: + break; + } + + return 0; +} + +int menu() +{ + string temp; + int choice; + + printf("\n\nWhat do you want to do?\n"); + printf("1. Ledwave\n"); + printf("2. External oscillator\n"); + printf("\n>> "); + getline(cin, temp); + stringstream(temp) >> choice; + + return choice; +} + +int askSeconds() +{ + int seconds; + + printf("How many seconds the test should run?\n"); + printf("\n>> "); + scanf("%d", &seconds); + + return seconds; +} diff --git a/src/tests/deployment/test-deployment-interactive.cpp b/src/tests/deployment/test-deployment-interactive.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eba886c72a164f7ff7e8ea3f24b846dd550a240f --- /dev/null +++ b/src/tests/deployment/test-deployment-interactive.cpp @@ -0,0 +1,501 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Deployment/DeploymentController.h> +#include <Deployment/DeploymentServo.h> +#include <logger/Logger.h> +#include <miosix.h> + +#include <cstdio> +#include <iostream> +#include <sstream> +#include <string> + +#include "configs/CutterConfig.h" +#include "events/Events.h" +#include "utils/testutils/TestHelper.h" + +using namespace DeathStackBoard; + +using namespace std; + +void cuttingSequence(); +void cutterContinuity(); +void noseconeEjection(); +void wiggleServo(); +void setServoFullyOpen(); +void setServoFullyClose(); +void resetServo(); +void manualServoControl(); +void setServoParameters(); +void setPrimaryCutterParameters(); +void setBackupCutterParameters(); +void resetServoParameters(); +void resetPrimaryCutterParameters(); +void resetBackupCutterParameters(); + +void waitUserInput(); + +float minPosition = DeploymentConfigs::DPL_SERVO_MIN_POS; +float maxPosition = DeploymentConfigs::DPL_SERVO_MAX_POS; +float resetPosition = DeploymentConfigs::DPL_SERVO_RESET_POS; + +unsigned int primaryCutterFrequency = + CutterConfig::PRIMARY_CUTTER_PWM_FREQUENCY; +float primaryCutterDutyCycle = CutterConfig::PRIMARY_CUTTER_PWM_DUTY_CYCLE; +unsigned int backupCutterFrequency = CutterConfig::BACKUP_CUTTER_PWM_FREQUENCY; +float backupCutterDutyCycle = CutterConfig::BACKUP_CUTTER_PWM_DUTY_CYCLE; +float cutterTestDutyCycle = primaryCutterDutyCycle / 100.0f; + +int main() +{ + // avoid servo to move while resetting the board + DeploymentServo ejection_servo; + ejection_servo.enable(); + ejection_servo.reset(); + + sEventBroker->start(); + + string temp; + for (;;) + { + int choice; + cout << "\n\nWhat do you want to do?: \n"; + cout << "1. Test cutting sequence\n"; + cout << "2. Continuity cutter test (non destructive)\n"; + cout << "3. Nosecone ejection\n"; + cout << "4. Servo wiggle (self test)\n"; + cout << "5. Servo fully open\n"; + cout << "6. Servo fully close\n"; + cout << "7. Servo reset\n"; + cout << "8. Servo manual control\n"; + cout << "9. Set servo parameters\n"; + cout << "10. Set primary cutter parameters\n"; + cout << "11. Set backup cutter parameters\n"; + cout << "12. Reset servo parameters (default)\n"; + cout << "13. Reset primary cutter parameters (default)\n"; + cout << "14. Reset backup cutter parameters (default)\n"; + + getline(cin, temp); + stringstream(temp) >> choice; + + switch (choice) + { + case 1: + cuttingSequence(); + break; + case 2: + cutterContinuity(); + break; + case 3: + noseconeEjection(); + break; + case 4: + wiggleServo(); + break; + case 5: + setServoFullyOpen(); + break; + case 6: + setServoFullyClose(); + break; + case 7: + resetServo(); + break; + case 8: + manualServoControl(); + break; + case 9: + setServoParameters(); + break; + case 10: + setPrimaryCutterParameters(); + break; + case 11: + setBackupCutterParameters(); + break; + case 12: + resetServoParameters(); + break; + case 13: + resetPrimaryCutterParameters(); + break; + case 14: + resetBackupCutterParameters(); + break; + default: + cout << "Invalid option\n"; + break; + } + } +} + +int waitEventTimeout = 10 * 1000; + +void cuttingSequence() +{ + cout << "\n\n** CUTTER SEQUENCE TEST **\n\n"; + + waitUserInput(); + + // DeploymentController initialization + HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM, + CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency, + primaryCutterDutyCycle}; + HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM, + CUTTER_CHANNEL_BACKUP, backupCutterFrequency, + backupCutterDutyCycle}; + DeploymentServo ejection_servo; + + DeploymentController deploymentController{&primaryCutter, &backupCutter, + &ejection_servo}; + + deploymentController.start(); + + Thread::sleep(1000); + sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DPL); + + waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL, waitEventTimeout); + + cout << "Primary cutter is done.\n"; + + waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL, waitEventTimeout); + cout << "Backup cutter is done.\n"; + + deploymentController.stop(); + + cout << "\n\tTest finished!\n\n"; +} + +void cutterContinuity() +{ + string temp; + int cutter; + + cout << "\n\n** CONTINUITY CUTTER TEST **\n\n"; + cout << "Non-destructive cutter test : duty cycle inserted is divided by " + "100\n\n"; + do + { + cout << "Which cutter to test? (1-primary / 2-backup)\n"; + + getline(cin, temp); + stringstream(temp) >> cutter; + } while (cutter != 1 && cutter != 2); + + waitUserInput(); + + // DeploymentController initialization + HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM, + CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency, + cutterTestDutyCycle}; + HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM, + CUTTER_CHANNEL_BACKUP, backupCutterFrequency, + cutterTestDutyCycle}; + DeploymentServo ejection_servo; + DeploymentController deploymentController{&primaryCutter, &backupCutter, + &ejection_servo}; + + deploymentController.start(); + + if (cutter == 1) + { + cout << "Activating primary cutter...\n"; + sEventBroker->post({EV_TEST_CUT_PRIMARY}, TOPIC_DPL); + } + else + { + cout << "Activating backup cutter...\n"; + sEventBroker->post({EV_TEST_CUT_BACKUP}, TOPIC_DPL); + } + + waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL); + + cout << "Cutter test is done.\n"; + + deploymentController.stop(); + + cout << "\n\tTest finished!\n\n"; +} + +void noseconeEjection() +{ + cout << "\n\n** NOSECONE EJECTION TEST **\n\n"; + + waitUserInput(); + + EventCounter counter{*sEventBroker}; + + HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM, + CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency, + primaryCutterDutyCycle}; + HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM, + CUTTER_CHANNEL_BACKUP, backupCutterFrequency, + backupCutterDutyCycle}; + DeploymentServo ejection_servo; + DeploymentController deploymentController{&primaryCutter, &backupCutter, + &ejection_servo}; + + counter.subscribe(TOPIC_DPL); + counter.start(); + + cout << "Nosecone ejection in\n"; + for (int i = 3; i > 0; i--) + { + cout << i << "\n"; + Thread::sleep(1000); + } + + deploymentController.start(); + sEventBroker->post({EV_NC_OPEN}, TOPIC_DPL); + + for (;;) + { + if (counter.getCount(EV_NC_OPEN_TIMEOUT) == 1) + break; + if (counter.getCount(EV_NC_DETACHED) >= 1) + break; + Thread::sleep(10); + } + + cout << "Nosecone ejection test is done.\n"; + + deploymentController.stop(); + counter.stop(); + + cout << "\n\tTest finished!\n\n"; +} + +void wiggleServo() +{ + cout << "\n\n** WIGGLE SERVO **\n\n"; + + waitUserInput(); + + DeploymentServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.reset(); + + cout << "Wiggling ...\n"; + servo.selfTest(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tTest finished!\n\n"; +} + +void setServoFullyOpen() +{ + cout << "\n\n** SERVO FULLY OPEN **\n\n"; + + waitUserInput(); + + DeploymentServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.setMaxPosition(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tTest finished!\n\n"; +} + +void setServoFullyClose() +{ + cout << "\n\n** SERVO FULLY CLOSE **\n\n"; + + waitUserInput(); + + DeploymentServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.setMinPosition(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tTest finished!\n\n"; +} + +void resetServo() +{ + cout << "\n\n** RESET SERVO **\n\n"; + + waitUserInput(); + + DeploymentServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.reset(); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tTest finished!\n\n"; +} + +void manualServoControl() +{ + cout << "\n\n** MANUAL SERVO CONTROL **\n\n"; + + string temp; + float angle; + + do + { + cout << "Write the servo postion in degrees:\n"; + getline(cin, temp); + stringstream(temp) >> angle; + } while (angle < minPosition || angle > maxPosition); + + DeploymentServo servo{minPosition, maxPosition, resetPosition}; + + servo.enable(); + servo.set(angle); + Thread::sleep(1000); + servo.disable(); + + cout << "\n\tTest finished!\n\n"; +} + +void setServoParameters() +{ + cout << "\n\n** SET SERVO PARAMETERS **\n\n"; + + string temp; + do + { + cout << "Write the servo minimum postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> minPosition; + } while (minPosition < 0 || minPosition > 180.0f); + + do + { + cout << "Write the servo maximum postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> maxPosition; + } while (maxPosition < 0 || maxPosition > 180.0f); + + do + { + cout << "Write the servo reset postion (degrees):\n"; + getline(cin, temp); + stringstream(temp) >> resetPosition; + } while (resetPosition < 0 || resetPosition > 180.0f); + + cout << "Configured servo parameteres:\n"; + cout << "\tminimum position: " << minPosition << "\n"; + cout << "\tmaximum position: " << maxPosition << "\n"; + cout << "\treset position: " << resetPosition << "\n"; + + // Reset servo + DeploymentServo servo{minPosition, maxPosition, resetPosition}; + servo.enable(); + servo.reset(); + Thread::sleep(1000); + servo.disable(); +} + +void setPrimaryCutterParameters() +{ + cout << "\n\n** SET PRIMARY CUTTER PARAMETERS **\n\n"; + + string temp; + + cout << "Write the primary cutter frequency in hertz:\n"; + getline(cin, temp); + stringstream(temp) >> primaryCutterFrequency; + + do + { + cout << "Write the primary cutter duty cycle in %:\n"; + getline(cin, temp); + stringstream(temp) >> primaryCutterDutyCycle; + primaryCutterDutyCycle /= 100; + } while (primaryCutterDutyCycle < 0 || primaryCutterDutyCycle > 1.0F); + + cout << "Configured primary cutter parameteres:\n"; + cout << "\tfrequency: " << primaryCutterFrequency << "Hz\n"; + cout << "\tduty cycle: " << primaryCutterDutyCycle * 100 << "%\n"; +} + +void setBackupCutterParameters() +{ + cout << "\n\n** SET BACKUP CUTTER PARAMETERS **\n\n"; + + string temp; + + cout << "Write the backup cutter frequency in hertz:\n"; + getline(cin, temp); + stringstream(temp) >> backupCutterFrequency; + + do + { + cout << "Write the backup cutter duty cycle in %:\n"; + getline(cin, temp); + stringstream(temp) >> backupCutterDutyCycle; + backupCutterDutyCycle /= 100; + } while (backupCutterDutyCycle < 0 || backupCutterDutyCycle > 1.0F); + + cout << "Configured backup cutter parameteres:\n"; + cout << "\tfrequency: " << backupCutterFrequency << "Hz\n"; + cout << "\tduty cycle: " << backupCutterDutyCycle * 100 << "%\n"; +} + +void resetServoParameters() +{ + minPosition = DeploymentConfigs::DPL_SERVO_MIN_POS; + maxPosition = DeploymentConfigs::DPL_SERVO_MAX_POS; + resetPosition = DeploymentConfigs::DPL_SERVO_MIN_POS; + + cout << "Configured servo parameteres (default):\n"; + cout << "\tminimum position: " << minPosition << "\n"; + cout << "\tmaximum position: " << maxPosition << "\n"; + cout << "\treset position: " << resetPosition << "\n"; +} + +void resetPrimaryCutterParameters() +{ + primaryCutterFrequency = CutterConfig::PRIMARY_CUTTER_PWM_FREQUENCY; + primaryCutterDutyCycle = CutterConfig::PRIMARY_CUTTER_PWM_DUTY_CYCLE; + + cout << "Configured primary cutter parameteres:\n"; + cout << "\tfrequency: " << primaryCutterFrequency << "Hz\n"; + cout << "\tduty cycle: " << primaryCutterDutyCycle * 100 << "%\n"; +} + +void resetBackupCutterParameters() +{ + backupCutterFrequency = CutterConfig::BACKUP_CUTTER_PWM_FREQUENCY; + backupCutterDutyCycle = CutterConfig::BACKUP_CUTTER_PWM_DUTY_CYCLE; + + cout << "Configured backup cutter parameteres:\n"; + cout << "\tfrequency: " << backupCutterFrequency << "Hz\n"; + cout << "\tduty cycle: " << backupCutterDutyCycle * 100 << "%\n"; +} + +void waitUserInput() +{ + string temp; + + do + { + cout << "Write 'start' to begin the test:\n"; + getline(cin, temp); + } while (temp != "start"); +} \ No newline at end of file diff --git a/src/tests/drivers/test-all-sensors.cpp b/src/tests/drivers/test-all-sensors.cpp index 17fbcd8a93c675350e645218da2ab9d3c9eadf4a..e7a22b3df2ef26592199f1ac99224a2baf22af91 100644 --- a/src/tests/drivers/test-all-sensors.cpp +++ b/src/tests/drivers/test-all-sensors.cpp @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,32 +13,30 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include "DeathStack/configs/SensorManagerConfig.h" - -#include "DeathStack/SensorManager/Sensors/AD7994Wrapper.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapper.h" - #include <drivers/piksi/piksi.h> +#include <interfaces-impl/hwmapping.h> #include <sensors/ADIS16405/ADIS16405.h> #include <sensors/LM75B.h> #include <sensors/MPU9250/MPU9250.h> -#include "sensors/MS580301BA07/MS580301BA07.h" #include <sensors/SensorSampling.h> -#include <interfaces-impl/hwmapping.h> +#include "SensorManager/Sensors/AD7994Wrapper.h" +#include "SensorManager/Sensors/ADCWrapper.h" +#include "configs/SensorManagerConfig.h" +#include "sensors/MS580301BA07/MS580301BA07.h" using namespace DeathStackBoard; using namespace miosix; typedef MPU9250<spiMPU9250> MPU9250Type; -//typedef ADIS16405<spiADIS16405, sensors::adis16405::rst> ADIS16405Type; +// typedef ADIS16405<spiADIS16405, sensors::adis16405::rst> ADIS16405Type; typedef LM75B<i2c1> LM75BType; typedef MS580301BA07<spiMS5803> MS580301BA07Type; @@ -47,7 +44,7 @@ AD7994Wrapper* adc_ad7994; ADCWrapper* adc_internal; MPU9250Type* imu_mpu9250; -//ADIS16405Type* imu_adis16405; +// ADIS16405Type* imu_adis16405; LM75BType* temp_lm75b_analog; LM75BType* temp_lm75b_imu; @@ -64,26 +61,33 @@ void init() adc_ad7994 = new AD7994Wrapper(sensors::ad7994::addr, AD7994_V_REF); temp_lm75b_analog = new LM75BType(sensors::lm75b_analog::addr); temp_lm75b_imu = new LM75BType(sensors::lm75b_imu::addr); - //imu_adis16405 = new ADIS16405Type(ADIS16405Type::GYRO_FS_300); - adc_internal = new ADCWrapper(); + // imu_adis16405 = new ADIS16405Type(ADIS16405Type::GYRO_FS_300); + adc_internal = new ADCWrapper(); imu_mpu9250 = new MPU9250Type(MPU9250Type::ACC_FS_16G, MPU9250Type::GYRO_FS_2000); - pressure_ms5803 = new MS580301BA07Type(); + pressure_ms5803 = new MS580301BA07Type(); piksi = new Piksi("/dev/gps"); Thread::sleep(1000); - printf("\nTesting imu_mpu9250... %s\n", imu_mpu9250->init() ? "Ok" : "Failed"); - printf("Testing imu_adis16405... RIP\n"); // \n", imu_adis16405->init() ? "Ok" : "Failed"); - printf("Testing temp_lm75b_analog... %s\n", temp_lm75b_analog->init() ? "Ok" : "Failed"); - printf("Testing temp_lm75b_imu... %s\n", temp_lm75b_imu->init() ? "Ok" : "Failed"); + printf("\nTesting imu_mpu9250... %s\n", + imu_mpu9250->init() ? "Ok" : "Failed"); + printf("Testing imu_adis16405... RIP\n"); // \n", imu_adis16405->init() ? + // "Ok" : "Failed"); + printf("Testing temp_lm75b_analog... %s\n", + temp_lm75b_analog->init() ? "Ok" : "Failed"); + printf("Testing temp_lm75b_imu... %s\n", + temp_lm75b_imu->init() ? "Ok" : "Failed"); printf("Testing adc_ad7994... %s\n", adc_ad7994->init() ? "Ok" : "Failed"); - printf("Testing battery sensor ... %s\n", adc_internal->getBatterySensorPtr()->init() ? "Ok" : "Failed"); - printf("Testing current sensor... %s\n", adc_internal->getCurrentSensorPtr()->init() ? "Ok" : "Failed"); - printf("Testing digital pressure sensor... %s\n", pressure_ms5803->init() ? "Ok" : "Failed"); + printf("Testing battery sensor ... %s\n", + adc_internal->getBatterySensorPtr()->init() ? "Ok" : "Failed"); + printf("Testing current sensor... %s\n", + adc_internal->getCurrentSensorPtr()->init() ? "Ok" : "Failed"); + printf("Testing digital pressure sensor... %s\n", + pressure_ms5803->init() ? "Ok" : "Failed"); printf("\n\n"); } @@ -101,24 +105,27 @@ void update() void print() { - printf("MPU9250 Accel: \tZ: %.3f\n", - imu_mpu9250->accelDataPtr()->getZ()); + printf("MPU9250 Accel: \tZ: %.3f\n", imu_mpu9250->accelDataPtr()->getZ()); /* printf("ADIS Acc: \tZ: %.3f\n", imu_adis16405->accelDataPtr()->getZ()); */ printf("LM75B imu Temp: \tT: %.3f\n", temp_lm75b_imu->getTemp()); printf("LM75B analog Temp: \tT: %.3f\n", temp_lm75b_analog->getTemp()); - printf("Digital temp: \tP: %f\n", pressure_ms5803->getData().temp); - printf("HW Pressure: \tP: %f\n", adc_ad7994->getDataPtr()->honeywell_baro_pressure); - printf("NXP Pressure: \tP: %f\n", adc_ad7994->getDataPtr()->nxp_baro_pressure); - printf("Digital Pressure: \tP: %f\n", pressure_ms5803->getData().pressure); - printf("Battery voltage:\tV: %f\n", - adc_internal->getBatterySensorPtr()->getBatteryDataPtr()->volt); - printf("Current sens 1: \tC: %f\n", - adc_internal->getCurrentSensorPtr()->getCurrentDataPtr()->current_1); - printf("Current sens 2: \tC: %f\n", - adc_internal->getCurrentSensorPtr()->getCurrentDataPtr()->current_2); - printf("Pins: \tLP: %d, MC: %d\n", inputs::lp_dtch::value(), nosecone::nc_dtch::value()); - + printf("Digital temp: \tP: %f\n", pressure_ms5803->getData().temp); + printf("HW Pressure: \tP: %f\n", + adc_ad7994->getDataPtr()->honeywell_baro_pressure); + printf("NXP Pressure: \tP: %f\n", + adc_ad7994->getDataPtr()->nxp_baro_pressure); + printf("Digital Pressure: \tP: %f\n", + pressure_ms5803->getData().pressure); + printf("Battery voltage:\tV: %f\n", + adc_internal->getBatterySensorPtr()->getBatteryDataPtr()->volt); + printf("Current sens 1: \tC: %f\n", + adc_internal->getCurrentSensorPtr()->getCurrentDataPtr()->current_1); + printf("Current sens 2: \tC: %f\n", + adc_internal->getCurrentSensorPtr()->getCurrentDataPtr()->current_2); + printf("Pins: \tLP: %d, MC: %d\n", inputs::lp_dtch::value(), + nosecone::nc_dtch::value()); + try { auto gps = piksi->getGpsData(); @@ -140,7 +147,7 @@ int main() printf("** SENSORS TEST SUITE **\n"); printf("Press enter to start\n"); - (void) getchar(); + (void)getchar(); for (;;) { diff --git a/src/tests/drivers/test-cutter.cpp b/src/tests/drivers/test-cutter.cpp index 71bbdfa0ce34d92e21d0385fb941972db13c6f8f..76d08d6fd8884b2b8517c1524fa54c6acaca8335 100644 --- a/src/tests/drivers/test-cutter.cpp +++ b/src/tests/drivers/test-cutter.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2018-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,138 +13,251 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <interfaces-impl/hwmapping.h> -#include <miosix.h> +#include <Common.h> +#include <configs/CutterConfig.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/hbridge/HBridge.h> +#include <sensors/analog/current/CurrentSensor.h> + #include <iostream> #include <sstream> -#include "DeathStack/DeploymentController/ThermalCutter/Cutter.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapper.h" +/** + * Pin map for DeathStackX + * + * PWM : PE6 (both primary and backup) + * ENA : PD11 (primary) + * PG2 (backup) + * Csense : PF6 (primary) + * PF8 (backup) + */ using namespace std; -using namespace miosix; -using namespace DeathStackBoard; -static constexpr int CUT_TIME = 60 * 1000; +using namespace DeathStackBoard::CutterConfig; + +static constexpr int MAX_CUTTING_TIME = 10 * 1000; // ms +constexpr int SAMPLING_FREQUENCY = 20; + +/** + * The constants below are used only for SRAD nichel cutters + * They are not useful for activating the COTS ones + * (i.e. they are currently used only in the test-cutter entrypoint) + */ +static const PWM::Timer CUTTER_TIM{ + TIM9, &(RCC->APB2ENR), RCC_APB2ENR_TIM9EN, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; + +static const PWMChannel CUTTER_CHANNEL_PRIMARY = PWMChannel::CH2; +static const PWMChannel CUTTER_CHANNEL_BACKUP = PWMChannel::CH2; +static const unsigned int PRIMARY_CUTTER_PWM_FREQUENCY = 10000; // Hz +static constexpr float PRIMARY_CUTTER_PWM_DUTY_CYCLE = 0.45f; +static const unsigned int BACKUP_CUTTER_PWM_FREQUENCY = 10000; // Hz +static constexpr float BACKUP_CUTTER_PWM_DUTY_CYCLE = 0.45f; +static constexpr float CUTTER_TEST_PWM_DUTY_CYCLE = 0.1f; +static constexpr int CUT_TEST_DURATION = 1 * 1000; + +// This could go into CutterConfig +static constexpr InternalADC::Channel ADC_CHANNEL_PRIMARY = + InternalADC::Channel::CH6; +static constexpr InternalADC::Channel ADC_CHANNEL_BACKUP = + InternalADC::Channel::CH4; + +// Check page 16 of datasheet for more informations +static constexpr float ADC_TO_CURR_DKILIS = 10204.85; // Typ: 19.5 +static constexpr float ADC_TO_CURR_RIS = 510; +static constexpr float ADC_TO_CURR_IISOFF = .0001662; // Typ: 170uA + +static constexpr float ADC_TO_CURR_COEFF = ADC_TO_CURR_DKILIS / ADC_TO_CURR_RIS; +static constexpr float ADC_TO_CURR_OFFSET = + ADC_TO_CURR_DKILIS * ADC_TO_CURR_IISOFF; -long long measured_cut_time = 0; -void wait() +function<float(float)> adc_to_current = [](float adc_in) { + return ADC_TO_CURR_DKILIS * (adc_in / ADC_TO_CURR_RIS - ADC_TO_CURR_IISOFF); +}; + +bool finished = false; + +void menu(unsigned int *cutterNo, uint32_t *frequency, float *dutyCycle, uint32_t *cutDuration); + +void elapsedTimeAndCsense(void *args); + +int main() { - long long t0 = getTick(); - for (long long t = t0; t < t0 + CUT_TIME; t += 50) + unsigned int cutterNo = 0; + uint32_t frequency = 0; + float dutyCycle = 0; + uint32_t cutDuration = 0; // for cots cutters + + TimestampTimer::enableTimestampTimer(); + + // Set the clock divider for the analog circuitry (/8) + ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1; + + // Ask the user the parameters + menu(&cutterNo, &frequency, &dutyCycle, &cutDuration); + + // Cutter setup + + GpioPin *ena_pin; + PWMChannel pwmChannel; + + if (cutterNo == 3) // COTS cutters { - if (inputs::btn_open::value() == 0) + PrimaryCutterEna::getPin().high(); + BackupCutterEna::getPin().high(); + CuttersInput::getPin().high(); + + Thread::sleep(cutDuration); + + PrimaryCutterEna::getPin().low(); + BackupCutterEna::getPin().low(); + CuttersInput::getPin().low(); + } + else + { // SRAD cutters + if (cutterNo == 1) { - break; + ena_pin = new GpioPin( + PrimaryCutterEna::getPin()); + pwmChannel = CUTTER_CHANNEL_PRIMARY; + } + else // if (cutterNo == 2) + { + ena_pin = new GpioPin( + BackupCutterEna::getPin()); + pwmChannel = CUTTER_CHANNEL_BACKUP; } - Thread::sleep(50); - } - measured_cut_time = getTick() - t0; - // printf("Stopped!\n"); -} -bool print = false; + HBridge cutter(*ena_pin, CUTTER_TIM, + pwmChannel, frequency, dutyCycle / 100.0f); -void csense(void*) -{ - ADCWrapper adc; - adc.getCurrentSensorPtr()->init(); + // Start the test - for (;;) - { - adc.getCurrentSensorPtr()->onSimpleUpdate(); - uint16_t raw1 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_1; - uint16_t raw2 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_2; - - float current1 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_1; - float current2 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_2; - if (print) + miosix::Thread::create(elapsedTimeAndCsense, 2048, + miosix::MAIN_PRIORITY, &cutterNo); + + cutter.enable(); + + // Wait for the user to press ENTER or the timer to elapse + string temp; + while (!finished) { - printf("%d,%d,%d,%f,%f\n", (int)miosix::getTick(), (int)raw1, - (int)raw2, current1, current2); + (void)getchar(); + finished = true; + printf("Stopped... \n\n"); } - Thread::sleep(100); + + cutter.disable(); } + + return 0; } -int main() +void menu(unsigned int *cutterNo, uint32_t *frequency, float *dutyCycle, + uint32_t *cutDuration) { - Thread::create(csense, 2048); + string temp; - for (;;) + do { - print = false; printf( - "Info: To stop cutting (and to stop the cut timer) press the OPEN " - "button\n"); - printf("What do you want to cut? (1 - primary / 2 - backup)\n"); - unsigned int c, freq = 0; - float duty = 0; - string temp; + "\nWhat do you want to cut? (1 - SRAD primary / 2 - SRAD backup / " + "3 - COTS (both channels enabled))\n"); + printf(">> "); getline(cin, temp); - stringstream(temp) >> c; - if (c != 1 && c != 2) - { - printf("Choose 1 or 2\n"); - continue; - } - cout << "Insert frequency (Hz): \n"; - getline(cin, temp); - stringstream(temp) >> freq; - - cout << "Insert duty cycle(%): \n"; - getline(cin, temp); - stringstream(temp) >> duty; + stringstream(temp) >> *cutterNo; + } while (*cutterNo != 1 && *cutterNo != 2 && *cutterNo != 3); - printf("Cutting %d, freq: %d, duty: %f\n", c, freq, duty); - - if (!(freq > 1 && freq <= 30000 && duty >= 0.0f && duty <= 100.0f)) + if (*cutterNo != 3) // SRAD cutters + { + do { - printf("Wrong inputs!\n"); - continue; - } + printf("\nInsert frequency (1-30000 Hz): \n"); + printf(">> "); + getline(cin, temp); + stringstream(temp) >> *frequency; + } while (*frequency < 1 || *frequency > 30000); do { - cout << "READY!\nWrite 'yeet' to begin:\n"; + printf("\nInsert duty cycle (1-100): \n"); + printf(">> "); getline(cin, temp); - } while (temp != "yeet"); + stringstream(temp) >> *dutyCycle; + } while (*dutyCycle < 1 || *dutyCycle > 100); + printf("\nCutting %s, frequency: %lu, duty cycle: %.1f\n\n", + (*cutterNo ? "PRIMARY" : "BACKUP"), *frequency, *dutyCycle); + } + else // COTS cutters + { + do { - print = true; - - Cutter cutter{freq, duty / 100, CUTTER_TEST_PWM_DUTY_CYCLE}; - - if (c == 1) - { - cutter.enablePrimaryCutter(); - wait(); - cutter.disablePrimaryCutter(); - } - else if (c == 2) - { - cutter.enableBackupCutter(); - wait(); - cutter.disableBackupCutter(); - } - } - Thread::sleep(2000); - print = false; - Thread::sleep(500); - printf("Cut Time: %.2f s\n", (measured_cut_time) / 1000.0f); - printf("Done!\n\n\n"); + printf("\nInsert cut duration (1-1000 milliseconds): \n"); + printf(">> "); + getline(cin, temp); + stringstream(temp) >> *cutDuration; + } while (*cutDuration < 1 || *cutDuration > 1000); } - return 0; -} \ No newline at end of file + do + { + printf("\nREADY!\n"); + printf("Write 'start' to begin then press ENTER to end the test:\n"); + getline(cin, temp); + } while (temp != "start"); +} + +void elapsedTimeAndCsense(void *args) +{ + int cutterNo = *(unsigned int *)args; + // Sensors setup + + InternalADC internalADC(*ADC3, 3.3); + internalADC.init(); + internalADC.enableChannel(ADC_CHANNEL_PRIMARY); + internalADC.enableChannel(ADC_CHANNEL_BACKUP); + + function<ADCData()> get_voltage_function; + + if (cutterNo == 1) + { + get_voltage_function = + bind(&InternalADC::getVoltage, &internalADC, ADC_CHANNEL_PRIMARY); + } + else + { + get_voltage_function = + bind(&InternalADC::getVoltage, &internalADC, ADC_CHANNEL_BACKUP); + } + + CurrentSensor current_sensor(get_voltage_function, adc_to_current); + current_sensor.init(); + + // Save the cuttent timestamp + uint64_t t = TimestampTimer::getTimestamp() / 1000; + uint64_t t0 = t; + + while (t < t0 + MAX_CUTTING_TIME && !finished) + { + Thread::sleep(1000 / SAMPLING_FREQUENCY); + + t = TimestampTimer::getTimestamp() / 1000; + internalADC.sample(); + current_sensor.sample(); + + CurrentSensorData current_data = current_sensor.getLastSample(); + printf("Elapsed time : %.2f\tCsense: %.4fV %.3fA\n", (t - t0) / 1000.0, + current_data.voltage, current_data.current); + } + + printf("Cut Time: %.2f s\n\n", (t - t0) / 1000.0f); +} diff --git a/src/tests/drivers/test-imus.cpp b/src/tests/drivers/test-imus.cpp index 1790876b2ecf5f2f1fb6d4e0eb7ca25d09a69ba8..78c6b4b9d2b84f60ebb48d5c755e4b0f62115efc 100644 --- a/src/tests/drivers/test-imus.cpp +++ b/src/tests/drivers/test-imus.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta <luca.erbetta@skywarder.eu> + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,7 +22,8 @@ #include <Common.h> #include <interfaces-impl/hwmapping.h> -#include "DeathStack/configs/SensorManagerConfig.h" + +#include "configs/SensorManagerConfig.h" #include "sensors/MPU9250/MPU9250.h" using miosix::Thread; diff --git a/src/tests/drivers/test-mavlink.cpp b/src/tests/drivers/test-mavlink.cpp index 03169bfd6ade1678358a028d09e85a2f8fdf4db2..45f2e47a70587dbc9dfec96cc519574384dd8372 100644 --- a/src/tests/drivers/test-mavlink.cpp +++ b/src/tests/drivers/test-mavlink.cpp @@ -13,35 +13,30 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <libs/mavlink_skyward_lib/mavlink_lib/r2a/mavlink.h> - #include <Common.h> +#include <configs/TMTCConfig.h> +#include <drivers/Xbee/Xbee.h> #include <drivers/gamma868/Gamma868.h> #include <drivers/mavlink/MavlinkDriver.h> +#include <libs/mavlink_skyward_lib/mavlink_lib/r2a/mavlink.h> -#include <boards/DeathStack/configs/TMTCConfig.h> - -#include <drivers/Xbee/Xbee.h> - -#include "DeathStack/events/Events.h" +#include "TMTCManager/XbeeInterrupt.h" +#include "events/Events.h" #include "events/FSM.h" - -#include "DeathStack/TMTCManager/XbeeInterrupt.h" - using namespace miosix; using namespace DeathStackBoard; -using MavChannel = MavlinkDriver<256,10>; +using MavChannel = MavlinkDriver<256, 10>; -Xbee_t* device; +Xbee::Xbee* device; MavChannel* channel; // Receive function: sends an ACK back @@ -57,28 +52,27 @@ static void onReceive(MavChannel* channel, const mavlink_message_t& msg) /* Send the message back to the sender */ bool ackSent = channel->enqueueMsg(ackMsg); - if(!ackSent) + if (!ackSent) TRACE("[Receiver] Could not enqueue ack\n"); } } - - int main() { enableXbeeInterrupt(); - busSPI2::init(); - device = new Xbee_t(); + SPIBus xbee_bus(SPI2); + device = new Xbee::Xbee(xbee_bus, XbeeCS::getPin(), XbeeATTN::getPin(), + XbeeRST::getPin()); channel = new MavChannel(device, &onReceive, 250); device->start(); channel->start(); // Send function: enqueue a ping every second - while(1) + while (1) { - //TRACE("[TmtcTest] Enqueueing ping\n"); + // TRACE("[TmtcTest] Enqueueing ping\n"); // Create a Mavlink message // mavlink_message_t pingMsg; diff --git a/src/tests/drivers/test-motor.cpp b/src/tests/drivers/test-motor.cpp index 1d5b804bc4e0dd95450667b4db43bc87626898a8..96271fbea5eaef2300a8152fd6ada222c7f0dbad 100644 --- a/src/tests/drivers/test-motor.cpp +++ b/src/tests/drivers/test-motor.cpp @@ -1,7 +1,5 @@ -/** - * - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -15,18 +13,20 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include "DeathStack/DeploymentController/Motor/MotorDriver.h" #include <interfaces-impl/hwmapping.h> + #include <iostream> -#include <string> #include <sstream> +#include <string> + +#include "DeploymentController/Motor/MotorDriver.h" using namespace miosix; using namespace DeathStackBoard; @@ -44,26 +44,28 @@ motorState state = STOPPED; void buttonMode() { - while(true) + while (true) { - if(inputs::btn_open::value() == 0) + if (inputs::btn_open::value() == 0) { Thread::sleep(10); motor.start(MotorDirection::NORMAL); - while(!inputs::btn_open::value()); + while (!inputs::btn_open::value()) + ; motor.stop(); } - if(inputs::btn_close::value() == 0) + if (inputs::btn_close::value() == 0) { Thread::sleep(10); motor.start(MotorDirection::REVERSE); - while(!inputs::btn_close::value()); + while (!inputs::btn_close::value()) + ; motor.stop(); } @@ -75,7 +77,7 @@ int main() Thread::sleep(500); - while(true) + while (true) { printf("\nOptions:\n"); printf(" o - open\n"); @@ -90,7 +92,7 @@ int main() getline(cin, temp); stringstream(temp) >> c; - switch(c) + switch (c) { case 'o': motor.start(MotorDirection::NORMAL); @@ -108,9 +110,8 @@ int main() return 0; break; default: - break; + break; } - } return 0; diff --git a/src/tests/drivers/test-power-board.cpp b/src/tests/drivers/test-power-board.cpp deleted file mode 100644 index d54868c0a49c167de6f27d1cb8145291d76527d5..0000000000000000000000000000000000000000 --- a/src/tests/drivers/test-power-board.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <miosix.h> -#include <Common.h> -#include <interfaces-impl/hwmapping.h> - -using namespace miosix; -//using namespace DeathStackBoard; - -int main() -{ - - using namespace actuators; - tcPwm::mode(Mode::OUTPUT); - - thCut1::ena::mode(Mode::OUTPUT); - thCut1::ena::low(); - thCut1::csens::mode(Mode::INPUT_ANALOG); - - thCut2::ena::mode(Mode::OUTPUT); - thCut2::ena::low(); - thCut2::csens::mode(Mode::INPUT_ANALOG); - - char c; - while(true) - { - scanf("%c", &c); - - switch(c) - { - case 'a': - thCut1::ena::high(); - printf("thCut1 ena\n"); - break; - case 'b': - thCut2::ena::high(); - printf("thCut2 ena\n"); - break; - case 'c': - tcPwm::high(); - printf("tcPwm\n"); - break; - case 'd': - thCut1::ena::low(); - thCut2::ena::low(); - tcPwm::low(); - printf("Closing everything\n"); - break; - default: - break; - } - } -} \ No newline at end of file diff --git a/src/tests/drivers/test-pressure-calib.cpp b/src/tests/drivers/test-pressure-calib.cpp index 9b15f8ac3c9858e70463ea715d20e122346d21d6..317b613c2649d83ced5b2d3076649ceeb57a676c 100644 --- a/src/tests/drivers/test-pressure-calib.cpp +++ b/src/tests/drivers/test-pressure-calib.cpp @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,26 +13,24 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include "DeathStack/configs/SensorManagerConfig.h" - -#include "DeathStack/SensorManager/Sensors/AD7994Wrapper.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapper.h" - #include <drivers/piksi/piksi.h> +#include <interfaces-impl/hwmapping.h> #include <sensors/ADIS16405/ADIS16405.h> #include <sensors/LM75B.h> #include <sensors/MPU9250/MPU9250.h> #include <sensors/SensorSampling.h> -#include "sensors/MS580301BA07/MS580301BA07.h" -#include <interfaces-impl/hwmapping.h> +#include "SensorManager/Sensors/AD7994Wrapper.h" +#include "SensorManager/Sensors/ADCWrapper.h" +#include "configs/SensorManagerConfig.h" +#include "sensors/MS580301BA07/MS580301BA07.h" using namespace DeathStackBoard; using namespace miosix; @@ -85,8 +82,9 @@ void update() void print() { - printf("%d,%f,%f,%f,%f,%f,%f\n", (int)miosix::getTick(), temp_lm75b_imu->getTemp(), - temp_lm75b_analog->getTemp(), pressure_ms5803->getData().temp, + printf("%d,%f,%f,%f,%f,%f,%f\n", (int)miosix::getTick(), + temp_lm75b_imu->getTemp(), temp_lm75b_analog->getTemp(), + pressure_ms5803->getData().temp, adc_ad7994->getDataPtr()->honeywell_baro_pressure, adc_ad7994->getDataPtr()->nxp_baro_pressure, pressure_ms5803->getData().pressure); diff --git a/src/tests/drivers/test-servo.cpp b/src/tests/drivers/test-servo.cpp index 915841cb7201d304aa164ed3988b8a8fb243963a..4fdf148062336732e48a0c2cf63092148d99ab70 100644 --- a/src/tests/drivers/test-servo.cpp +++ b/src/tests/drivers/test-servo.cpp @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,58 +13,332 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <arch/common/drivers/servo_stm32.h> +#include <AirBrakes/AirBrakesServo.h> +#include <Deployment/DeploymentServo.h> #include <miosix.h> + #include <cstdio> #include <iostream> #include <sstream> #include <string> -#include "drivers/servo/servo.h" -#include "DeathStack/configs/DeploymentConfig.h" -using namespace miosix; using namespace std; using namespace DeathStackBoard; -static const PWMChannel CHANNEL = PWMChannel::CH1; +int servoMenu(); +int actionMenu(); +void waitUserInput(); +void testServo(); +void wiggleServo(); +void setServoFullyOpen(); +void setServoFullyClose(); +void manualServoControl(); +int askMinutes(); +void servoBreakIn(); + +// Noise generating functions +int noise2(int x, int y); +float smooth_inter(float x, float y, float s); +float noise2d(float x, float y); +float perlin2d(float x, float y, float freq, int depth); + +int servoChoice; +ServoInterface* servo; +PWMChannel channel; + +constexpr int WIGGLE_STEPS = 5; + +static int hashh[] = { + 208, 34, 231, 213, 32, 248, 233, 56, 161, 78, 24, 140, 71, 48, 140, + 254, 245, 255, 247, 247, 40, 185, 248, 251, 245, 28, 124, 204, 204, 76, + 36, 1, 107, 28, 234, 163, 202, 224, 245, 128, 167, 204, 9, 92, 217, + 54, 239, 174, 173, 102, 193, 189, 190, 121, 100, 108, 167, 44, 43, 77, + 180, 204, 8, 81, 70, 223, 11, 38, 24, 254, 210, 210, 177, 32, 81, + 195, 243, 125, 8, 169, 112, 32, 97, 53, 195, 13, 203, 9, 47, 104, + 125, 117, 114, 124, 165, 203, 181, 235, 193, 206, 70, 180, 174, 0, 167, + 181, 41, 164, 30, 116, 127, 198, 245, 146, 87, 224, 149, 206, 57, 4, + 192, 210, 65, 210, 129, 240, 178, 105, 228, 108, 245, 148, 140, 40, 35, + 195, 38, 58, 65, 207, 215, 253, 65, 85, 208, 76, 62, 3, 237, 55, + 89, 232, 50, 217, 64, 244, 157, 199, 121, 252, 90, 17, 212, 203, 149, + 152, 140, 187, 234, 177, 73, 174, 193, 100, 192, 143, 97, 53, 145, 135, + 19, 103, 13, 90, 135, 151, 199, 91, 239, 247, 33, 39, 145, 101, 120, + 99, 3, 186, 86, 99, 41, 237, 203, 111, 79, 220, 135, 158, 42, 30, + 154, 120, 67, 87, 167, 135, 176, 183, 191, 253, 115, 184, 21, 233, 58, + 129, 233, 142, 39, 128, 211, 118, 137, 139, 255, 114, 20, 218, 113, 154, + 27, 127, 246, 250, 1, 8, 198, 250, 209, 92, 222, 173, 21, 88, 102, + 219}; int main() { - Servo s{DeploymentConfigs::SERVO_TIMER}; - s.setMinPulseWidth(800); - s.setMaxPulseWidth(2200); - s.setPosition(CHANNEL, 0.77f); - s.enable(CHANNEL); - - s.start(); - // Thread::sleep(5000); + TimestampTimer::enableTimestampTimer(); + + servoChoice = servoMenu(); + + switch (servoChoice) + { + case 1: + testServo(); + break; + case 2: + testServo(); + break; + + default: + break; + } + + return 0; +} + +int servoMenu() +{ + string temp; + int choice; + + printf("\nWhat do you want to move?\n"); + printf("1. Deployment servo\n"); + printf("2. Airbrakes servo\n"); + printf("\n>> "); + getline(cin, temp); + stringstream(temp) >> choice; + + return choice; +} + +int actionMenu() +{ + string temp; + int choice; + + printf("\nWhat do you want to do?\n"); + printf("1. Servo wiggle (self test)\n"); + printf("2. Servo fully open\n"); + printf("3. Servo fully close\n"); + printf("4. Servo manual control\n"); + printf("5. Servo break-in (rodaggio)\n"); + printf("6. Exit\n"); + printf("\n>> "); + getline(cin, temp); + stringstream(temp) >> choice; + + return choice; +} + +void waitUserInput() +{ string temp; - printf("Ready.\n"); - int pos; - for (;;) + + do { - temp = ""; - printf("Insert position: \n"); + printf("Write 'start' to begin the test:\n"); getline(cin, temp); - - stringstream(temp) >> pos; + } while (temp != "start"); +} - if (pos >= 0 && pos <= 100) +void testServo() +{ + if (servoChoice == 1) + { + servo = new DeploymentServo{}; + channel = DPL_SERVO_PWM_CH; + } + else + { + servo = new AirBrakesServo{}; + channel = AB_SERVO_PWM_CH; + } + + servo->enable(); + servo->reset(); + + while (true) + { + switch (actionMenu()) { - printf("Position set to %d\n", pos); - s.setPosition(CHANNEL, pos / 100.0f); + case 1: + wiggleServo(); + break; + case 2: + setServoFullyOpen(); + break; + case 3: + setServoFullyClose(); + break; + case 4: + manualServoControl(); + break; + case 5: + servoBreakIn(); + break; + case 6: + return; + break; + + default: + break; } - else + } + + servo->reset(); + miosix::Thread::sleep(1000); + servo->disable(); +} + +void wiggleServo() +{ + printf("\n\n** WIGGLE SERVO **\n\n"); + + waitUserInput(); + + printf("Wiggling ...\n"); + + servo->selfTest(); + miosix::Thread::sleep(1000); + servo->disable(); + + printf("\n\tTest finished!\n\n"); +} + +void setServoFullyOpen() +{ + printf("\n\n** SERVO FULLY OPEN **\n\n"); + + waitUserInput(); + + servo->setMaxPosition(); + + printf("\n\tTest finished!\n\n"); +} + +void setServoFullyClose() +{ + printf("\n\n** SERVO FULLY CLOSE **\n\n"); + + waitUserInput(); + + servo->setMinPosition(); + + printf("\n\tTest finished!\n\n"); +} + +void manualServoControl() +{ + printf("\n\n** MANUAL SERVO CONTROL **\n\n"); + + string temp; + float angle; + + do + { + printf("Write the servo postion in degrees:\n"); + getline(cin, temp); + stringstream(temp) >> angle; + } while (angle < 0 || angle > 180); + + servo->set(angle); + + printf("\n\tTest finished!\n\n"); +} + +int askMinutes() +{ + int seconds; + + printf("How many minutes the test should run?\n"); + printf("\n>> "); + scanf("%d", &seconds); + + return seconds; +} + +void servoBreakIn() +{ + printf("\n\n** SERVO BREAK-IN **\n\n"); + + uint64_t minutes = askMinutes(); + + waitUserInput(); + + uint64_t start = TimestampTimer::getTimestamp(); + + while (TimestampTimer::getTimestamp() - start < minutes * 60 * 1000000) + { + uint64_t start2 = TimestampTimer::getTimestamp(); + float counter = 0; + + // 10 seconds + while (TimestampTimer::getTimestamp() - start2 < 10 * 1000000) { - printf("You dumb fuck.\n"); + double angolo = + servo->MIN_POS + abs(perlin2d(counter, 0, .075, 10)) * + (servo->MAX_POS - servo->MIN_POS); + servo->set(angolo); + + counter++; + + miosix::delayMs(10); } - Thread::sleep(100); + + for (int i = 0; i < 10; i++) + { + servo->setMinPosition(); + miosix::delayMs(1500); + servo->setMaxPosition(); + miosix::delayMs(1500); + } + } +} + +int noise2(int x, int y) +{ + int tmp = hashh[y % 256]; + return hashh[(tmp + x) % 256]; +} + +float smooth_inter(float x, float y, float s) +{ + return x + (s * s * (3 - 2 * s)) * (y - x); +} + +float noise2d(float x, float y) +{ + int x_int = x; + int y_int = y; + float x_frac = x - x_int; + float y_frac = y - y_int; + int s = noise2(x_int, y_int); + int t = noise2(x_int + 1, y_int); + int u = noise2(x_int, y_int + 1); + int v = noise2(x_int + 1, y_int + 1); + float low = smooth_inter(s, t, x_frac); + float high = smooth_inter(u, v, x_frac); + return smooth_inter(low, high, y_frac); +} + +float perlin2d(float x, float y, float freq, int depth) +{ + float xa = x * freq; + float ya = y * freq; + float amp = 1.0; + float fin = 0; + float div = 0.0; + + int i; + for (i = 0; i < depth; i++) + { + div += 256 * amp; + fin += noise2d(xa, ya) * amp; + amp /= 2; + xa *= 2; + ya *= 2; } + + return fin / div; } \ No newline at end of file diff --git a/src/tests/eigen-test.cpp b/src/tests/eigen-test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7a3ebb4d07b05ec4bed898451dd19c11e022e42f --- /dev/null +++ b/src/tests/eigen-test.cpp @@ -0,0 +1,189 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Common.h> +#include <miosix.h> + +#include <Eigen/Dense> +#include <Eigen/Sparse> +#include <iomanip> +#include <iostream> + +#include "drivers/HardwareTimer.h" + +using namespace std; +using namespace Eigen; +using namespace miosix; +using miosix::Thread; + +void nProducts2Mat(int n, MatrixXd& m1, MatrixXd& m2) +{ + HardwareTimer<uint32_t, 2>& hrclock = + HardwareTimer<uint32_t, 2>::instance(); + hrclock.setPrescaler(127); + hrclock.start(); + + int i = 0; + uint32_t t1 = hrclock.tick(); + + for (i = 0; i < n; i++) + MatrixXd m = m1 * m2; + + uint32_t t2 = hrclock.tick(); + double time = hrclock.toMilliSeconds(t2 - t1); + hrclock.stop(); + + TRACE("\nTime for %d products using 2 matrices: %f [ms] \n\n", n, time); +} + +void nProducts3Mat(int n, MatrixXd& m1, MatrixXd& m2, MatrixXd& m3) +{ + HardwareTimer<uint32_t, 2>& hrclock = + HardwareTimer<uint32_t, 2>::instance(); + hrclock.setPrescaler(127); + hrclock.start(); + + int i = 0; + uint32_t t1 = hrclock.tick(); + + for (i = 0; i < n; i++) + MatrixXd m = m1 * m2 * m3; + + uint32_t t2 = hrclock.tick(); + double time = hrclock.toMilliSeconds(t2 - t1); + hrclock.stop(); + + TRACE("\nTime for %d products using 3 matrices: %f [ms] \n\n", n, time); +} + +void kalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, MatrixXd& m4, + MatrixXd& m5, MatrixXd& eye, MatrixXd& v1, MatrixXd& v2) +{ + HardwareTimer<uint32_t, 2>& hrclock = + HardwareTimer<uint32_t, 2>::instance(); + hrclock.setPrescaler(127); + hrclock.start(); + + auto x = v1; + auto P = m2; + auto Q = m3; + auto R = m5; + auto y = v2; + + uint32_t t1 = hrclock.tick(); + + auto F = 0.5 * m1; + x = F * x; + P = F * P * (F.transpose()) + Q; + auto H = 2 * m4; + auto K = P * H.transpose() * ((H * P * H.transpose() + R).inverse()); + auto U = K * (y.transpose() - H * x); + auto x_new = x + U; + auto P_new = (eye - K * H) * P; + + uint32_t t2 = hrclock.tick(); + double time = hrclock.toMilliSeconds(t2 - t1); + hrclock.stop(); + + TRACE("\nTime for a single kalman cycle: %f [ms] \n\n", time); +} + +void sparseKalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, + MatrixXd& m4, MatrixXd& m5, MatrixXd& eye, + MatrixXd& v1, MatrixXd& v2) +{ + // H, P and R can't be sparse since their product needs to be inverted. + HardwareTimer<uint32_t, 2>& hrclock = + HardwareTimer<uint32_t, 2>::instance(); + hrclock.setPrescaler(127); + hrclock.start(); + + auto x = v1; + auto P = m2; + auto Q = m3.sparseView(); + auto R = m5; + auto y = v2; + + uint32_t t1 = hrclock.tick(); + + auto F = 0.5 * m1.sparseView(); + x = F * x; + P = F * P * (F.transpose()) + Q; + auto H = 2 * m4; + auto K = (P * (H.transpose()) * ((H * P * H.transpose() + R).inverse())) + .sparseView(); + auto U = (K * (y.transpose() - H * x)).sparseView(); + auto x_new = x + U; + auto P_new = (eye - K * H) * P; + + uint32_t t2 = hrclock.tick(); + double time = hrclock.toMilliSeconds(t2 - t1); + hrclock.stop(); + + TRACE( + "\nTime for a single kalman cycle with some sparse matrices: %f [ms] " + "\n\n", + time); +} + +void determinant(MatrixXd& m1) +{ + HardwareTimer<uint32_t, 2>& hrclock = + HardwareTimer<uint32_t, 2>::instance(); + hrclock.setPrescaler(127); + hrclock.start(); + + uint32_t t1 = hrclock.tick(); + + float det = m1.determinant(); + + uint32_t t2 = hrclock.tick(); + double time = hrclock.toMilliSeconds(t2 - t1); + hrclock.stop(); + + TRACE("\nTime to find the determinant: %f [ms] \n\n", time); +} + +int main() +{ + static const int ROWS = 10; + static const int COL = 10; + static const int N = 100; + + MatrixXd m1 = MatrixXd::Random(ROWS, COL); + MatrixXd m2 = MatrixXd::Random(ROWS, COL); + MatrixXd m3 = MatrixXd::Random(ROWS, COL); + MatrixXd m4 = MatrixXd::Random(ROWS, COL); + MatrixXd m5 = MatrixXd::Random(ROWS, COL); + MatrixXd eye = MatrixXd::Identity(ROWS, ROWS); + + MatrixXd v1 = MatrixXd::Random(ROWS, 1); + MatrixXd v2 = MatrixXd::Random(1, ROWS); + + determinant(m1); + nProducts2Mat(N, m1, m2); + nProducts3Mat(N, m1, m2, m3); + kalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2); + sparseKalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2); + + return 0; +} \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/HILSimulationConfig.h b/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..40d10858bf503a651e6ee35c57c3131c31494b09 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/HILSimulationConfig.h @@ -0,0 +1,145 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" +#include "sensors/SensorInfo.h" + +struct SensorConfig : public SensorInfo +{ + SensorConfig(const uint32_t freq) : SensorInfo{freq, []() {}, false, false} + { + } +}; + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 2; + +/** baudrate of connection */ +const int SIM_BAUDRATE = 115200; + +/** Period of simulation in milliseconds */ +const int SIMULATION_PERIOD = 100; + +/** sample frequency of sensor (samples/second) */ +const int ACCEL_FREQ = 100; +const int GYRO_FREQ = 100; +const int MAGN_FREQ = 100; +const int BARO_FREQ = 20; +const int GPS_FREQ = 10; + +/** update frequency of the Navigation System */ +const int KALM_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int CONTROL_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int ADA_FREQ = 20; + +/** min and max values in radiants of the actuator */ +const float MinAlphaDegree = 0.0; +const float MaxAlphaDegree = 48 * 3.14 / 180; + +/** ADA configs + * [TODO?] Prendi valori da config ADA + */ +float deploymentAltitude = 100; +float referenceAltitude = 109; // launchpad Altitude for Pont De Sor? +float referenceTemperature = 15; + +/** sensors configuration */ +SensorConfig accelConfig(ACCEL_FREQ); +SensorConfig gyroConfig(GYRO_FREQ); +SensorConfig magnConfig(MAGN_FREQ); +SensorConfig baroConfig(BARO_FREQ); +SensorConfig gpsConfig(GPS_FREQ); +SensorConfig kalmConfig(KALM_FREQ); + +/** Number of samples per sensor at each simulator iteration */ +const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + */ +struct SensorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + // bool fix; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d57fd42a411b0fc260af3bb371549a71c7495b97 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp @@ -0,0 +1,247 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#define EIGEN_RUNTIME_NO_MALLOC // enable eigen malloc usage assert + +#include <Common.h> +#include <events/EventBroker.h> +#include <events/Events.h> + +#include "TimestampTimer.h" +#include "miosix.h" +#include "scheduler/TaskScheduler.h" +#include "sensors/SensorManager.h" + +/* HIL includes */ +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/HIL_actuators/HILServo.h" +#include "hardware_in_the_loop/HIL_sensors/HILAccelerometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILGps.h" +#include "hardware_in_the_loop/HIL_sensors/HILGyroscope.h" +#include "hardware_in_the_loop/HIL_sensors/HILKalman.h" +#include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h" +/*#include "hardware_in_the_loop/events/Events.h" +#include "hardware_in_the_loop/events/Topics.h"*/ + +/* Airbrakes includes */ +#include "DeathStack/AirBrakesController/AirBrakesControllerAlgorithm.h" + +/* ADA includes */ +#include <ADA/ADA.h> +#include <ADA/ADAController.h> +#include <configs/ADAconfig.h> +#include <events/FSM.h> +#include <events/utils/EventCounter.h> + +using namespace std; +using namespace miosix; +using namespace DeathStackBoard; + +bool isSimulationRunning; + +/** + * structure that contains all the sensors used in the simulation + */ +struct StateComplete +{ + HILAccelerometer *accelerometer; + HILBarometer *barometer; + HILGps *gps; + HILGyroscope *gyro; + HILMagnetometer *magnetometer; + HILKalman *kalman; +}; + +uint8_t getNextSchedulerId(TaskScheduler *s) +{ + return (s->getTaskStats()).back().id + 1; +} + +void setIsSimulationRunning(bool running) { isSimulationRunning = running; } + +int main() +{ + isSimulationRunning = true; + + // crash if eigen dynamically allocates memory + Eigen::internal::set_is_malloc_allowed(false); + + TimestampTimer::enableTimestampTimer(); + + // Definition of the flight phases manager + HILFlightPhasesManager *flightPhasesManager = + HILFlightPhasesManager::getInstance(); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&setIsSimulationRunning, false)); + + /*-------------- [TS] Task Scheduler --------------*/ + + TaskScheduler scheduler; + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); + + /*-------------- [HILT] HILTransceiver --------------*/ + + // Definition of the transceiver object + HILTransceiver *matlab = HILTransceiver::getInstance(); + + // registering the HILTransceiver in order to let him know when it has to + // wait to the control algorithm or not + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, bind(&HILTransceiver::setIsAirbrakePhase, matlab, true)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, bind(&HILTransceiver::setIsAirbrakePhase, matlab, false)); + + /*-------------- Sensors & Actuators --------------*/ + + // Definition of the fake sensors for the simulation + StateComplete state; + state.accelerometer = new HILAccelerometer(matlab, N_DATA_ACCEL); + state.barometer = new HILBarometer(matlab, N_DATA_BARO); + state.gps = new HILGps(matlab, N_DATA_GPS); + state.gyro = new HILGyroscope(matlab, N_DATA_GYRO); + state.magnetometer = new HILMagnetometer(matlab, N_DATA_MAGN); + state.kalman = new HILKalman(matlab, N_DATA_KALM); + + // Definition of the fake actuators for the simulation + HILServo servo(matlab); + servo.init(); + + /*-------------- [SM] Sensor Manager --------------*/ + + // instantiate the sensor manager with the given scheduler + SensorManager SM(&scheduler, {{state.accelerometer, accelConfig}, + {state.barometer, baroConfig}, + {state.gps, gpsConfig}, + {state.gyro, gyroConfig}, + {state.magnetometer, magnConfig}, + {state.kalman, kalmConfig}}); + + // registering the enabling of the sensors to the flightPhasesManager + + // registering the enabling of the sensors to the flightPhasesManager + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, bind(&SensorManager::enableAllSensor, &SM)); + + /*---------------- [ADA] ADA ---------------*/ + ADAController<HILBaroData, HILGpsData> *ada_controller = + new ADAController<HILBaroData, HILGpsData>(state.barometer, state.gps); + + // setDeploymentAltitude when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setDeploymentAltitude, + ada_controller, deploymentAltitude)); + + // setReferenceAltitude when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceAltitude, + ada_controller, referenceAltitude)); + + // setReferenceTemperature when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceTemperature, + ada_controller, referenceTemperature)); + + /*-------------- [CA] Control Algorithm --------------*/ + + // definition of the control algorithm + AirBrakesControllerAlgorithm<HILKalmanData> airbrakeCA(state.kalman, + &servo); + + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, + bind(&AirBrakesControllerAlgorithm<HILKalmanData>::begin, &airbrakeCA)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, + bind(&AirBrakesControllerAlgorithm<HILKalmanData>::end, &airbrakeCA)); + + /*-------------- Events --------------*/ + + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_FLIGHT_EVENTS); + + counter.subscribe(TOPIC_ADA); + /* INSERT THE SUBSCRIBING FOR THE EVENTBROKER (inserted in the + * flightPhasesManager object) */ + + /*-------------- Adding tasks to scheduler --------------*/ + + // adding the updating of the ADA to the scheduler + { + TaskScheduler::function_t update_ADA{bind( + &ADAController<HILBaroData, HILGpsData>::update, ada_controller)}; + + scheduler.add(update_ADA, (uint32_t)(1000 / ADA_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the updating of the algorithm to the scheduler + { + TaskScheduler::function_t update_Airbrake{bind( + &AirBrakesControllerAlgorithm<HILKalmanData>::update, &airbrakeCA)}; + + scheduler.add(update_Airbrake, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the Idle updating of the aperture when airbrakes are disabled + { + TaskScheduler::function_t update_Idle{ + bind(&HILTransceiver::setIdleActuatorData, matlab)}; + + scheduler.add(update_Idle, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + /*---------- Starting threads --------*/ + + matlab->start(); + ada_controller->start(); + sEventBroker->start(); + scheduler.start(); // started only the scheduler instead of the SM + + /*---------- Normal execution --------*/ + + bool apogeeReached = false; + + while (isSimulationRunning) + { + // [TODO] metti nel taskscheduler + if (!apogeeReached && counter.getCount(EV_ADA_APOGEE_DETECTED) > 0) + { + flightPhasesManager->setFlagsFlightPhases(APOGEE, true); + apogeeReached = true; + } + + Thread::sleep(1000 * ADAConfigs::SAMPLING_PERIOD); + } + + return 0; +} \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/HILSimulationConfig.h b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..1cb418d07fc47a4cde4a20a17a032042c13f9631 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/HILSimulationConfig.h @@ -0,0 +1,145 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" +#include "sensors/SensorInfo.h" + +struct SensorConfig : public SensorInfo +{ + SensorConfig(const uint32_t freq) : SensorInfo{freq, []() {}, false, false} + { + } +}; + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 2; + +/** baudrate of connection */ +const int SIM_BAUDRATE = 115200; + +/** Period of simulation in milliseconds */ +const int SIMULATION_PERIOD = 100; + +/** sample frequency of sensor (samples/second) */ +const int IMU_FREQ = 100; +const int ACCEL_FREQ = 100; +const int GYRO_FREQ = 100; +const int MAGN_FREQ = 100; +const int BARO_FREQ = 20; +const int GPS_FREQ = 10; + +/** update frequency of the Navigation System */ +const int KALM_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int CONTROL_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int ADA_FREQ = 20; + +/** min and max values in radiants of the actuator */ +const float MinAlphaDegree = 0.0; +const float MaxAlphaDegree = 48; + +/** ADA configs + * [TODO?] Prendi valori da config ADA + */ +float deploymentAltitude = 100; +float referenceAltitude = 109; // launchpad Altitude for Pont De Sor? +float referenceTemperature = 15; + +/** sensors configuration */ +SensorConfig imuConfig(IMU_FREQ); +SensorConfig baroConfig(BARO_FREQ); +SensorConfig gpsConfig(GPS_FREQ); +SensorConfig kalmConfig(KALM_FREQ); // TODO: delete + +/** Number of samples per sensor at each simulator iteration */ +const int N_DATA_IMU = (IMU_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + */ +struct SensorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + // bool fix; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp new file mode 100644 index 0000000000000000000000000000000000000000..51bb28c8b8972298802f01bdadf12f1a0555be84 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp @@ -0,0 +1,227 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#define EIGEN_RUNTIME_NO_MALLOC // enable eigen malloc usage assert + +#include <Common.h> +#include <events/EventBroker.h> +#include <events/Events.h> +#include <events/utils/EventCounter.h> + +#include "TimestampTimer.h" +#include "miosix.h" +#include "scheduler/TaskScheduler.h" +#include "sensors/SensorManager.h" + +/* HIL includes */ +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/HIL_actuators/HILServo.h" +#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILGps.h" +#include "hardware_in_the_loop/HIL_sensors/HILImu.h" +#include "hardware_in_the_loop/HIL_sensors/HILNas.h" // TODO: delete + +/* Airbrakes includes */ +#include "DeathStack/AirBrakesController/AirBrakesController.h" + +/* ADA includes */ +#include <ADA/ADAController.h> + +/* Navigation System includes */ +//#include "NavigationSystem/NASController.h" + +using namespace std; +using namespace miosix; +using namespace DeathStackBoard; +using namespace NASConfigs; + +bool isSimulationRunning; + +/** + * structure that contains all the sensors used in the simulation + */ +struct StateComplete +{ + HILImu *imu; + HILBarometer *barometer; + HILGps *gps; + HILNas *nas; +}; + +uint8_t getNextSchedulerId(TaskScheduler *s) +{ + return (s->getTaskStats()).back().id + 1; +} + +void setIsSimulationRunning(bool running) { isSimulationRunning = running; } + +int main() +{ + isSimulationRunning = true; + + // crash if eigen dynamically allocates memory + Eigen::internal::set_is_malloc_allowed(false); + TimestampTimer::enableTimestampTimer(); + + // Definition of the flight phases manager + HILFlightPhasesManager *flightPhasesManager = + HILFlightPhasesManager::getInstance(); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&setIsSimulationRunning, false)); + + /*-------------- [TS] Task Scheduler --------------*/ + + TaskScheduler scheduler; + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); + + /*-------------- [HILT] HILTransceiver --------------*/ + + // Definition of the transceiver object + HILTransceiver *matlab = HILTransceiver::getInstance(); + + // registering the HILTransceiver in order to let him know when it has to + // wait to the control algorithm or not + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, bind(&HILTransceiver::setIsAirbrakePhase, matlab, true)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, bind(&HILTransceiver::setIsAirbrakePhase, matlab, false)); + + /*-------------- Sensors & Actuators --------------*/ + + // Definition of the fake sensors for the simulation + StateComplete state; + state.imu = new HILImu(matlab, N_DATA_IMU); + state.barometer = new HILBarometer(matlab, N_DATA_BARO); + state.gps = new HILGps(matlab, N_DATA_GPS); + + // Definition of the fake actuators for the simulation + HILServo servo(matlab); + servo.init(); + + /*--------------- [NAS] Navigation System ----------------*/ + // TRACE("Before NAS\n"); + NASController<HILImuData, HILBaroData, HILGpsData> nas_controller( + *state.imu, *state.barometer, *state.gps); + + state.nas = new HILNas(matlab, N_DATA_KALM, &nas_controller); + // TRACE("After NAS\n"); + /*--------------- [SM] Sensor Manager --------------*/ + + // instantiate the sensor manager with the given scheduler + SensorManager SM(&scheduler, {{state.imu, imuConfig}, + {state.barometer, baroConfig}, + {state.gps, gpsConfig}, + {state.nas, kalmConfig}}); + + // registering the enabling of the sensors to the flightPhasesManager + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, bind(&SensorManager::enableAllSensors, &SM)); + + /*---------------- [ADA] ADA ---------------*/ + ADAController<HILBaroData, HILGpsData> *ada_controller = + new ADAController<HILBaroData, HILGpsData>(*state.barometer, + *state.gps); + + // setDeploymentAltitude when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setDeploymentAltitude, + ada_controller, deploymentAltitude)); + + // setReferenceAltitude when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceAltitude, + ada_controller, referenceAltitude)); + + // setReferenceTemperature when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceTemperature, + ada_controller, referenceTemperature)); + + /*-------------- [CA] Control Algorithm --------------*/ + + // definition of the control algorithm + AirBrakesController<HILNasData> airbrakeController(*state.nas, &servo); + + /*-------------- Events --------------*/ + + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_FLIGHT_EVENTS); + counter.subscribe(TOPIC_ADA); + counter.subscribe(TOPIC_NAS); + + /* INSERT THE SUBSCRIBING FOR THE EVENTBROKER (inserted in the + * flightPhasesManager object) */ + + /*-------------- Adding tasks to scheduler --------------*/ + + // adding the updating of the ADA to the scheduler + { + TaskScheduler::function_t update_ADA{bind( + &ADAController<HILBaroData, HILGpsData>::update, ada_controller)}; + + scheduler.add(update_ADA, (uint32_t)(1000 / ADA_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the updating for the navigation system + { + TaskScheduler::function_t update_NAS{ + bind(&NASController<HILImuData, HILBaroData, HILGpsData>::update, + &nas_controller)}; + + scheduler.add(update_NAS, (uint32_t)(1000 / KALM_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the updating of the algorithm to the scheduler + { + TaskScheduler::function_t update_Airbrake{bind( + &AirBrakesController<HILNasData>::update, &airbrakeController)}; + + scheduler.add(update_Airbrake, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + /*---------- Starting threads --------*/ + + matlab->start(); + ada_controller->start(); + nas_controller.start(); + airbrakeController.start(); + sEventBroker->start(); + scheduler.start(); // started only the scheduler instead of the SM + + /*---------- Normal execution --------*/ + while (isSimulationRunning) + { + Thread::sleep(1000 * ADAConfigs::SAMPLING_PERIOD); + } + + return 0; +} \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA/HILSimulationConfig.h b/src/tests/hardware_in_the_loop/test-HIL+ADA/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..40d10858bf503a651e6ee35c57c3131c31494b09 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA/HILSimulationConfig.h @@ -0,0 +1,145 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" +#include "sensors/SensorInfo.h" + +struct SensorConfig : public SensorInfo +{ + SensorConfig(const uint32_t freq) : SensorInfo{freq, []() {}, false, false} + { + } +}; + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 2; + +/** baudrate of connection */ +const int SIM_BAUDRATE = 115200; + +/** Period of simulation in milliseconds */ +const int SIMULATION_PERIOD = 100; + +/** sample frequency of sensor (samples/second) */ +const int ACCEL_FREQ = 100; +const int GYRO_FREQ = 100; +const int MAGN_FREQ = 100; +const int BARO_FREQ = 20; +const int GPS_FREQ = 10; + +/** update frequency of the Navigation System */ +const int KALM_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int CONTROL_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int ADA_FREQ = 20; + +/** min and max values in radiants of the actuator */ +const float MinAlphaDegree = 0.0; +const float MaxAlphaDegree = 48 * 3.14 / 180; + +/** ADA configs + * [TODO?] Prendi valori da config ADA + */ +float deploymentAltitude = 100; +float referenceAltitude = 109; // launchpad Altitude for Pont De Sor? +float referenceTemperature = 15; + +/** sensors configuration */ +SensorConfig accelConfig(ACCEL_FREQ); +SensorConfig gyroConfig(GYRO_FREQ); +SensorConfig magnConfig(MAGN_FREQ); +SensorConfig baroConfig(BARO_FREQ); +SensorConfig gpsConfig(GPS_FREQ); +SensorConfig kalmConfig(KALM_FREQ); + +/** Number of samples per sensor at each simulator iteration */ +const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; +const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + */ +struct SensorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + // bool fix; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c7b21dd23993d243ce536155b6ab25e375c4bd61 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp @@ -0,0 +1,271 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#define EIGEN_RUNTIME_NO_MALLOC // enable eigen malloc usage assert + +#include <Common.h> +#include <events/EventBroker.h> +#include <events/Events.h> + +#include <algorithm> +#include <cmath> +#include <iostream> +#include <random> +#include <sstream> +#include <utils/testutils/catch.hpp> + +#include "TimestampTimer.h" +#include "miosix.h" +#include "scheduler/TaskScheduler.h" +#include "sensors/SensorManager.h" + +/* HIL includes */ +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/HIL_actuators/HILServo.h" +#include "hardware_in_the_loop/HIL_sensors/HILAccelerometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILGps.h" +#include "hardware_in_the_loop/HIL_sensors/HILGyroscope.h" +#include "hardware_in_the_loop/HIL_sensors/HILKalman.h" +#include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h" + +/* Airbrakes includes */ +#include "hardware_in_the_loop/HIL_algorithms/MockAirbrakeAlgorithm.h" + +/* ADA includes */ +#include <ADA/ADA.h> +#include <ADA/ADAController.h> +#include <configs/ADAconfig.h> +#include <events/FSM.h> +#include <events/utils/EventCounter.h> + +using namespace std; +using namespace miosix; +using namespace DeathStackBoard; + +bool isSimulationRunning; + +/** + * structure that contains all the sensors used in the simulation + */ +struct StateComplete +{ + HILAccelerometer *accelerometer; + HILBarometer *barometer; + HILGps *gps; + HILGyroscope *gyro; + HILMagnetometer *magnetometer; + HILKalman *kalman; +}; + +uint8_t getNextSchedulerId(TaskScheduler *s) +{ + return (s->getTaskStats()).back().id + 1; +} + +void setIsSimulationRunning(bool running) { isSimulationRunning = running; } + +int main() +{ + isSimulationRunning = true; + + // crash if eigen dynamically allocates memory + Eigen::internal::set_is_malloc_allowed(false); + + TimestampTimer::enableTimestampTimer(); + + // Definition of the flight phases manager + HILFlightPhasesManager *flightPhasesManager = + HILFlightPhasesManager::getInstance(); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&setIsSimulationRunning, false)); + + /*-------------- [TS] Task Scheduler --------------*/ + + TaskScheduler scheduler; + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); + + /*-------------- [HILT] HILTransceiver --------------*/ + + // Definition of the transceiver object + HILTransceiver *matlab = HILTransceiver::getInstance(); + + // registering the HILTransceiver in order to let him know when it has to + // wait to the control algorithm or not + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, bind(&HILTransceiver::setIsAirbrakePhase, matlab, true)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, bind(&HILTransceiver::setIsAirbrakePhase, matlab, false)); + + /*-------------- Sensors & Actuators --------------*/ + + // Definition of the fake sensors for the simulation + StateComplete state; + state.accelerometer = new HILAccelerometer(matlab, N_DATA_ACCEL); + state.barometer = new HILBarometer(matlab, N_DATA_BARO); + state.gps = new HILGps(matlab, N_DATA_GPS); + state.gyro = new HILGyroscope(matlab, N_DATA_GYRO); + state.magnetometer = new HILMagnetometer(matlab, N_DATA_MAGN); + state.kalman = new HILKalman(matlab, N_DATA_KALM); + + // Definition of the fake actuators for the simulation + HILServo servo(matlab); + servo.init(); + + /*-------------- [SM] Sensor Manager --------------*/ + + // instantiate the sensor manager with the given scheduler + SensorManager SM(&scheduler, {{state.accelerometer, accelConfig}, + {state.barometer, baroConfig}, + {state.gps, gpsConfig}, + {state.gyro, gyroConfig}, + {state.magnetometer, magnConfig}, + {state.kalman, kalmConfig}}); + + // registering the enabling of the sensors to the flightPhasesManager + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.accelerometer)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.barometer)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, bind(&SensorManager::enableSensor, &SM, state.gps)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.gyro)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.magnetometer)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.kalman)); + + /*---------------- [ADA] ADA ---------------*/ + ADAController<HILBaroData, HILGpsData> *ada_controller = + new ADAController<HILBaroData, HILGpsData>(*state.barometer, + *state.gps); + + // setDeploymentAltitude when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setDeploymentAltitude, + ada_controller, deploymentAltitude)); + + // setReferenceAltitude when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceAltitude, + ada_controller, referenceAltitude)); + + // setReferenceTemperature when starting calibration + flightPhasesManager->registerToFlightPhase( + CALIBRATION, + bind(&ADAController<HILBaroData, HILGpsData>::setReferenceTemperature, + ada_controller, referenceTemperature)); + + /*-------------- [CA] Control Algorithm --------------*/ + + // definition of the control algorithm + MockAirbrakeAlgorithm<HILKalmanData> mockAirbrake(state.kalman, &servo); + + // registering the starting and ending of the algorithm in base of the phase + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, + bind(&MockAirbrakeAlgorithm<HILKalmanData>::begin, &mockAirbrake)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, + bind(&MockAirbrakeAlgorithm<HILKalmanData>::end, &mockAirbrake)); + + /*-------------- Events --------------*/ + + EventCounter counter{*sEventBroker}; + counter.subscribe({TOPIC_FLIGHT_EVENTS}); + + counter.subscribe({TOPIC_ADA}); + /* INSERT THE SUBSCRIBING FOR THE EVENTBROKER (inserted in the + * flightPhasesManager object) */ + + /*-------------- Adding tasks to scheduler --------------*/ + + // adding the updating of the ADA to the scheduler + { + TaskScheduler::function_t update_ADA{bind( + &ADAController<HILBaroData, HILGpsData>::update, ada_controller)}; + + scheduler.add(update_ADA, (uint32_t)(1000 / ADA_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the updating of the algorithm to the scheduler + { + TaskScheduler::function_t update_Airbrake{ + bind(&MockAirbrakeAlgorithm<HILKalmanData>::update, &mockAirbrake)}; + + scheduler.add(update_Airbrake, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the Idle updating of the aperture when airbrakes are disabled + { + TaskScheduler::function_t update_Idle{ + bind(&HILTransceiver::setIdleActuatorData, matlab)}; + + scheduler.add(update_Idle, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + /*---------- Starting threads --------*/ + + matlab->start(); + ada_controller->start(); + sEventBroker->start(); + scheduler.start(); // started only the scheduler instead of the SM + + /*---------- Normal execution --------*/ + + bool apogeeReached = false; + + while (isSimulationRunning) + { + // [TODO] metti nel taskscheduler + if (!apogeeReached && counter.getCount({EV_ADA_APOGEE_DETECTED}) > 0) + { + flightPhasesManager->setFlagsFlightPhases(APOGEE, true); + apogeeReached = true; + } + + Thread::sleep(1000 * ADAConfigs::SAMPLING_PERIOD); + } + + return 0; +} diff --git a/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/HILAerobrakeConfig.h b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/HILAerobrakeConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..822e45ba90cb6fe4c99a1403e93956757d06a2a1 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/HILAerobrakeConfig.h @@ -0,0 +1,84 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" + +const float SIM_BAUDRATE = 256000; /**< baudrate of connection */ +const float SIMULATION_PERIOD = + 100; /**< Period of simulation in milliseconds */ + +const int N_DATA_ACCEL = 10; /**< Number of simulations */ +const int N_DATA_GYRO = 10; /**< Number of simulations */ +const int N_DATA_MAGN = 10; /**< Number of simulations */ +const int N_DATA_BARO = 2; /**< Number of simulations */ +const int N_DATA_GPS = 1; /**< Number of simulations */ +const int N_DATA_KALM = 1; /**< Number of simulations */ + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by every mock sensor in order to get the data + * they need + */ +struct SensorData +{ + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/HILSimulationConfig.h b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..82ecde3d49ebd677adcdb4f3bde8b935805829ca --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/HILSimulationConfig.h @@ -0,0 +1,134 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" +#include "sensors/SensorInfo.h" + +struct SensorConfig : public SensorInfo +{ + SensorConfig(const uint32_t freq) : SensorInfo{freq, []() {}, false, true} + { + } +}; + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 3; + +/** baudrate of connection */ +const int SIM_BAUDRATE = 115200; + +/** Period of simulation in milliseconds */ +const int SIMULATION_PERIOD = 100; + +/** sample frequency of sensor (samples/second) */ +const int ACCEL_FREQ = 100; +const int GYRO_FREQ = 100; +const int MAGN_FREQ = 100; +const int BARO_FREQ = 20; +const int GPS_FREQ = 10; + +/** update frequency of the Navigation System */ +const int KALM_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int CONTROL_FREQ = 10; + +/** min and max values in radiants of the actuator */ +const float MinAlphaDegree = 0.0; +const float MaxAlphaDegree = 0.84; + +/** sensors configuration */ +SensorConfig accelConfig(ACCEL_FREQ); +SensorConfig gyroConfig(GYRO_FREQ); +SensorConfig magnConfig(MAGN_FREQ); +SensorConfig baroConfig(BARO_FREQ); +SensorConfig gpsConfig(GPS_FREQ); +SensorConfig kalmConfig(KALM_FREQ); + +/** Number of samples per sensor at each simulator iteration */ +const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; // 10 +const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; // 10 +const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; // 10 +const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; // 2 +const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; // 1 +const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; // 1 + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + */ +struct SensorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8f250978b01dc55cc5fb7bb039d07b5f470f4c83 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp @@ -0,0 +1,200 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Common.h> +#include <events/EventBroker.h> + +#include "TimestampTimer.h" +#include "miosix.h" +#include "scheduler/TaskScheduler.h" +#include "sensors/SensorManager.h" + +/* HIL includes */ +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/HIL_actuators/HILServo.h" +#include "hardware_in_the_loop/HIL_sensors/HILAccelerometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h" +#include "hardware_in_the_loop/HIL_sensors/HILGps.h" +#include "hardware_in_the_loop/HIL_sensors/HILGyroscope.h" +#include "hardware_in_the_loop/HIL_sensors/HILKalman.h" +#include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h" +#include "hardware_in_the_loop/events/Events.h" +#include "hardware_in_the_loop/events/Topics.h" + +/* Airbrakes includes */ +#include "DeathStack/AirBrakesController/AirBrakesControlAlgorithm.h" + +using namespace std; +using namespace miosix; +using namespace DeathStackBoard; + +bool isSimulationRunning; + +/** + * structure that contains all the sensors used in the simulation + */ +struct StateComplete +{ + HILAccelerometer *accelerometer; + HILBarometer *barometer; + HILGps *gps; + HILGyroscope *gyro; + HILMagnetometer *magnetometer; + HILKalman *kalman; +}; + +uint8_t getNextSchedulerId(TaskScheduler *s) +{ + return (s->getTaskStats()).back().id + 1; +} + +void setIsSimulationRunning(bool running) { isSimulationRunning = running; } + +int main() +{ + isSimulationRunning = true; + + TimestampTimer::enableTimestampTimer(); + + // Definition of the flight phases manager + HILFlightPhasesManager *flightPhasesManager = + HILFlightPhasesManager::getInstance(); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&setIsSimulationRunning, false)); + + /*-------------- [TS] Task Scheduler --------------*/ + + TaskScheduler scheduler; + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); + + /*-------------- [HILT] HILTransceiver --------------*/ + + // Definition of the transceiver object + HILTransceiver *matlab = HILTransceiver::getInstance(); + + // registering the HILTransceiver in order to let him know when it has to + // wait to the control algorithm or not + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, bind(&HILTransceiver::setIsAirbrakePhase, matlab, true)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, bind(&HILTransceiver::setIsAirbrakePhase, matlab, false)); + + /*-------------- Sensors & Actuators --------------*/ + + // Definition of the fake sensors for the simulation + StateComplete state; + state.accelerometer = new HILAccelerometer(matlab, N_DATA_ACCEL); + state.barometer = new HILBarometer(matlab, N_DATA_BARO); + state.gps = new HILGps(matlab, N_DATA_GPS); + state.gyro = new HILGyroscope(matlab, N_DATA_GYRO); + state.magnetometer = new HILMagnetometer(matlab, N_DATA_MAGN); + state.kalman = new HILKalman(matlab, N_DATA_KALM); + + // Definition of the fake actuators for the simulation + HILServo servo(matlab); + servo.init(); + + /*-------------- [SM] Sensor Manager --------------*/ + + // instantiate the sensor manager with the given scheduler + SensorManager SM(&scheduler, {{state.accelerometer, accelConfig}, + {state.barometer, baroConfig}, + {state.gps, gpsConfig}, + {state.gyro, gyroConfig}, + {state.magnetometer, magnConfig}, + {state.kalman, kalmConfig}}); + + // registering the enabling of the sensors to the flightPhasesManager + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.accelerometer)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.barometer)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, bind(&SensorManager::enableSensor, &SM, state.gps)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.gyro)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.magnetometer)); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, + bind(&SensorManager::enableSensor, &SM, state.kalman)); + + /*-------------- [CA] Control Algorithm --------------*/ + + // definition of the control algorithm + AirBrakesControllerAlgorithm<HILKalmanData> airbrakeCA(state.kalman, + &servo); + + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, + bind(&AirBrakesControllerAlgorithm<HILKalmanData>::begin, &airbrakeCA)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, + bind(&AirBrakesControllerAlgorithm<HILKalmanData>::end, &airbrakeCA)); + + /*-------------- Adding tasks to scheduler --------------*/ + + // adding the updating of the algorithm to the scheduler + { + TaskScheduler::function_t update_Airbrake{bind( + &AirBrakesControllerAlgorithm<HILKalmanData>::update, &airbrakeCA)}; + + scheduler.add(update_Airbrake, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + // adding the Idle updating of the aperture when airbrakes are disabled + { + TaskScheduler::function_t update_Idle{ + bind(&HILTransceiver::setIdleActuatorData, matlab)}; + + scheduler.add(update_Idle, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + /*---------- Starting threads --------*/ + + matlab->start(); + scheduler.start(); // started only the scheduler instead of the SM + + /*---------- Normal execution --------*/ + + while (isSimulationRunning) + { + } + + return 0; +} diff --git a/src/tests/hardware_in_the_loop/test-HIL/HILSimulationConfig.h b/src/tests/hardware_in_the_loop/test-HIL/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..2b04ce015aae9439a85845db10bd8f01be3a67d8 --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL/HILSimulationConfig.h @@ -0,0 +1,134 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" +#include "sensors/SensorInfo.h" + +struct SensorConfig : public SensorInfo +{ + SensorConfig(const uint32_t freq) : SensorInfo{freq, []() {}, false, true} + { + } +}; + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 2; + +/** baudrate of connection */ +const int SIM_BAUDRATE = 115200; + +/** Period of simulation in milliseconds */ +const int SIMULATION_PERIOD = 100; + +/** sample frequency of sensor (samples/second) */ +const int ACCEL_FREQ = 100; +const int GYRO_FREQ = 100; +const int MAGN_FREQ = 100; +const int BARO_FREQ = 20; +const int GPS_FREQ = 10; + +/** update frequency of the Navigation System */ +const int KALM_FREQ = 10; + +/** update frequency of airbrakes control algorithm */ +const int CONTROL_FREQ = 10; + +/** min and max values in radiants of the actuator */ +const float MinAlphaDegree = 0.0; +const float MaxAlphaDegree = 0.84; + +/** sensors configuration */ +SensorConfig accelConfig(ACCEL_FREQ); +SensorConfig gyroConfig(GYRO_FREQ); +SensorConfig magnConfig(MAGN_FREQ); +SensorConfig baroConfig(BARO_FREQ); +SensorConfig gpsConfig(GPS_FREQ); +SensorConfig kalmConfig(KALM_FREQ); + +/** Number of samples per sensor at each simulator iteration */ +const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; // 10 +const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; // 10 +const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; // 10 +const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; // 2 +const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; // 1 +const int N_DATA_KALM = (KALM_FREQ * SIMULATION_PERIOD) / 1000; // 1 + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + */ +struct SimulatorData +{ +public: + struct Accelerometer + { + Vec3 measures[N_DATA_ACCEL]; + } accelerometer; + + struct Gyro + { + Vec3 measures[N_DATA_GYRO]; + } gyro; + + struct Magnetometer + { + Vec3 measures[N_DATA_MAGN]; + } magnetometer; + + struct Gps + { + Vec3 positionMeasures[N_DATA_GPS]; + Vec3 velocityMeasures[N_DATA_GPS]; + } gps; + + struct Barometer + { + float measures[N_DATA_BARO]; + } barometer; + + struct Kalman + { + float z; + float vz; + float vMod; + } kalman; + + struct Flags + { + float flag_flight; + float flag_ascent; + float flag_burning; + float flag_airbrakes; + float flag_para1; + float flag_para2; + } flags; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = float; \ No newline at end of file diff --git a/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp b/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0658ee79182f62b2ab497d3812acf48c946953cd --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp @@ -0,0 +1,160 @@ +/* Copyright (c) 2020-2021 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Common.h> +#include <events/EventBroker.h> + +#include "TimestampTimer.h" +#include "miosix.h" +#include "scheduler/TaskScheduler.h" +#include "sensors/SensorManager.h" + +/* HIL includes */ +#include "hardware_in_the_loop/HIL.h" + +using namespace std; +using namespace miosix; +using namespace DeathStackBoard; + +bool isSimulationRunning; + +/** + * structure that contains all the sensors used in the simulation + */ +struct StateComplete +{ + HILAccelerometer *accelerometer; + HILBarometer *barometer; + HILGps *gps; + HILGyroscope *gyro; + HILMagnetometer *magnetometer; + HILKalman *kalman; +}; + +uint8_t getNextSchedulerId(TaskScheduler *s) +{ + return (s->getTaskStats()).back().id + 1; +} + +void setIsSimulationRunning(bool running) { isSimulationRunning = running; } + +int main() +{ + isSimulationRunning = true; + + TimestampTimer::enableTimestampTimer(); + + // Definition of the flight phases manager + HILFlightPhasesManager *flightPhasesManager = + HILFlightPhasesManager::getInstance(); + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&setIsSimulationRunning, false)); + + /*-------------- [TS] Task Scheduler --------------*/ + + TaskScheduler scheduler; + + flightPhasesManager->registerToFlightPhase( + SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); + + /*-------------- [HILT] HILTransceiver --------------*/ + + // Definition of the transceiver object + HILTransceiver *matlab = HILTransceiver::getInstance(); + + // registering the HILTransceiver in order to let him know when it has to + // wait to the control algorithm or not + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, bind(&HILTransceiver::setIsAirbrakePhase, matlab, true)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, bind(&HILTransceiver::setIsAirbrakePhase, matlab, false)); + + /*-------------- Sensors & Actuators --------------*/ + + // Definition of the fake sensors for the simulation + StateComplete state; + state.accelerometer = new HILAccelerometer(matlab, N_DATA_ACCEL); + state.barometer = new HILBarometer(matlab, N_DATA_BARO); + state.gps = new HILGps(matlab, N_DATA_GPS); + state.gyro = new HILGyroscope(matlab, N_DATA_GYRO); + state.magnetometer = new HILMagnetometer(matlab, N_DATA_MAGN); + state.kalman = new HILKalman(matlab, N_DATA_KALM); + + // Definition of the fake actuators for the simulation + HILServo servo(matlab); + servo.init(); + + /*-------------- [SM] Sensor Manager --------------*/ + + // instantiate the sensor manager with the given scheduler + SensorManager SM(&scheduler, {{state.accelerometer, accelConfig}, + {state.barometer, baroConfig}, + {state.gps, gpsConfig}, + {state.gyro, gyroConfig}, + {state.magnetometer, magnConfig}, + {state.kalman, kalmConfig}}); + + // registering the enabling of the sensors to the flightPhasesManager + flightPhasesManager->registerToFlightPhase( + SIMULATION_STARTED, bind(&SensorManager::enableAllSensors, &SM)); + + /*-------------- [CA] Control Algorithm --------------*/ + + // definition of the control algorithm + MockAirbrakeAlgorithm<HILKalmanData> mockAirbrake(state.kalman, &servo); + + // registering the starting and ending of the algorithm in base of the phase + flightPhasesManager->registerToFlightPhase( + AEROBRAKES, + bind(&MockAirbrakeAlgorithm<HILKalmanData>::begin, &mockAirbrake)); + + flightPhasesManager->registerToFlightPhase( + APOGEE, + bind(&MockAirbrakeAlgorithm<HILKalmanData>::end, &mockAirbrake)); + + /*-------------- Adding tasks to scheduler --------------*/ + + // adding the updating of the algorithm to the scheduler + { + TaskScheduler::function_t update_Airbrake{ + bind(&MockAirbrakeAlgorithm<HILKalmanData>::update, &mockAirbrake)}; + + scheduler.add(update_Airbrake, (uint32_t)(1000 / CONTROL_FREQ), + getNextSchedulerId(&scheduler)); + } + + /*---------- Starting threads --------*/ + + matlab->start(); + scheduler.start(); // started only the scheduler instead of the SM + + /*---------- Normal execution --------*/ + + while (isSimulationRunning) + { + Thread::sleep(1000); + } + + return 0; +} diff --git a/src/tests/hardware_in_the_loop/test-SerialInterface/HILSimulationConfig.h b/src/tests/hardware_in_the_loop/test-SerialInterface/HILSimulationConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..d6a11c2c7f9e77946fb1b1ee48a151a8dde2f7ce --- /dev/null +++ b/src/tests/hardware_in_the_loop/test-SerialInterface/HILSimulationConfig.h @@ -0,0 +1,47 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include "math/Vec3.h" + +/** serial port number */ +const int SIM_SERIAL_PORT_NUM = 2; +/** baudrate of connection */ +const int SIM_BAUDRATE = 256000; + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by every mock sensor in order to get the data + * they need + */ +struct SimulatorData +{ + float data[6]; +}; + +/** + * @brief Data structure expected by the simulator + */ +using ActuatorData = SimulatorData; \ No newline at end of file diff --git a/src/tests/test-canproxy.cpp b/src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp similarity index 58% rename from src/tests/test-canproxy.cpp rename to src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp index 6b3fe0f7c39370907fddbafe31ebe1a728dfb78c..62cf5a6e3f52a33a7cb00a9c8eb3106f8a6444d8 100644 --- a/src/tests/test-canproxy.cpp +++ b/src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Alvise de'Faveri Tron +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Emilio Corigliano * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -21,34 +21,35 @@ */ #include <Common.h> -#include <miosix.h> -#include "boards/DeathStack/Canbus/CanProxy.h" -#include "boards/CanInterfaces.h" -#include <events/EventBroker.h> +#include "hardware_in_the_loop/HILConfig.h" +#include "hardware_in_the_loop/simulator_communication/SerialInterface.h" +#include "math/Vec3.h" using namespace miosix; -using namespace CanInterfaces; -using namespace DeathStackBoard; int main() { - CanManager* can_mgr = new CanManager(CAN1); - CanProxy* can = new CanProxy(can_mgr); + SerialInterface simulation(115200); + SimulatorData sensorData; + ActuatorData data{10}; - sEventBroker->start(); - - const char *pkt = "TestMSG"; + if (!simulation.init()) + { + TRACE("Wrong initialization\n"); + return -1; + } - while (1) + for (;;) { - TRACE("[CAN] Sending \n"); - ledOn(); - can->send(CAN_TOPIC_IGNITION, (const uint8_t *)pkt, strlen(pkt)); - //socket.receive(buf, 64); - Thread::sleep(50); - ledOff(); - - Thread::sleep(500); + simulation.sendData<ActuatorData>(&data); + simulation.recvData<SimulatorData>(&sensorData); } -} \ No newline at end of file + + return 0; +} + +/* +baudrate: 256000, avarage time: 0.0175 +structToSingles avg_time: 0.0003 +*/ \ No newline at end of file diff --git a/src/tests/ledwave.cpp b/src/tests/ledwave.cpp index 302f6fcabeefe8c84b829edebf955ecb67fdf144..3dbced42ab422062b27a4b1285f6619e83152e6e 100644 --- a/src/tests/ledwave.cpp +++ b/src/tests/ledwave.cpp @@ -1,46 +1,79 @@ +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Alvise de'Faveri Tron, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <interfaces-impl/hwmapping.h> #include <miosix.h> #include <stdio.h> -typedef miosix::Gpio<GPIOG_BASE, 2> led1; -typedef miosix::Gpio<GPIOG_BASE, 3> led2; -typedef miosix::Gpio<GPIOD_BASE, 11> led3; +using led1 = miosix::leds::led_red1; +using led2 = miosix::leds::led_red2; +using led3 = miosix::leds::led_blue1; + +/* NOTE: + * These are conencted to the enable pin of the thermal + * cutters and the cs of the lis3mdl magnetometer + */ +using led4 = miosix::leds::led_blue2; +using led5 = miosix::leds::led_green1; +using led6 = miosix::leds::led_green2; -typedef miosix::Gpio<GPIOB_BASE, 1> led4; -typedef miosix::Gpio<GPIOC_BASE, 4> led5; -typedef miosix::Gpio<GPIOA_BASE, 4> led6; +// Test timeout +constexpr int TEST_TIMEOUT = 10; // seconds int main() { - // DeathStack board; - led1::mode(miosix::Mode::OUTPUT); - led2::mode(miosix::Mode::OUTPUT); - led3::mode(miosix::Mode::OUTPUT); - led4::mode(miosix::Mode::OUTPUT); - led5::mode(miosix::Mode::OUTPUT); - led6::mode(miosix::Mode::OUTPUT); - - printf("Started\n"); - FILE* file = fopen("/sd/log.txt", "w"); + printf("Waving!\n"); - if (file == NULL) - printf("Error opening log file\n"); - - while (1) + // Sampling + for (int i = 0; i < TEST_TIMEOUT; i++) { - printf("Working\n"); - led1::high(); - led2::high(); + // On led3::high(); - led4::high(); + miosix::delayMs(500 / 6); + led1::high(); + miosix::delayMs(500 / 6); led5::high(); + miosix::delayMs(500 / 6); led6::high(); - miosix::Thread::sleep(100); - led1::low(); - led2::low(); + miosix::delayMs(500 / 6); + led2::high(); + miosix::delayMs(500 / 6); + led4::high(); + miosix::delayMs(500 / 6); + + // Off led3::low(); - led4::low(); + miosix::delayMs(500 / 6); + led1::low(); + miosix::delayMs(500 / 6); led5::low(); + miosix::delayMs(500 / 6); led6::low(); - miosix::Thread::sleep(100); + miosix::delayMs(500 / 6); + led2::low(); + miosix::delayMs(500 / 6); + led4::low(); + miosix::delayMs(500 / 6); } + + return 0; } \ No newline at end of file diff --git a/src/tests/mock_sensors/test-mock-data.cpp b/src/tests/mock_sensors/test-mock-data.cpp index 35d837d7d35baf323abef2a007ca14fbc4f369d0..0c7d0f7fdd7bb1b613a9b47187927f2f9ca302ac 100644 --- a/src/tests/mock_sensors/test-mock-data.cpp +++ b/src/tests/mock_sensors/test-mock-data.cpp @@ -1,30 +1,14664 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli * - * Permission is hereby grantedf, free of chargef, to any person obtaining a - * copy of this software and associated documentation files (the "Software")f, - * to deal in the Software without restrictionf, including without limitation - * the rights to usef, copyf, modifyf, mergef, publishf, distributef, - * sublicensef, and/or sell copies of the Softwaref, and to permit persons to - * whom the Software is furnished to do sof, subject to the following - * conditions: + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * - * THE SOFTWARE IS PROVIDED "AS IS"f, WITHOUT WARRANTY OF ANY KINDf, EXPRESS OR - * IMPLIEDf, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIMf, DAMAGES OR OTHER - * LIABILITYf, WHETHER IN AN ACTION OF CONTRACTf, TORT OR OTHERWISEf, ARISING - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS - * IN THE SOFTWARE. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. */ #include "test-mock-data.h" -const float SIMULATED_PRESSURE[PRESSURE_DATA_SIZE] = {85395.507810f, 85395.507810f, 85395.507810f, 85395.507810f, 85390.819442f, 85369.508680f, 85348.191968f, 85326.875002f, 85310.250494f, 85297.678567f, 85276.361607f, 85235.438738f, 85194.089845f, 85160.101932f, 85144.219120f, 85208.157612f, 85135.692339f, 85075.160717f, 85041.909722f, 84978.397445f, 84935.769715f, 84860.744916f, 84807.458706f, 84773.354536f, 84698.757560f, 84697.265620f, 84666.573660f, 84643.554690f, 84643.554690f, 84643.554690f, 84683.624749f, 84668.278769f, 84607.747397f, 84525.901660f, 84461.956969f, 84378.832586f, 84308.479082f, 84267.578120f, 84267.578120f, 84267.578120f, 84285.058034f, 84282.499998f, 84248.396572f, 84183.179063f, 84087.265126f, 83949.787949f, 83833.839041f, 83781.833210f, 83824.460940f, 83824.460940f, 83761.371899f, 83665.887273f, 83592.143721f, 83634.771451f, 83554.631326f, 83491.754340f, 83396.265997f, 83266.250004f, 83223.628473f, 83161.389635f, 83045.652345f, 82926.293278f, 82850.843624f, 82765.588171f, 82750.242190f, 82654.756079f, 82509.821802f, 82425.419020f, 82361.480528f, 82272.601190f, 82193.314484f, 82118.291171f, 82011.718750f, 81905.146329f, 81830.123016f, 81740.607145f, 81635.104046f, 81571.159348f, 81548.143229f, 81482.071178f, 81340.547121f, 81158.100449f, 81115.472718f, 81023.821678f, 80910.644530f, 80808.339033f, 80747.810263f, 80726.493303f, 80652.746779f, 80549.588544f, 80441.954614f, 80357.551592f, 80293.606894f, 80246.289430f, 80181.495285f, 80101.782242f, 80037.843750f, 79973.899060f, 79902.071678f, 79801.897322f, 79688.080606f, 79624.135908f, 79560.197168f, 79496.258676f, 79413.561258f, 79312.954228f, 79239.522830f, 79174.629469f, 79111.947842f, 79049.305683f, 78987.066342f, 78944.438611f, 78884.336057f, 78796.522569f, 78704.727901f, 78647.414639f, 78590.101377f, 78532.788115f, 78475.474853f, 78418.161590f, 78360.848328f, 78303.535066f, 78246.221804f, 78188.908542f, 78131.595279f, 78074.282017f, 78016.968755f, 77959.655493f, 77902.342231f, 77845.028968f, 77787.715706f, 77743.619423f, 77675.388822f, 77599.110365f, 77536.018353f, 77482.732139f, 77438.190970f, 77347.819447f, 77251.903649f, 77228.887522f, 77164.942832f, 77117.630211f, 77063.920637f, 77011.060762f, 76989.750000f, 76968.433032f, 76962.890620f, 76906.622765f, 76805.809399f, 76763.181669f, 76700.089657f, 76636.149306f, 76562.831351f, 76489.084826f, 76467.767857f, 76446.456599f, 76398.925780f, 76344.373654f, 76303.011535f, 76271.042289f, 76230.546256f, 76164.899551f, 76091.579858f, 76049.378475f, 76028.067705f, 75990.551340f, 75937.265134f, 75883.555808f, 75862.238839f, 75820.043407f, 75777.412946f, 75739.684770f, 75705.583834f, 75662.956104f, 75620.328374f, 75534.222469f, 75454.510413f, 75418.703120f, 75418.703120f, 75380.338170f, 75338.549881f, 75304.146730f, 75283.143234f, 75261.832464f, 75223.468132f, 75180.840406f, 75144.179689f, 75122.649675f, 75090.680429f, 75066.811631f, 75013.528524f, 74943.618308f, 74922.301339f, 74880.099824f, 74852.820311f, 74811.897942f, 74756.480036f, 74735.169266f, 74713.852677f, 74678.044393f, 74635.416667f, 74596.198665f, 74574.881697f, 74553.570314f, 74512.844591f, 74471.625364f, 74450.627232f, 74429.310272f, 74425.046880f, 74390.520094f, 74338.515629f, 74317.198785f, 74295.888023f, 74274.572544f, 74242.600195f, 74205.088537f, 74183.777775f, 74141.576384f, 74114.292409f, 74083.172494f, 74044.808158f, 74023.497396f, 74002.180807f, 73966.372523f, 73932.697920f, 73913.091523f, 73902.433038f, 73882.187497f, 73848.069471f, 73813.551218f, 73792.240449f, 73770.929687f, 73749.613843f, 73728.296878f, 73706.981771f, 73685.671010f, 73664.360248f, 73643.044645f, 73621.727677f, 73600.412322f, 73579.101560f, 73557.790798f, 73536.475443f, 73515.158479f, 73493.842882f, 73472.532120f, 73451.221351f, 73429.906248f, 73408.589287f, 73387.273441f, 73365.962672f, 73344.651909f, 73323.976565f, 73306.922995f, 73297.117190f, 73297.117190f, 73279.642359f, 73258.328122f, 73237.013021f, 73216.554690f, 73216.554690f, 73199.927455f, 73167.528894f, 73135.984380f, 73135.984380f, 73114.673610f, 73109.132810f, 73098.900669f, 73077.585072f, 73056.274310f, 73055.421880f, 73040.500002f, 73019.185765f, 72997.873886f, 72976.556917f, 72974.851560f, 72961.425780f, 72948.000000f, 72946.720982f, 72925.404014f, 72921.140620f, 72909.632809f, 72894.289060f, 72893.862721f, 72872.545760f, 72867.429690f, 72856.774305f, 72840.578120f, 72840.578120f, 72819.687499f, 72813.718750f, 72803.915800f, 72786.867190f, 72786.867190f, 72766.829240f, 72760.007810f, 72751.057290f, 72733.156250f, 72733.156250f, 72733.156250f, 72733.156250f, 72729.532365f, 72719.726560f, 72719.726560f, 72719.726560f, 72707.792533f, 72692.875000f, 72692.875000f, 72692.875000f, 72692.875000f, 72692.875000f, 72687.332588f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72662.178567f, 72640.861607f, 72658.767853f, 72652.980926f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72639.156250f, 72654.078122f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72666.015620f, 72683.069195f, 72692.875000f, 72692.875000f, 72692.875000f, 72692.875000f, 72692.875000f, 72703.530381f, 72719.726560f, 72719.726560f, 72719.726560f, 72724.330131f, 72730.723981f, 72737.117830f, 72743.511679f, 72749.905528f, 72756.299377f, 72760.007810f, 72763.418525f, 72784.735493f, 72786.867190f, 72800.086420f, 72813.718750f, 72813.718750f, 72813.718750f, 72831.198657f, 72840.578120f, 72846.971351f, 72867.429690f, 72867.429690f, 72884.056919f, 72903.674005f, 72921.721977f, 72939.769949f, 72957.817921f, 72975.865892f, 72993.913864f, 73011.961836f, 73030.009808f, 73048.057780f, 73066.105751f, 73084.153723f, 73102.201695f, 73120.249667f, 73138.297638f, 73156.345610f, 73174.393582f, 73192.441554f, 73210.489525f, 73228.537497f, 73246.585469f, 73264.633441f, 73282.681413f, 73300.729384f, 73318.777356f, 73336.825328f, 73354.873300f, 73372.921271f, 73390.969243f, 73409.017215f, 73427.065187f, 73445.113159f, 73463.161130f, 73481.209102f, 73499.257074f, 73517.305046f, 73535.353017f, 73553.400989f, 73571.448961f, 73589.496933f, 73607.544905f, 73625.592876f, 73643.640848f, 73661.688820f, 73679.736792f, 73697.784763f, 73715.832735f, 73733.880707f, 73751.928679f, 73769.976650f, 73788.024622f, 73806.072594f, 73824.120566f, 73842.168538f, 73860.216509f, 73878.264481f, 73896.312453f, 73914.360425f, 73932.408396f, 73950.456368f, 73968.504340f, 73986.552312f, 74004.600284f, 74022.648255f, 74040.696227f, 74058.744199f, 74076.792171f, 74094.840142f, 74126.652779f, 74162.887278f, 74184.203990f, 74205.514752f, 74226.830355f, 74259.228920f, 74296.314238f, 74317.625000f, 74338.941968f, 74360.254344f, 74391.797250f, 74434.424979f, 74477.052701f, 74499.222100f, 74505.617190f, 74519.683658f, 74553.571431f, 74596.199162f, 74619.220983f, 74648.197242f, 74681.119199f, 74714.041156f, 74746.963113f, 74779.885070f, 74812.807027f, 74845.728984f, 74878.650941f, 74911.572898f, 74944.494856f, 74977.416813f, 75017.784226f, 75081.728916f, 75083.007810f, 75098.351564f, 75134.371035f, 75185.525546f, 75228.153276f, 75230.710940f, 75259.697797f, 75302.325523f, 75341.543525f, 75362.860493f, 75384.171876f, 75431.509133f, 75486.907488f, 75527.829861f, 75549.140623f, 75587.931299f, 75642.496527f, 75693.651041f, 75714.535719f, 75725.194195f, 75744.161454f, 75787.641863f, 75846.044150f, 75888.671880f, 75909.988840f, 75931.301214f, 75973.076261f, 76032.329857f, 76074.951389f, 76097.120532f, 76132.888790f, 76184.083333f, 76241.206602f, 76262.517364f, 76293.420761f, 76344.595073f, 76399.993428f, 76442.621158f, 76485.248889f, 76510.396705f, 76555.582467f, 76606.736979f, 76628.900419f, 76671.528149f, 76714.155880f, 76767.348878f, 76820.302083f, 76841.612845f, 76883.814236f, 76903.424109f, 76938.594248f, 76998.274305f, 77040.895829f, 77083.529020f, 77111.663198f, 77159.830730f, 77241.676460f, 77305.621151f, 77331.195313f, 77378.532573f, 77433.928818f, 77474.845977f, 77496.162946f, 77526.214782f, 77588.024303f, 77660.491439f, 77703.119169f, 77745.746899f, 77771.752235f, 77826.317215f, 77900.489460f, 77943.117190f, 77964.427952f, 78017.287821f, 78076.113468f, 78127.051343f, 78169.685263f, 78212.307290f, 78254.937500f, 78306.945437f, 78370.883929f, 78434.822421f, 78479.160714f, 78547.367185f, 78622.817207f, 78684.198160f, 78716.167414f, 78756.876612f, 78811.441837f, 78862.496457f, 78884.333584f, 78948.278274f, 78996.022075f, 79049.305186f, 79113.245166f, 79177.189856f, 79220.243926f, 79262.871650f, 79320.205229f, 79384.144964f, 79405.455734f, 79446.810265f, 79474.944443f, 79523.111980f, 79598.136282f, 79619.447044f, 79659.948663f, 79754.619957f, 79843.669647f, 79869.249008f, 79943.845984f, 79992.228179f, 80070.664931f, 80158.053939f, 80200.681669f, 80243.309399f, 80285.937130f, 80350.731275f, 80419.361110f, 80440.671880f, 80483.299602f, 80502.270088f, 80569.196428f, 80683.013142f, 80746.957832f, 80790.432290f, 80833.062500f, 80903.826638f, 80998.883931f, 81041.511662f, 81084.139384f, 81132.203125f, 81194.760294f, 81276.605528f, 81319.233259f, 81361.860989f, 81440.294270f, 81533.774289f, 81596.739710f, 81660.678202f, 81692.224330f, 81729.522814f, 81785.362348f, 81849.307046f, 81892.361116f, 81965.680806f, 82050.936258f, 82131.928318f, 82195.866810f, 82219.741070f, 82255.549357f, 82311.604167f, 82384.496772f, 82448.441470f, 82512.380583f, 82613.523439f, 82716.993551f, 82779.659224f, 82822.286954f, 82864.914684f, 82936.316966f, 83011.248779f, 83032.226560f, 83074.854290f, 83133.251984f, 83227.886903f, 83337.017357f, 83379.638889f, 83463.188984f, 83511.148437f, 83557.187627f, 83617.290180f, 83659.917902f, 83702.545632f, 83797.605470f, 83899.913075f, 83967.267487f, 84073.833701f, 84119.230408f, 84181.039933f, 84247.113838f, 84270.135660f, 84334.074152f, 84398.017485f, 84484.125376f, 84579.610000f, 84643.554690f, 84686.182412f, 84705.152898f}; +const float SIMULATED_PRESSURE[PRESSURE_DATA_SIZE] = { + 101158.8, 100978.8, 101196, 101187.6, 101259.6, 101278.8, 101410.8, + 101190, 101308.8, 101184, 101368.8, 101226, 101270.4, 101332.8, + 101074.8, 101311.2, 101244, 101118, 101239.2, 101320.8, 101437.2, + 101168.4, 101379.6, 101335.2, 101284.8, 101161.2, 101217.6, 101184, + 101240.4, 101100, 101496, 101292, 101430, 101240.4, 101164.8, + 101160, 101188.8, 101266.8, 101103.6, 101215.2, 101298, 101134.8, + 101234.4, 101269.2, 101118, 101089.2, 101187.6, 101270.4, 101224.8, + 101120.4, 101208, 101166, 101133.6, 101103.6, 100971.6, 101043.6, + 101130, 101052, 101128.8, 101072.4, 101142, 101108.4, 101206.8, + 101082, 100965.6, 100947.6, 101005.2, 101016, 101218.8, 101043.6, + 101006.4, 101107.2, 101036.4, 101037.6, 101138.4, 101100, 101007.6, + 100960.8, 101024.4, 100856.4, 101028, 100891.2, 100898.4, 100896, + 100935.6, 101067.6, 100872, 101091.6, 101032.8, 101025.6, 100806, + 100920, 101070, 100870.8, 100953.6, 100938, 100918.8, 100812, + 100808.4, 100948.8, 100670.4, 100800, 100737.6, 100819.2, 100813.2, + 100795.2, 100722, 100680, 100638, 100646.4, 100654.8, 100784.4, + 100639.2, 100567.2, 100647.6, 100641.6, 100695.6, 100501.2, 100531.2, + 100416, 100681.2, 100518, 100485.6, 100521.6, 100320, 100417.2, + 100542, 100718.4, 100425.6, 100240.8, 100357.2, 100299.6, 100356, + 100302, 100356, 100334.4, 100273.2, 100389.6, 100345.2, 100228.8, + 100340.4, 100300.8, 100302, 100177.2, 100122, 100126.8, 100070.4, + 100144.8, 100143.6, 100162.8, 100190.4, 100058.4, 100245.6, 99991.2, + 100029.6, 99950.4, 100034.4, 100164, 99891.6, 100011.6, 99954, + 99751.2, 99742.8, 99972, 99973.2, 100005.6, 99957.6, 99794.4, + 99883.2, 99900, 99868.8, 99601.2, 99734.4, 99626.4, 99463.2, + 99621.6, 99766.8, 99610.8, 99584.4, 99516, 99852, 99553.2, + 99634.8, 99444, 99460.8, 99362.4, 99416.4, 99553.2, 99406.8, + 99357.6, 99172.8, 99350.4, 99264, 99370.8, 99346.8, 99195.6, + 99136.8, 99068.4, 99160.8, 99285.6, 99258, 99238.8, 99135.6, + 99087.6, 98980.8, 99030, 99008.4, 98827.2, 99152.4, 98905.2, + 98864.4, 98886, 98862, 98944.8, 98878.8, 98728.8, 98781.6, + 98881.2, 98820, 98584.8, 98696.4, 98640, 98684.4, 98701.2, + 98646, 98463.6, 98611.2, 98522.4, 98571.6, 98396.4, 98484, + 98529.6, 98280, 98431.2, 98576.4, 98430, 98300.4, 98344.8, + 98089.2, 98234.4, 98204.4, 98170.8, 98137.2, 98163.6, 98426.4, + 98166, 98001.6, 98002.8, 98018.4, 97959.6, 97941.6, 98044.8, + 97903.2, 98001.6, 97825.2, 97765.2, 97704, 97942.8, 97768.8, + 97868.4, 97839.6, 97524, 97736.4, 97744.8, 97558.8, 97543.2, + 97584, 97468.8, 97696.8, 97477.2, 97467.6, 97444.8, 97450.8, + 97336.8, 97222.8, 97311.6, 97261.2, 97311.6, 97322.4, 97209.6, + 96990, 96921.6, 97089.6, 97126.8, 96904.8, 97246.8, 96854.4, + 96962.4, 96975.6, 96841.2, 96883.2, 96992.4, 96818.4, 96850.8, + 96716.4, 96720, 96536.4, 96669.6, 96627.6, 96594, 96616.8, + 96649.2, 96679.2, 96250.8, 96608.4, 96550.8, 96444, 96391.2, + 96243.6, 96484.8, 96272.4, 96134.4, 96302.4, 96351.6, 96150, + 95992.8, 95953.2, 96084, 95959.2, 95928, 95853.6, 96062.4, + 95856, 95732.4, 95818.8, 95552.4, 95788.8, 95682, 95857.2, + 95695.2, 95877.6, 95874, 95703.6, 95401.2, 95408.4, 95317.2, + 95440.8, 95526, 95394, 95337.6, 95377.2, 95296.8, 95116.8, + 95212.8, 95193.6, 95072.4, 95325.6, 95186.4, 95103.6, 95152.8, + 94970.4, 94978.8, 95018.4, 94867.2, 95008.8, 94782, 94588.8, + 94724.4, 94802.4, 94836, 94778.4, 94693.2, 94819.2, 94399.2, + 94507.2, 94581.6, 94399.2, 94286.4, 94284, 94263.6, 94287.6, + 94470, 94189.2, 94225.2, 94077.6, 93970.8, 94102.8, 94173.6, + 94020, 94044, 94075.2, 94016.4, 93832.8, 93958.8, 93870, + 93718.8, 93607.2, 93849.6, 93698.4, 93459.6, 93648, 93782.4, + 93702, 93584.4, 93592.8, 93487.2, 93403.2, 93388.8, 93422.4, + 93453.6, 93423.6, 93349.2, 93588, 93259.2, 93172.8, 93105.6, + 93265.2, 93229.2, 93211.2, 93043.2, 93192, 93019.2, 93096, + 92859.6, 92995.2, 92919.6, 92800.8, 92846.4, 92710.8, 92834.4, + 92778, 92624.4, 92517.6, 92474.4, 92660.4, 92692.8, 92526, + 92426.4, 92493.6, 92310, 92371.2, 92377.2, 92310, 92226, + 92505.6, 92484, 92271.6, 92259.6, 92108.4, 92084.4, 92068.8, + 92042.4, 92175.6, 91982.4, 91990.8, 92082, 91821.6, 91922.4, + 91867.2, 91879.2, 92061.6, 91701.6, 91623.6, 91791.6, 91750.8, + 91747.2, 91723.2, 91620, 91633.2, 91692, 91576.8, 91429.2, + 91479.6, 91398, 91353.6, 91237.2, 91382.4, 91046.4, 91285.2, + 91117.2, 91224, 91206, 91333.2, 91119.6, 90898.8, 91126.8, + 90934.8, 90939.6, 90958.8, 90841.2, 90865.2, 90832.8, 90708, + 90910.8, 90794.4, 90675.6, 90715.2, 90808.8, 90870, 90576, + 90463.2, 90620.4, 90583.2, 90645.6, 90518.4, 90447.6, 90466.8, + 90517.2, 90344.4, 90320.4, 90429.6, 90330, 90188.4, 90339.6, + 90213.6, 90195.6, 90234, 90236.4, 90099.6, 90082.8, 90025.2, + 89965.2, 90201.6, 90210, 89840.4, 89968.8, 89850, 90031.2, + 89650.8, 89713.2, 89872.8, 89775.6, 89732.4, 89826, 89662.8, + 89712, 89490, 89534.4, 89570.4, 89502, 89392.8, 89643.6, + 89310, 89446.8, 89317.2, 89466, 89222.4, 89372.4, 89260.8, + 89356.8, 89193.6, 89211.6, 89274, 89140.8, 89103.6, 88778.4, + 88971.6, 88947.6, 89008.8, 88809.6, 88717.2, 88886.4, 89052, + 88734, 88762.8, 88780.8, 88588.8, 88574.4, 88945.2, 88728, + 88722, 88628.4, 88440, 88557.6, 88527.6, 88563.6, 88222.8, + 88446, 88627.2, 88358.4, 88374, 88381.2, 88231.2, 88154.4, + 88250.4, 88263.6, 88245.6, 88296, 88183.2, 88246.8, 88155.6, + 88136.4, 88075.2, 87933.6, 88063.2, 88101.6, 88249.2, 87756, + 87927.6, 88063.2, 87986.4, 87895.2, 87898.8, 87626.4, 87888, + 87520.8, 87901.2, 87666, 87642, 87726, 87430.8, 87616.8, + 87639.6, 87618, 87374.4, 87595.2, 87572.4, 87264, 87655.2, + 87199.2, 87409.2, 87432, 87156, 87302.4, 87121.2, 87034.8, + 87172.8, 87260.4, 87241.2, 87096, 87306, 87097.2, 86790, + 87037.2, 87040.8, 86763.6, 87139.2, 86818.8, 86697.6, 87030, + 86868, 87025.2, 86898, 86938.8, 86798.4, 86632.8, 86766, + 86606.4, 86774.4, 86683.2, 86539.2, 86671.2, 86515.2, 86467.2, + 86571.6, 86526, 86554.8, 86607.6, 86518.8, 86335.2, 86245.2, + 86316, 86293.2, 86302.8, 86346, 86196, 86212.8, 86276.4, + 86148, 86050.8, 86090.4, 86214, 86187.6, 86179.2, 85905.6, + 86024.4, 86140.8, 85767.6, 85840.8, 85954.8, 85971.6, 85980, + 85816.8, 86030.4, 85624.8, 85702.8, 85764, 85600.8, 85666.8, + 85789.2, 85629.6, 85510.8, 85730.4, 85670.4, 85411.2, 85686, + 85322.4, 85594.8, 85412.4, 85744.8, 85513.2, 85416, 85663.2, + 85446, 85580.4, 85195.2, 85312.8, 85431.6, 85240.8, 85222.8, + 85114.8, 85198.8, 85038, 85045.2, 85028.4, 85046.4, 85033.2, + 84813.6, 85240.8, 85040.4, 84973.2, 85040.4, 84806.4, 84775.2, + 84886.8, 84878.4, 84790.8, 84850.8, 84730.8, 84590.4, 84710.4, + 84572.4, 84696, 84588, 84477.6, 84464.4, 84534, 84573.6, + 84472.8, 84522, 84442.8, 84444, 84362.4, 84438, 84512.4, + 84242.4, 84384, 84339.6, 84211.2, 84264, 84284.4, 84181.2, + 84264, 84363.6, 84082.8, 84248.4, 84076.8, 84234, 83976, + 84027.6, 83986.8, 84102, 83919.6, 83894.4, 84144, 84040.8, + 83984.4, 83971.2, 83929.2, 83800.8, 83689.2, 83890.8, 83751.6, + 83707.2, 83887.2, 83798.4, 83805.6, 83634, 83810.4, 83658, + 83679.6, 83523.6, 83431.2, 83516.4, 83500.8, 83413.2, 83562, + 83496, 83505.6, 83430, 83302.8, 83367.6, 83274, 83415.6, + 83434.8, 83174.4, 83398.8, 83198.4, 83199.6, 83293.2, 83068.8, + 83139.6, 83198.4, 82993.2, 83100, 82940.4, 82970.4, 83060.4, + 83047.2, 82975.2, 82814.4, 82994.4, 82794, 82960.8, 82904.4, + 82791.6, 82600.8, 82681.2, 82794, 82641.6, 82599.6, 82844.4, + 82632, 82663.2, 82693.2, 82602, 82544.4, 82584, 82495.2, + 82587.6, 82488, 82614, 82472.4, 82560, 82340.4, 82474.8, + 82594.8, 82546.8, 82310.4, 82479.6, 82270.8, 82341.6, 82280.4, + 82023.6, 82101.6, 82297.2, 82098, 82176, 82096.8, 82303.2, + 82156.8, 82065.6, 82016.4, 81987.6, 82083.6, 82088.4, 81974.4, + 82016.4, 81861.6, 81790.8, 81796.8, 81943.2, 82110, 81745.2, + 81962.4, 81794.4, 81811.2, 81694.8, 81732, 81775.2, 81537.6, + 81721.2, 81708, 81778.8, 81715.2, 81532.8, 81650.4, 81597.6, + 81632.4, 81508.8, 81470.4, 81642, 81496.8, 81483.6, 81392.4, + 81398.4, 81469.2, 81344.4, 81277.2, 81424.8, 81399.6, 81249.6, + 81250.8, 81108, 81386.4, 81213.6, 81124.8, 81304.8, 81217.2, + 81084, 81212.4, 81091.2, 80942.4, 80892, 80910, 81033.6, + 80937.6, 80995.2, 81018, 80878.8, 80955.6, 80941.2, 80928, + 80943.6, 80901.6, 80672.4, 80860.8, 80918.4, 80820, 80713.2, + 80829.6, 80786.4, 80608.8, 80734.8, 80613.6, 80782.8, 80545.2, + 80678.4, 80538, 80636.4, 80775.6, 80508, 80472, 80343.6, + 80589.6, 80275.2, 80458.8, 80281.2, 80396.4, 80308.8, 80355.6, + 80222.4, 80212.8, 80242.8, 80274, 80216.4, 80172, 80180.4, + 80149.2, 80167.2, 80209.2, 80115.6, 80238, 80283.6, 80179.2, + 80102.4, 79945.2, 79957.2, 80089.2, 79744.8, 80019.6, 79926, + 79893.6, 79874.4, 79935.6, 79996.8, 79906.8, 79923.6, 79989.6, + 79910.4, 80073.6, 79788, 79912.8, 79843.2, 79675.2, 79681.2, + 79742.4, 79790.4, 79561.2, 79548, 79657.2, 79580.4, 79516.8, + 79754.4, 79537.2, 79581.6, 79465.2, 79422, 79510.8, 79459.2, + 79376.4, 79461.6, 79478.4, 79347.6, 79476, 79152, 79342.8, + 79328.4, 79561.2, 79347.6, 79282.8, 79257.6, 79257.6, 79262.4, + 79347.6, 79060.8, 79216.8, 79158, 78950.4, 79168.8, 79092, + 79203.6, 79111.2, 78902.4, 79052.4, 78988.8, 78914.4, 79068, + 78879.6, 78921.6, 79006.8, 78831.6, 78838.8, 78948, 79035.6, + 78770.4, 78730.8, 78860.4, 78627.6, 78753.6, 78698.4, 78848.4, + 78874.8, 78696, 78751.2, 78918, 78682.8, 78508.8, 78634.8, + 78524.4, 78523.2, 78524.4, 78596.4, 78336, 78583.2, 78312, + 78452.4, 78471.6, 78549.6, 78408, 78304.8, 78294, 78345.6, + 78368.4, 78378, 78343.2, 78218.4, 78438, 78300, 78429.6, + 78056.4, 78267.6, 78343.2, 78297.6, 78337.2, 78020.4, 78115.2, + 78093.6, 77992.8, 78222, 78157.2, 78028.8, 78010.8, 77858.4, + 78069.6, 77886, 78063.6, 77995.2, 78040.8, 78099.6, 78218.4, + 77818.8, 77847.6, 77935.2, 77850, 77810.4, 77796, 77870.4, + 77676, 77782.8, 77764.8, 77632.8, 77680.8, 77773.2, 77926.8, + 77877.6, 77730, 77632.8, 77691.6, 77661.6, 77676, 77568, + 77416.8, 77517.6, 77428.8, 77426.4, 77508, 77500.8, 77640, + 77430, 77692.8, 77416.8, 77353.2, 77469.6, 77246.4, 77222.4, + 77348.4, 77356.8, 77216.4, 77288.4, 77384.4, 77332.8, 77550, + 77118, 77322, 77157.6, 77353.2, 77134.8, 77128.8, 77337.6, + 77091.6, 77013.6, 77042.4, 77144.4, 77121.6, 77094, 77020.8, + 77110.8, 76970.4, 76994.4, 76977.6, 76785.6, 77098.8, 76939.2, + 76882.8, 76879.2, 77030.4, 76959.6, 76986, 76870.8, 76828.8, + 76759.2, 76825.2, 76776, 76795.2, 76730.4, 76849.2, 76804.8, + 76620, 76798.8, 76662, 76702.8, 76545.6, 76610.4, 76790.4, + 76645.2, 76860, 76657.2, 76530, 76711.2, 76561.2, 76416, + 76532.4, 76624.8, 76456.8, 76524, 76550.4, 76327.2, 76521.6, + 76406.4, 76200, 76515.6, 76282.8, 76197.6, 76390.8, 76414.8, + 76290, 76347.6, 76269.6, 76347.6, 76202.4, 76374, 76239.6, + 76269.6, 76113.6, 76162.8, 76178.4, 76206, 76135.2, 76257.6, + 76180.8, 76120.8, 76238.4, 76143.6, 76077.6, 76215.6, 76054.8, + 76077.6, 75993.6, 75924, 75913.2, 75934.8, 76047.6, 75978, + 76021.2, 75979.2, 75703.2, 75987.6, 75993.6, 76036.8, 75661.2, + 75792, 75910.8, 75846, 75817.2, 75860.4, 75934.8, 75903.6, + 75675.6, 75846, 75607.2, 75742.8, 75597.6, 75699.6, 75565.2, + 75564, 75376.8, 75524.4, 75384, 75675.6, 75615.6, 75626.4, + 75589.2, 75408, 75541.2, 75572.4, 75607.2, 75177.6, 75328.8, + 75724.8, 75612, 75291.6, 75316.8, 75530.4, 75220.8, 75206.4, + 75260.4, 75133.2, 75244.8, 75453.6, 75259.2, 75211.2, 75321.6, + 75206.4, 75255.6, 75132, 75325.2, 75194.4, 75111.6, 75061.2, + 75039.6, 75309.6, 75031.2, 75019.2, 75039.6, 75297.6, 74966.4, + 74919.6, 74932.8, 75222, 75009.6, 74965.2, 75031.2, 75181.2, + 74934, 74979.6, 74814, 74910, 74858.4, 74799.6, 74799.6, + 75000, 74772, 74922, 74995.2, 74756.4, 74830.8, 74772, + 74834.4, 74874, 74844, 74691.6, 74911.2, 74757.6, 74652, + 74686.8, 74643.6, 74540.4, 74542.8, 74559.6, 74895.6, 74570.4, + 74557.2, 74829.6, 74583.6, 74677.2, 74612.4, 74517.6, 74508, + 74574, 74649.6, 74472, 74494.8, 74473.2, 74490, 74475.6, + 74380.8, 74527.2, 74526, 74582.4, 74248.8, 74208, 74437.2, + 74263.2, 74088, 74469.6, 74451.6, 74347.2, 74380.8, 74348.4, + 74326.8, 74263.2, 74361.6, 74205.6, 74238, 74170.8, 74314.8, + 74209.2, 74192.4, 74146.8, 74152.8, 74002.8, 74059.2, 74102.4, + 74172, 74197.2, 74132.4, 74210.4, 74042.4, 74037.6, 73977.6, + 74013.6, 73983.6, 74019.6, 73915.2, 73845.6, 74086.8, 73926, + 73850.4, 74018.4, 73824, 74176.8, 73867.2, 73878, 73904.4, + 73708.8, 73832.4, 73717.2, 73959.6, 74008.8, 73807.2, 73780.8, + 73708.8, 73724.4, 73593.6, 73866, 73773.6, 73894.8, 73744.8, + 73620, 73927.2, 73706.4, 73706.4, 73639.2, 73720.8, 73705.2, + 73551.6, 73737.6, 73621.2, 73482, 73537.2, 73500, 73543.2, + 73518, 73503.6, 73371.6, 73492.8, 73459.2, 73370.4, 73398, + 73519.2, 73586.4, 73306.8, 73460.4, 73357.2, 73407.6, 73450.8, + 73407.6, 73334.4, 73243.2, 73279.2, 73314, 73362, 73317.6, + 73558.8, 73144.8, 73371.6, 73330.8, 73273.2, 73294.8, 73098, + 73226.4, 73234.8, 73104, 73197.6, 73100.4, 73272, 73143.6, + 73068, 73106.4, 73138.8, 73132.8, 72980.4, 73056, 72980.4, + 73064.4, 72979.2, 73093.2, 73047.6, 73044, 72957.6, 73304.4, + 72904.8, 72927.6, 72919.2, 72835.2, 73062, 73132.8, 72877.2, + 72835.2, 72886.8, 72910.8, 72902.4, 72859.2, 73132.8, 72894, + 72684, 73059.6, 72979.2, 72846, 72804, 72734.4, 72906, + 72690, 72684, 72814.8, 72763.2, 72746.4, 72586.8, 72698.4, + 72627.6, 72675.6, 72705.6, 72625.2, 72712.8, 72675.6, 72523.2, + 72624, 72686.4, 72565.2, 72627.6, 72468, 72672, 72480, + 72636, 72686.4, 72453.6, 72578.4, 72405.6, 72453.6, 72483.6, + 72541.2, 72312, 72300, 72548.4, 72288, 72555.6, 72427.2, + 72411.6, 72450, 72375.6, 72398.4, 72334.8, 72542.4, 72358.8, + 72393.6, 72368.4, 72242.4, 72374.4, 72348, 72381.6, 72216, + 72246, 72372, 72319.2, 72127.2, 72187.2, 72174, 72163.2, + 72124.8, 72325.2, 72220.8, 72016.8, 72300, 72103.2, 72212.4, + 72332.4, 72086.4, 72100.8, 72289.2, 72056.4, 71959.2, 72069.6, + 72133.2, 72010.8, 71964, 71967.6, 72076.8, 72145.2, 72277.2, + 72090, 71924.4, 71852.4, 71755.2, 72148.8, 71959.2, 71924.4, + 71858.4, 71866.8, 72091.2, 71860.8, 71794.8, 71758.8, 71913.6, + 71827.2, 71809.2, 71925.6, 71811.6, 71761.2, 72064.8, 71770.8, + 71784, 71877.6, 71786.4, 71706, 71743.2, 71688, 71752.8, + 71835.6, 71792.4, 71599.2, 71672.4, 71661.6, 71581.2, 71728.8, + 71600.4, 71631.6, 71584.8, 71510.4, 71809.2, 71772, 71592, + 71529.6, 71517.6, 71808, 71815.2, 71614.8, 71461.2, 71552.4, + 71599.2, 71490, 71492.4, 71569.2, 71562, 71392.8, 71408.4, + 71360.4, 71517.6, 71326.8, 71469.6, 71366.4, 71485.2, 71168.4, + 71485.2, 71308.8, 71317.2, 71660.4, 71396.4, 71176.8, 71304, + 71253.6, 71522.4, 71272.8, 71359.2, 71276.4, 71407.2, 71328, + 71233.2, 71089.2, 71330.4, 71443.2, 71346, 71275.2, 71176.8, + 71198.4, 71228.4, 71211.6, 71293.2, 71323.2, 71305.2, 71209.2, + 71188.8, 71265.6, 71119.2, 71115.6, 71300.4, 71109.6, 71060.4, + 71064, 71252.4, 71158.8, 71149.2, 70826.4, 71046, 71032.8, + 71095.2, 71168.4, 71055.6, 71030.4, 71096.4, 70928.4, 71043.6, + 70998, 70856.4, 70980, 71005.2, 71040, 70972.8, 71056.8, + 71024.4, 70952.4, 70893.6, 70831.2, 70994.4, 70776, 70789.2, + 70932, 70965.6, 70808.4, 70776, 70707.6, 70897.2, 70965.6, + 70737.6, 70677.6, 70768.8, 70785.6, 70833.6, 70898.4, 70569.6, + 70900.8, 70708.8, 70862.4, 70816.8, 70755.6, 70755.6, 70875.6, + 70771.2, 70744.8, 70639.2, 70677.6, 70646.4, 70732.8, 70623.6, + 70663.2, 70584, 70686, 70538.4, 70592.4, 70495.2, 70659.6, + 70603.2, 70456.8, 70633.2, 70485.6, 70634.4, 70498.8, 70456.8, + 70362, 70617.6, 70596, 70508.4, 70543.2, 70639.2, 70345.2, + 70602, 70536, 70537.2, 70508.4, 70440, 70335.6, 70428, + 70218, 70342.8, 70465.2, 70418.4, 70521.6, 70497.6, 70178.4, + 70491.6, 70395.6, 70221.6, 70416, 70336.8, 70122, 70275.6, + 70342.8, 70072.8, 70095.6, 70401.6, 70321.2, 70286.4, 70267.2, + 70263.6, 70194, 70282.8, 70341.6, 70185.6, 70194, 70116, + 70153.2, 70108.8, 70364.4, 70251.6, 70142.4, 70092, 70053.6, + 70117.2, 70201.2, 70350, 70305.6, 70197.6, 70096.8, 70033.2, + 70207.2, 70065.6, 69981.6, 70219.2, 70083.6, 69919.2, 70023.6, + 70162.8, 70090.8, 69842.4, 69978, 70080, 69850.8, 70026, + 70146, 70009.2, 70084.8, 69993.6, 70018.8, 69774, 70086, + 69873.6, 69898.8, 70056, 70052.4, 69969.6, 69916.8, 70080, + 69985.2, 69960, 70092, 69817.2, 69966, 69748.8, 69810, + 69961.2, 69832.8, 69699.6, 69765.6, 69856.8, 69616.8, 69856.8, + 69777.6, 69820.8, 69937.2, 69847.2, 69789.6, 69889.2, 69751.2, + 69870, 69727.2, 69756, 69919.2, 69855.6, 69657.6, 69745.2, + 69726, 69805.2, 69752.4, 69628.8, 69573.6, 69834, 69711.6, + 69676.8, 69625.2, 69810, 69637.2, 69673.2, 69702, 69567.6, + 69530.4, 69651.6, 69770.4, 69516, 69616.8, 69612, 69644.4, + 69469.2, 69594, 69644.4, 69324, 69476.4, 69430.8, 69541.2, + 69614.4, 69609.6, 69660, 69624, 69573.6, 69546, 69531.6, + 69453.6, 69392.4, 69445.2, 69444, 69225.6, 69391.2, 69436.8, + 69444, 69340.8, 69321.6, 69442.8, 69349.2, 69416.4, 69486, + 69466.8, 69280.8, 69478.8, 69460.8, 69154.8, 69417.6, 69368.4, + 69262.8, 69343.2, 69379.2, 69246, 69397.2, 69120, 69296.4, + 69229.2, 69339.6, 69386.4, 69361.2, 69099.6, 69472.8, 69246, + 69213.6, 69279.6, 69410.4, 69324, 69402, 69187.2, 69307.2, + 69398.4, 69374.4, 69034.8, 69284.4, 69130.8, 68974.8, 69253.2, + 69178.8, 69190.8, 69256.8, 69280.8, 69446.4, 69091.2, 69084, + 69111.6, 68940, 68953.2, 69123.6, 69060, 69206.4, 69172.8, + 68905.2, 68914.8, 69154.8, 69092.4, 69174, 69086.4, 68926.8, + 69160.8, 68935.2, 68922, 69122.4, 68961.6, 69046.8, 69030, + 68932.8, 68947.2, 69126, 68960.4, 69031.2, 69084, 69012, + 68794.8, 68934, 68966.4, 68895.6, 68853.6, 68888.4, 68845.2, + 69009.6, 69097.2, 68908.8, 69030, 68950.8, 68827.2, 69008.4, + 68833.2, 68985.6, 68907.6, 68607.6, 68919.6, 69024, 69044.4, + 68956.8, 69046.8, 68949.6, 68896.8, 68847.6, 68836.8, 68643.6, + 68676, 68925.6, 68863.2, 68719.2, 68798.4, 68630.4, 68592, + 68635.2, 68820, 68851.2, 68730, 68880, 68840.4, 68658, + 68882.4, 68853.6, 68788.8, 68749.2, 68815.2, 68677.2, 68685.6, + 68744.4, 68769.6, 68798.4, 68658, 68686.8, 68858.4, 68533.2, + 68668.8, 68712, 68733.6, 68618.4, 68556, 68626.8, 68622, + 68652, 68911.2, 68696.4, 68430, 68673.6, 68734.8, 68370, + 68424, 68692.8, 68511.6, 68550, 68608.8, 68502, 68650.8, + 68607.6, 68636.4, 68424, 68716.8, 68539.2, 68439.6, 68559.6, + 68566.8, 68485.2, 68336.4, 68601.6, 68396.4, 68404.8, 68491.2, + 68416.8, 68584.8, 68547.6, 68716.8, 68328, 68521.2, 68565.6, + 68546.4, 68679.6, 68505.6, 68475.6, 68581.2, 68568, 68090.4, + 68469.6, 68673.6, 68626.8, 68539.2, 68343.6, 68655.6, 68307.6, + 68288.4, 68449.2, 68472, 68296.8, 68576.4, 68384.4, 68332.8, + 68503.2, 68505.6, 68484, 68413.2, 68254.8, 68329.2, 68625.6, + 68186.4, 68403.6, 68487.6, 68284.8, 68251.2, 68222.4, 68349.6, + 68414.4, 68492.4, 68214, 68251.2, 68265.6, 68550, 68164.8, + 68462.4, 68335.2, 68155.2, 68436, 68388, 68377.2, 68097.6, + 68308.8, 68271.6, 68338.8, 68145.6, 68325.6, 68215.2, 68313.6, + 68253.6, 68154, 68372.4, 68275.2, 68254.8, 68562, 68392.8, + 68133.6, 68245.2, 68131.2, 68209.2, 68256, 68224.8, 68152.8, + 67959.6, 68251.2, 68154, 68056.8, 68281.2, 68210.4, 68275.2, + 68344.8, 68041.2, 68200.8, 67970.4, 68181.6, 68064, 68137.2, + 68269.2, 68269.2, 67960.8, 68380.8, 68203.2, 68126.4, 68162.4, + 67940.4, 68198.4, 68233.2, 68059.2, 68280, 68322, 68023.2, + 67990.8, 68020.8, 68113.2, 68110.8, 68085.6, 67956, 68068.8, + 68191.2, 68005.2, 67921.2, 67987.2, 68139.6, 68028, 68137.2, + 68083.2, 68077.2, 68026.8, 68066.4, 67993.2, 67957.2, 67975.2, + 68144.4, 68030.4, 67908, 68115.6, 68024.4, 68030.4, 68136, + 68026.8, 68008.8, 68036.4, 68060.4, 67896, 68126.4, 68090.4, + 67944, 67806, 68047.2, 67910.4, 67791.6, 67866, 67850.4, + 67974, 67814.4, 68154, 68044.8, 67867.2, 68072.4, 67936.8, + 67958.4, 68013.6, 67844.4, 67900.8, 67887.6, 67770, 67964.4, + 67935.6, 67830, 67861.2, 67978.8, 67689.6, 68109.6, 67874.4, + 67905.6, 67852.8, 67707.6, 67974, 67645.2, 67988.4, 67851.6, + 68056.8, 67915.2, 67986, 67876.8, 67812, 67741.2, 67920, + 67884, 67819.2, 67903.2, 67737.6, 67803.6, 67788, 67860, + 67812, 67716, 67975.2, 67929.6, 67864.8, 67786.8, 67876.8, + 67868.4, 67706.4, 67944, 67850.4, 67696.8, 67779.6, 67783.2, + 67850.4, 67682.4, 67714.8, 67808.4, 67786.8, 67927.2, 67790.4, + 67946.4, 67687.2, 67790.4, 67720.8, 67622.4, 67597.2, 67701.6, + 67731.6, 67725.6, 67798.8, 67826.4, 67932, 67638, 67668, + 67628.4, 67668, 67704, 67857.6, 67658.4, 67623.6, 67771.2, + 67672.8, 67842, 67764, 67676.4, 67602, 67780.8, 67611.6, + 67788, 67716, 67688.4, 67660.8, 67836, 67804.8, 67855.2, + 67912.8, 67545.6, 67722, 67572, 67542, 67701.6, 67578, + 67658.4, 67802.4, 67663.2, 67580.4, 67556.4, 67768.8, 67617.6, + 67693.2, 67455.6, 67747.2, 67602, 67681.2, 67682.4, 67658.4, + 67602, 67545.6, 67570.8, 67594.8, 67506, 67606.8, 67482, + 67596, 67569.6, 67622.4, 67572, 67629.6, 67567.2, 67608, + 67580.4, 67494, 67622.4, 67582.8, 67671.6, 67563.6, 67646.4, + 67754.4, 67574.4, 67538.4, 67527.6, 67596, 67412.4, 67558.8, + 67660.8, 67580.4, 67510.8, 67530, 67712.4, 67520.4, 67633.2, + 67584, 67611.6, 67635.6, 67536, 67550.4, 67537.2, 67455.6, + 67414.8, 67550.4, 67656, 67668, 67564.8, 67538.4, 67557.6, + 67404, 67474.8, 67639.2, 67538.4, 67791.6, 67834.8, 67508.4, + 67586.4, 67468.8, 67466.4, 67460.4, 67648.8, 67527.6, 67423.2, + 67689.6, 67642.8, 67504.8, 67486.8, 67502.4, 67413.6, 67558.8, + 67546.8, 67492.8, 67365.6, 67489.2, 67531.2, 67417.2, 67467.6, + 67440, 67496.4, 67527.6, 67600.8, 67327.2, 67387.2, 67513.2, + 67532.4, 67634.4, 67462.8, 67399.2, 67513.2, 67402.8, 67341.6, + 67586.4, 67621.2, 67496.4, 67371.6, 67460.4, 67460.4, 67288.8, + 67405.2, 67467.6, 67506, 67599.6, 67549.2, 67450.8, 67490.4, + 67540.8, 67428, 67471.2, 67542, 67453.2, 67495.2, 67438.8, + 67437.6, 67422, 67369.2, 67405.2, 67572, 67449.6, 67338, + 67407.6, 67402.8, 67302, 67479.6, 67603.2, 67482, 67438.8, + 67479.6, 67322.4, 67356, 67429.2, 67578, 67422, 67412.4, + 67506, 67376.4, 67332, 67372.8, 67377.6, 67629.6, 67444.8, + 67492.8, 67500, 67437.6, 67411.2, 67423.2, 67324.8, 67387.2, + 67317.6, 67440, 67554, 67455.6, 67382.4, 67522.8, 67357.2, + 67394.4, 67248, 67395.6, 67670.4, 67350, 67250.4, 67240.8, + 67399.2, 67376.4, 67574.4, 67342.8, 67323.6, 67401.6, 67239.6, + 67344, 67510.8, 67209.6, 67242, 67436.4, 67285.2, 67261.2, + 67470, 67272, 67251.6, 67412.4, 67216.8, 67425.6, 67273.2, + 67314, 67432.8, 67219.2, 67414.8, 67483.2, 67294.8, 67357.2, + 67245.6, 67420.8, 67436.4, 67459.2, 67399.2, 67233.6, 67377.6, + 67328.4, 67341.6, 67413.6, 67420.8, 67356, 67293.6, 67363.2, + 67370.4, 67311.6, 67424.4, 67308, 67198.8, 67324.8, 67327.2, + 67401.6, 67257.6, 67407.6, 67242, 67490.4, 67338, 67388.4, + 67305.6, 67384.8, 67245.6, 67514.4, 67510.8, 67249.2, 67222.8, + 67332, 67561.2, 67215.6, 67506, 67311.6, 67296, 67360.8, + 67438.8, 67284, 67323.6, 67312.8, 67425.6, 67350, 67178.4, + 67346.4, 67215.6, 67263.6, 67234.8, 67322.4, 67179.6, 67399.2, + 67284, 67378.8, 67454.4, 67178.4, 67300.8, 67413.6, 67329.6, + 67138.8, 67184.4, 67458, 67467.6, 67285.2, 67364.4, 67189.2, + 67380, 67452, 67279.2, 67480.8, 67423.2, 67392, 67257.6, + 67210.8, 67479.6, 67340.4, 67370.4, 67338, 67369.2, 67315.2, + 67380}; -const float SIMULATED_LAT[GPS_DATA_SIZE] = {41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809465f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809488f, 41.809433f, 41.809379f, 41.809324f, 41.809269f, 41.809215f, 41.809160f, 41.809106f, 41.809051f, 41.809034f, 41.809020f, 41.809002f, 41.808992f, 41.808992f, 41.808981f, 41.808964f, 41.808964f, 41.808936f, 41.808915f, 41.808908f, 41.808882f, 41.808880f, 41.808936f, 41.808936f, 41.808880f, 41.808926f, 41.808936f, 41.808826f, 41.808824f, 41.808796f, 41.808838f, 41.808852f, 41.808801f, 41.808782f, 41.808768f, 41.808875f, 41.808852f, 41.808739f, 41.808679f, 41.808656f, 41.808624f, 41.808600f, 41.808600f, 41.808712f, 41.808558f, 41.808572f, 41.808628f, 41.808646f, 41.808656f, 41.808747f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808550f, 41.808432f, 41.808386f, 41.808399f, 41.808432f, 41.808399f, 41.808404f, 41.808404f, 41.808404f, 41.808326f, 41.808320f, 41.808320f, 41.808303f, 41.808292f, 41.808292f, 41.808303f, 41.808320f, 41.808320f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808208f, 41.808227f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808264f, 41.808238f, 41.808223f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808208f, 41.808208f, 41.808208f, 41.808208f, 41.808196f, 41.808180f, 41.808147f, 41.808180f, 41.808159f, 41.808129f, 41.808103f, 41.808076f, 41.808068f, 41.808068f, 41.808096f, 41.808072f, 41.808081f, 41.808068f, 41.808051f, 41.808038f, 41.808036f, 41.808033f, 41.808031f, 41.808028f, 41.808026f, 41.808023f, 41.808021f, 41.808018f, 41.808016f, 41.808013f, 41.808011f, 41.808008f, 41.808006f, 41.808003f, 41.808001f, 41.807998f, 41.807996f, 41.807993f, 41.807991f, 41.807988f, 41.807986f, 41.807983f, 41.807981f, 41.807978f, 41.807976f, 41.807973f, 41.807971f, 41.807968f, 41.807966f, 41.807963f, 41.807961f, 41.807958f, 41.807956f, 41.807956f, 41.807939f, 41.807928f, 41.807906f, 41.807900f, 41.807900f, 41.807900f, 41.807872f, 41.807852f, 41.807848f, 41.807853f, 41.807858f, 41.807863f, 41.807868f, 41.807872f, 41.807851f, 41.807844f, 41.807818f, 41.807831f, 41.807816f, 41.807796f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807782f, 41.807704f, 41.807727f, 41.807754f, 41.807843f, 41.807844f, 41.807844f, 41.807844f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807788f, 41.807812f, 41.807804f, 41.807788f, 41.807788f, 41.807760f, 41.807760f, 41.807771f, 41.807734f, 41.807732f, 41.807760f, 41.807760f, 41.807732f, 41.807756f, 41.807760f, 41.807732f, 41.807732f, 41.807704f, 41.807704f, 41.807704f, 41.807624f, 41.807635f, 41.807648f, 41.807628f, 41.807620f, 41.807644f, 41.807648f, 41.807648f, 41.807666f, 41.807676f, 41.807676f, 41.807687f, 41.807677f, 41.807676f, 41.807704f, 41.807704f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807655f, 41.807659f, 41.807650f, 41.807648f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807664f, 41.807648f, 41.807648f, 41.807648f, 41.807648f, 41.807648f, 41.807648f, 41.807662f, 41.807648f, 41.807648f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807676f}; +const float SIMULATED_LAT[GPS_DATA_SIZE] = { + 3.76934E-07, 2.12122E-07, -1.71812E-07, 1.8447E-07, 2.22564E-07, + 1.61956E-07, -3.27081E-08, -3.67698E-07, -8.8245E-08, 2.36561E-07, + 1.66147E-07, 6.51119E-10, 5.97583E-08, 2.08472E-07, 1.10656E-07, + -2.03282E-07, -5.01795E-08, -2.49524E-08, -1.34027E-07, -6.24701E-08, + -1.11716E-07, -3.42272E-07, -5.52034E-08, 2.54705E-07, 1.73384E-07, + -2.34695E-07, 1.68062E-07, 6.71245E-08, -1.12273E-07, 8.19417E-08, + 5.39269E-08, -2.01298E-07, -1.3188E-07, -7.59062E-08, 2.87466E-07, + -4.21843E-08, 4.84208E-07, 2.28063E-07, -1.08865E-08, -6.15637E-08, + 1.01032E-07, -2.74606E-07, -5.82089E-08, -1.38132E-07, 4.65425E-09, + -1.52543E-07, -1.35982E-07, 1.58534E-07, -2.27316E-07, -1.63257E-08, + -6.15608E-08, -2.1505E-07, 1.38163E-07, -6.1444E-08, -3.51964E-07, + 2.18889E-07, -2.27062E-07, 1.42957E-08, 3.7949E-07, 1.315E-07, + 4.11432E-08, 4.38274E-08, 5.32794E-09, 2.80431E-07, -1.14516E-07, + 9.52305E-08, 8.11298E-08, -2.89916E-08, 2.95824E-07, 3.01238E-07, + 2.97799E-08, -2.01909E-07, -5.44123E-08, 3.23747E-07, 1.93315E-07, + 3.62168E-08, -6.77334E-08, 1.24657E-07, 1.65701E-07, 5.39991E-08, + -1.63911E-07, -2.0362E-08, 1.97007E-07, -2.58482E-07, -4.10798E-07, + 3.45168E-07, 4.01655E-07, -3.64355E-07, -2.83517E-07, -5.81187E-08, + 2.98609E-08, -1.22408E-07, -4.65224E-07, -5.09004E-08, -2.19067E-07, + 9.80145E-09, -8.92895E-09, -2.75929E-07, -1.27299E-07, -4.71347E-07, + -1.04301E-07, 2.57856E-07, -9.40528E-08, 7.85311E-08, 1.11771E-07, + -2.63113E-07, -2.14946E-07, 2.11097E-08, -3.24808E-08, -2.23056E-07, + -4.70109E-07, 4.26524E-08, 4.61181E-08, -2.86103E-07, -1.93316E-07, + 1.94906E-07, 1.43105E-07, 1.5916E-07, -1.11042E-08, -1.16749E-07, + -1.72716E-07, -2.49052E-08, 9.74565E-08, 1.35746E-07, 1.66931E-07, + 1.29304E-07, 7.76291E-08, -1.54247E-07, -6.56644E-08, -2.93574E-07, + -7.41989E-08, 6.58051E-08, 2.28718E-07, -8.82466E-08, -7.59378E-08, + -2.17243E-09, -6.30933E-08, 4.87519E-08, 3.87932E-07, -3.08058E-07, + -3.7991E-07, 6.09921E-09, -7.63216E-07, 4.1687E-07, 2.1309E-07, + 6.84652E-09, -1.05982E-07, 8.98257E-08, 2.48217E-07, -1.35843E-07, + 2.18528E-07, 1.04768E-07, 6.395E-08, 8.59627E-08, -1.37216E-07, + -2.71795E-07, -3.46347E-07, -1.79066E-07, -1.59695E-07, -1.91646E-07, + 2.67202E-07, 1.29719E-07, -2.33781E-08, -2.08154E-07, -1.02285E-07, + -8.53781E-08, 7.95985E-08, -2.01229E-07, -6.30371E-08, -1.98727E-07, + 2.43502E-08, -3.17344E-07, 1.53155E-07, 4.89643E-07, -1.96821E-07, + 1.08141E-07, 1.30763E-07, 1.74285E-07, 1.59301E-07, -8.38133E-08, + -3.76713E-08, 3.7599E-07, 3.54125E-07, 2.25801E-07, 3.51639E-07, + 8.97044E-09, -1.19867E-08, -3.04683E-07, 3.10394E-07, -1.62884E-07, + -5.85739E-08, -1.41581E-07, 2.39545E-07, -1.97129E-08, -2.81045E-07, + 5.18673E-08, -4.36651E-07, 1.43414E-07, 3.50339E-07, 1.79237E-07, + 8.86588E-08, 2.03958E-07, 4.77796E-07, -8.48529E-08, -3.82859E-08, + -1.05497E-07, 9.38306E-08, 2.75094E-07, 2.05437E-07, 1.08035E-07, + 2.70916E-07, -1.51795E-07, -2.65978E-08, 3.65574E-08, -2.22295E-07, + 3.26666E-07, -4.78899E-08, 3.60915E-07, -3.88395E-07, 2.09375E-07, + -2.29577E-07, 5.5004E-08, 3.31515E-07, 2.81451E-08, -2.44606E-07, + 2.42212E-07, -7.16788E-08, -3.87553E-08, 2.335E-08, -1.40683E-07, + 2.58588E-08, -2.01772E-08, 3.76379E-07, -1.15572E-07, -2.42319E-07, + 3.74149E-08, 2.43294E-08, 1.10673E-07, 8.72279E-08, 1.27571E-07, + -2.25981E-07, 1.27195E-07, 2.23826E-08, -3.20174E-07, 3.21856E-08, + -2.68136E-07, -3.71116E-07, -7.344E-08, -1.07119E-07, 5.31866E-08, + 9.04118E-08, 2.48115E-07, -2.42338E-08, -1.73219E-07, -2.47003E-08, + 1.85956E-07, -2.24154E-07, -1.70864E-07, 1.67873E-07, 3.75802E-08, + 7.54004E-08, 5.66849E-09, -9.58147E-08, -3.9131E-07, -2.4055E-07, + 2.64792E-07, 9.05351E-08, 3.08814E-07, 9.02224E-08, -2.3836E-07, + -4.01966E-08, -4.91776E-07, -3.09592E-09, 4.34199E-07, 4.56393E-07, + -2.85331E-07, -4.28243E-08, -9.0535E-08, -1.66351E-07, -9.6062E-08, + 2.36603E-07, -8.17096E-09, 1.52501E-07, 2.6562E-07, 3.18883E-08, + 3.85047E-08, -9.9221E-08, 3.40032E-08, -1.78432E-08, -1.45885E-07, + 1.13226E-07, 1.83697E-07, 4.30458E-07, 2.4484E-08, -1.78475E-08, + -1.54759E-07, 3.05778E-07, -2.05695E-07, 6.39378E-08, -1.21166E-07, + 1.17516E-07, 4.93873E-08, -2.72045E-08, 1.75233E-08, 3.24445E-07, + 5.29042E-08, 1.01796E-07, 1.12074E-07, 7.02373E-08, -2.47207E-07, + 7.92019E-07, 2.83616E-07, 5.91542E-08, 2.64248E-07, 1.742E-07, + -1.64314E-07, 5.17861E-09, -2.23724E-07, -3.18962E-07, 1.67487E-07, + 8.90668E-08, -2.84997E-07, -4.77309E-07, -3.96104E-08, -1.20889E-07, + 4.05684E-08, 1.56952E-07, 5.28963E-07, 1.54991E-08, 1.0416E-07, + -6.41972E-08, 1.48924E-08, 2.89395E-07, 1.00629E-07, -2.1309E-07, + 6.71141E-07, 2.88868E-07, 1.63219E-07, 1.15999E-07, 9.5636E-08, + -1.23719E-07, 1.49386E-07, -2.93838E-07, 4.28709E-07, -1.85328E-07, + 1.28787E-07, 1.11139E-07, -1.1925E-07, -7.4553E-08, -2.69638E-08, + 1.26527E-07, 6.57827E-07, -1.74398E-07, 3.40294E-07, 8.47443E-09, + 9.78399E-08, -8.1528E-08, 3.04273E-07, -4.15214E-08, 5.86915E-08, + 5.68868E-07, -1.62137E-07, -1.71217E-08, 5.6724E-08, -1.06476E-07, + 1.85715E-07, -3.8599E-09, -6.64407E-09, 1.17404E-07, -2.56204E-07, + -1.75241E-07, 1.71596E-07, -1.4762E-07, 4.16646E-07, 9.69343E-08, + 1.82059E-07, -8.3673E-08, 1.26423E-07, -1.02764E-07, 1.39617E-08, + 2.47254E-07, 6.72091E-08, -4.55932E-08, 4.89326E-08, 5.28676E-07, + -1.34328E-07, 2.51601E-07, 4.22153E-07, -2.38294E-07, -3.2331E-08, + -9.2493E-08, -1.73735E-07, 4.90407E-08, 6.12707E-08, 3.47552E-07, + -6.39062E-08, 1.61706E-07, 1.88663E-07, 1.26598E-07, -1.23814E-07, + 1.90992E-07, 8.8243E-09, 1.14491E-07, 2.70083E-07, 3.94591E-07, + 1.82845E-07, 4.07766E-07, 3.16535E-07, -1.90365E-07, -3.51666E-08, + 1.05588E-07, 3.92326E-07, 1.00873E-07, 1.79402E-07, 1.33758E-07, + -4.23842E-08, -2.49972E-07, 1.81966E-07, 5.06369E-07, 8.14509E-09, + 1.55945E-07, -3.22357E-07, 5.0511E-07, 1.89089E-07, 5.0358E-07, + -5.85379E-08, 3.76608E-07, 1.32459E-07, 3.97731E-07, 1.26059E-07, + 1.2545E-07, -4.79149E-08, -1.88091E-07, -1.0095E-07, -2.09027E-07, + 4.55574E-07, 2.3318E-07, -2.25007E-07, 1.41289E-07, 3.60225E-07, + -3.51109E-08, 2.77369E-08, 3.12882E-07, 1.37439E-07, -3.22587E-08, + 5.02227E-07, 2.0798E-08, 2.27387E-08, -5.25458E-09, 1.92431E-07, + 1.16708E-07, -3.64609E-07, 2.50435E-07, 2.03234E-07, -5.69815E-08, + 1.35059E-07, -3.19073E-08, 4.79436E-08, 9.50161E-08, 4.37039E-07, + 2.90042E-08, 3.94144E-07, 7.00003E-07, -6.74312E-08, 4.29219E-07, + 7.49522E-08, 1.56493E-07, 3.45089E-07, -2.51653E-09, 3.71912E-08, + -4.75243E-07, 3.67593E-07, 2.60062E-07, 3.04229E-07, -5.58112E-08, + 2.33565E-07, -1.0599E-07, 1.09906E-07, 3.57026E-07, -3.251E-07, + -1.4299E-07, 1.77976E-07, 5.80065E-07, 6.21202E-09, -2.11489E-07, + 3.26742E-07, 2.96697E-07, 3.01139E-07, -3.84749E-08, 1.55941E-07, + 5.27668E-08, 2.19175E-08, 2.47696E-07, 3.44804E-07, 2.2413E-07, + 4.32379E-07, 8.00394E-08, 6.79679E-08, 1.04711E-07, -2.93326E-07, + 5.15893E-07, 6.86966E-08, 1.41436E-07, -5.1175E-09, 2.79959E-08, + 4.25647E-07, 1.02152E-07, 6.18334E-07, 2.48212E-07, 4.08957E-07, + 4.22657E-07, 9.87536E-08, -6.15099E-08, -1.71758E-08, 4.5777E-08, + 1.13523E-07, 5.09769E-07, 3.22232E-07, 8.0301E-08, 3.28425E-07, + 1.829E-07, 3.79398E-07, 1.24663E-07, 9.5638E-08, 3.42618E-07, + 5.51992E-08, 1.139E-07, -8.79583E-08, 2.34549E-07, 3.2584E-07, + 1.32815E-07, 2.23702E-07, 6.4596E-07, 2.01261E-07, 1.69186E-07, + 1.10583E-07, 8.54538E-08, 4.05363E-09, 2.30544E-07, -2.81641E-07, + 4.32508E-07, 3.03467E-08, 3.39017E-07, 1.43908E-07, 2.9265E-07, + 3.8134E-07, -1.24763E-07, -3.5858E-07, 1.6614E-08, 2.07675E-07, + 4.10685E-08, 1.56855E-07, 3.14779E-07, 5.14184E-07, 2.25746E-07, + 2.98339E-07, -2.96785E-08, 6.90695E-09, -1.83533E-07, 7.82588E-08, + 3.1694E-07, 4.22748E-08, 1.69505E-07, 3.46751E-07, 5.22543E-08, + 2.44437E-07, -2.14923E-07, 4.15526E-07, 2.76735E-08, 2.65348E-07, + 1.31599E-07, 1.14923E-07, 1.84765E-07, 2.10938E-07, 4.99983E-07, + 1.66245E-07, 1.61719E-07, 4.50525E-08, 1.5116E-07, 1.70525E-07, + 5.15745E-07, 6.75539E-07, 6.04561E-07, 1.28565E-07, -1.68076E-07, + 3.1469E-07, 1.67784E-07, 2.53309E-07, 6.15648E-07, -1.37485E-07, + 4.02993E-07, 5.81048E-07, 1.07197E-07, -3.84591E-08, -4.70557E-08, + 4.03077E-07, 2.97067E-07, 3.93186E-07, 2.74167E-07, -3.14718E-07, + -3.31321E-07, 4.21682E-07, 3.98249E-07, 3.63485E-07, 4.21728E-07, + -1.62575E-08, 5.41185E-07, 4.03863E-07, 7.39092E-07, 2.59721E-07, + 7.31732E-07, 1.80858E-07, 3.47947E-07, 1.86029E-07, 3.27507E-07, + 1.73963E-07, 4.24296E-07, 2.94336E-08, 7.87342E-08, 9.61727E-08, + 1.37111E-07, -1.07064E-07, -4.31554E-08, -1.63853E-07, 2.13895E-07, + 4.26615E-07, 6.7162E-07, 6.50324E-07, 5.14163E-07, 2.69901E-08, + 7.34751E-07, 2.19974E-07, 3.92115E-07, 3.21091E-07, 3.27927E-07, + -1.15923E-07, 5.67661E-07, 3.36355E-07, -2.68389E-08, 8.38852E-08, + 2.7667E-07, 6.22976E-08, 1.50801E-07, 5.19361E-07, 3.25352E-07, + 7.36864E-08, 4.48116E-07, 3.69353E-07, 1.39618E-07, 3.25917E-07, + 3.23688E-07, 2.51912E-07, 6.97721E-07, 4.91709E-07, 7.25924E-08, + -4.96837E-08, 3.58817E-07, 4.28857E-07, 3.78063E-07, 1.53949E-07, + -3.39137E-08, 4.37494E-07, 2.75632E-07, 1.25201E-07, 2.88497E-07, + 1.48405E-07, 7.43164E-08, 2.72429E-07, 4.46933E-09, 2.48668E-07, + 1.36716E-07, 3.15534E-07, 1.46811E-09, -7.62545E-08, 1.97662E-07, + 4.0739E-07, 3.30811E-07, 2.00283E-07, 1.93852E-08, 9.80604E-08, + -1.27911E-07, 3.32752E-07, 2.59761E-07, 1.83099E-07, 3.75473E-07, + 5.43587E-07, -4.41739E-08, -3.44284E-07, 6.02875E-07, 2.28467E-07, + 2.82846E-07, 6.78824E-07, -8.58618E-08, 2.32597E-07, 8.44949E-08, + 2.84292E-07, 3.47198E-07, 4.78178E-07, 6.61631E-07, 2.59669E-07, + 3.13543E-08, 2.66253E-07, -1.91963E-08, 3.30198E-07, -1.74934E-09, + 2.43273E-07, -1.55982E-07, 4.5496E-07, 2.18412E-07, 4.03623E-07, + 4.22904E-07, 3.86318E-07, 3.45661E-07, 2.19722E-07, 2.95303E-07, + 2.53137E-07, 2.31238E-07, 2.27292E-07, 3.54686E-07, 3.41388E-07, + -3.10595E-09, 3.99313E-07, -1.80688E-07, -2.38015E-08, 8.96541E-09, + -2.62316E-09, 6.90012E-07, 1.2403E-07, 1.67086E-07, 4.67675E-07, + 1.62634E-07, 3.22439E-07, 2.35106E-07, 2.98439E-07, 7.56885E-08, + 3.1733E-07, 1.66434E-07, 2.6317E-07, 5.18119E-07, 3.52609E-07, + -2.92566E-07, 1.03912E-07, 2.74456E-07, 5.24184E-08, 8.78391E-08, + 4.40749E-07, 2.30784E-07, 7.33978E-07, 5.70596E-07, 4.92872E-07, + 1.87486E-07, 2.60052E-07, 6.08186E-07, 5.05143E-07, -1.45117E-07, + 6.15534E-08, 3.90429E-07, 1.88996E-07, 4.62594E-07, 4.60953E-07, + 2.17185E-07, 4.36624E-07, 3.3126E-07, 2.53269E-08, 4.33314E-07, + 3.03545E-07, 1.58187E-07, -1.06813E-07, 3.13589E-07, 1.54831E-07, + 1.56194E-08, 4.79352E-07, 2.81844E-07, 3.43639E-07, 3.5813E-07, + 2.42665E-07, 5.763E-07, 2.71523E-07, -1.62852E-07, 4.66001E-07, + 1.36111E-07, 3.12783E-07, 1.21314E-07, 1.9202E-07, 1.30285E-07, + 1.85722E-08, 4.56776E-07, 2.54433E-07, 2.16893E-07, 6.25719E-07, + 2.67656E-07, 3.37036E-07, 2.93638E-07, 1.28865E-07, 1.73601E-07, + 4.58188E-07, 4.18983E-07, 4.62456E-07, 1.1491E-07, -1.68978E-09, + 2.96414E-07, 2.59159E-07, 3.91169E-07, 1.13165E-07, -5.60955E-08, + 4.62481E-07, 5.26984E-07, 3.47018E-07, 2.55614E-07, 4.56998E-07, + 2.88484E-07, 4.24019E-07, 3.18946E-07, 5.8359E-08, -1.89786E-08, + 5.80776E-07, 4.60101E-07, 6.96266E-07, 4.81719E-07, 3.30842E-07, + 3.25864E-07, 3.26425E-07, 1.69471E-07, 6.58624E-07, 2.26247E-07, + 2.38573E-07, 1.19507E-07, 5.13737E-07, 1.76005E-07, 6.98724E-07, + 3.41996E-07, 3.57223E-07, 2.39603E-07, 4.14681E-07, 4.76989E-07, + 2.76831E-07, 4.44548E-07, 4.83482E-08, 8.111E-07, 4.05891E-07, + 5.03339E-07, -1.37595E-07, 6.70822E-07, 1.09145E-07, 8.35362E-08, + 1.15073E-08, 1.24085E-07, 9.25172E-08, 6.84858E-07, 4.99671E-07, + 2.77959E-07, 1.83701E-07, 1.87268E-07, 3.42776E-07, 3.53348E-07, + 5.33311E-08, 1.63708E-07, 1.93731E-07, 3.35638E-07, 6.58741E-07, + 5.25028E-07, 3.15438E-08, 2.81862E-07, 5.14124E-07, -2.96436E-08, + 3.62692E-07, 3.11441E-07, 7.99812E-07, 2.94803E-07, 5.42033E-07, + 4.43783E-07, 4.72707E-07, 8.85159E-07, 4.56953E-07, 3.53887E-07, + 3.82514E-07, 8.87906E-07, 2.68582E-07, 3.84237E-07, 3.92789E-07, + 8.1394E-07, 5.26365E-07, 2.34925E-07, 3.69081E-07, 3.04649E-07, + 5.68641E-07, 3.13766E-07, 8.44336E-07, 2.48769E-07, 4.24854E-07, + 6.01978E-07, 7.14109E-07, 2.36984E-07, 1.26208E-07, 6.26918E-07, + -3.67796E-08, 3.15344E-07, 3.10279E-07, 4.40788E-07, 5.91082E-07, + 5.40173E-07, 2.92114E-07, 1.02907E-07, 4.19004E-07, 6.22223E-08, + 7.77029E-07, 5.35426E-07, 5.80406E-07, 1.43836E-07, 5.5226E-07, + 3.75246E-07, 6.40993E-07, 5.0517E-07, 2.83657E-07, 2.13748E-07, + 2.8984E-07, 6.87168E-07, 3.27529E-07, 3.41771E-07, 6.37378E-07, + 6.90703E-07, 6.90406E-07, 4.86049E-07, 3.20396E-08, 3.39522E-07, + 3.50431E-07, 2.16964E-07, 4.01237E-07, 4.49876E-07, 6.85882E-07, + 8.67061E-07, 2.89037E-07, 6.38809E-07, 1.39138E-07, 5.52058E-07, + 6.69598E-07, 6.38367E-08, 4.91919E-07, 4.8665E-07, 4.67906E-07, + 3.15027E-07, 6.38065E-08, 2.39189E-07, 4.65544E-07, 5.70284E-07, + 7.27188E-07, 2.95759E-07, 2.42866E-07, 4.50052E-07, 1.15958E-07, + 6.02928E-07, 3.46188E-07, 4.40401E-07, 1.16984E-07, -1.07559E-07, + 7.31657E-07, 4.89459E-07, 1.09201E-07, 4.125E-07, 5.76982E-07, + 4.61014E-07, 3.75026E-07, 6.3537E-07, 2.49458E-07, 2.66286E-07, + 1.72686E-07, 4.11271E-07, 4.61492E-07, 6.37899E-07, 3.36987E-07, + 7.74077E-07, 6.60381E-07, 2.24091E-07, -7.53405E-08, 3.37517E-07, + 4.4892E-07, 1.13367E-07, 9.43538E-07, 1.99671E-07, 2.88424E-07, + 7.11627E-07, 6.99267E-07, 3.6811E-07, 5.46671E-07, 1.98761E-07, + 6.6924E-07, 4.54519E-07, 2.89802E-07, 4.22434E-07, 4.50725E-07, + 3.3101E-07, 3.30194E-07, 4.74076E-07, 3.9691E-07, 4.36405E-07, + 1.84678E-07, 4.02748E-07, 4.62169E-07, 7.38964E-07, 6.30887E-07, + 2.75148E-07, 6.9305E-07, 1.65885E-07, 4.01321E-07, 5.43484E-07, + 4.37937E-08, 3.28615E-07, 5.90611E-07, 4.39795E-07, 4.36635E-07, + 5.1449E-07, 3.73898E-07, 4.82856E-08, 5.28446E-07, 7.10669E-07, + 4.61377E-07, 5.46876E-07, 5.03047E-07, 7.13356E-07, 2.58069E-07, + -9.62128E-08, 4.71884E-07, 2.51452E-07, 3.40176E-07, 3.95032E-07, + 2.68876E-07, 3.75268E-07, -2.51222E-08, 6.16164E-07, 1.07169E-07, + 3.82748E-07, 1.26826E-07, 3.71241E-07, 4.69658E-07, 1.76793E-07, + 7.59831E-07, 2.11284E-07, 2.81563E-07, 4.37143E-07, 5.03805E-07, + 8.31503E-07, 3.32433E-07, 6.68708E-07, 8.99892E-07, 6.10378E-07, + 5.16117E-07, 5.93846E-07, 4.92307E-07, -6.11545E-08, 5.20679E-07, + 3.49166E-07, 2.04111E-07, 4.55106E-07, 5.65561E-07, 3.24893E-07, + 3.96027E-07, 5.37924E-07, 4.13636E-07, 4.47237E-07, 3.07009E-07, + 5.972E-07, 5.88401E-07, 1.69286E-07, 4.16011E-07, 1.29484E-07, + 1.33329E-07, 5.14593E-07, 4.59271E-07, 1.35368E-08, 4.85127E-07, + 6.7831E-07, 1.41112E-07, 3.18904E-07, 4.65298E-07, 3.12985E-07, + 5.43456E-07, 7.55217E-07, 1.67973E-07, 3.16582E-07, 3.38444E-07, + 3.77391E-07, 6.75231E-07, 3.10216E-07, 6.42141E-07, 5.05824E-07, + 6.06116E-07, 5.67224E-07, 3.95601E-07, 5.90806E-07, 6.10517E-07, + 7.76635E-08, 5.85962E-07, 3.75336E-07, 7.43052E-07, 4.81385E-07, + 8.58713E-08, 6.75051E-07, 3.33767E-07, 5.06048E-07, 2.28521E-07, + 1.68417E-07, 5.22104E-07, 7.48642E-07, -4.91246E-08, 5.02858E-07, + 1.90265E-07, 3.28226E-07, 3.32533E-07, 2.63497E-07, 4.66771E-07, + 2.10939E-07, 3.63075E-07, 5.31147E-07, 6.1292E-07, 5.18619E-07, + 3.602E-07, 5.1435E-07, 4.71911E-07, 5.85486E-07, 3.55702E-07, + 6.94706E-07, 1.95845E-07, 2.52414E-07, 6.84836E-07, 3.9232E-07, + 6.08009E-07, 5.63924E-07, 8.17148E-07, 4.67387E-07, 4.52419E-07, + 5.63551E-07, 5.20357E-07, -4.43501E-09, 3.56878E-07, 4.98768E-07, + 3.99647E-07, 5.04262E-07, 4.87356E-07, 3.60702E-07, 5.08408E-07, + 2.66501E-07, 1.63755E-07, 5.83924E-07, 6.97951E-07, 7.24249E-07, + 1.78187E-07, 4.26173E-07, 5.47619E-07, 7.12395E-07, 6.59994E-07, + 6.95583E-07, 4.98723E-07, 2.05388E-07, 5.71976E-07, 4.98643E-08, + 3.63037E-07, 2.05371E-07, 7.01863E-08, 1.33856E-07, 9.0386E-07, + 5.78131E-07, 6.25304E-07, 3.251E-07, 2.5932E-07, 4.42904E-07, + 5.00095E-07, 1.29878E-07, 3.17349E-08, 4.97886E-07, 2.87772E-07, + 2.1782E-07, 4.36432E-07, 1.55109E-07, 4.34165E-07, 7.53837E-07, + 4.30119E-07, 2.97717E-07, 4.3584E-07, -1.31868E-07, 3.13235E-07, + 8.45309E-07, 3.0712E-07, 5.20997E-07, 9.88335E-07, 5.85708E-07, + 6.14821E-07, 8.09163E-07, 3.1095E-07, 3.78177E-07, 1.09151E-06, + 5.34319E-07, 9.1483E-07, 9.06959E-07, 1.84598E-07, 6.26935E-07, + 6.56133E-08, 1.37399E-07, 8.85784E-07, 4.08103E-07, 3.01895E-07, + 5.07298E-07, 1.9605E-07, 3.89331E-07, 2.76314E-07, 7.51574E-07, + 4.31574E-07, 7.22702E-07, 7.02651E-07, 1.57576E-07, 2.41137E-07, + 8.03308E-07, 4.36404E-07, 6.83234E-07, 3.85247E-07, 2.06997E-07, + 7.36806E-07, 7.96621E-07, 7.15796E-07, 9.31265E-07, 3.31266E-08, + 4.71284E-07, 4.25304E-07, 4.2604E-07, 1.51657E-07, 7.76277E-07, + 5.66973E-07, 6.91298E-07, 4.02118E-07, 6.53179E-07, 5.38639E-07, + 4.68801E-07, 6.49496E-07, 2.38776E-07, 4.65935E-07, 5.03468E-07, + 6.84853E-07, 7.64307E-07, 1.06877E-07, 5.93099E-07, 2.7211E-07, + 2.24536E-07, 3.18711E-07, 4.54799E-07, 2.05728E-07, 5.45438E-07, + 3.77433E-07, 3.36027E-07, 4.37484E-07, 5.7132E-07, 6.57772E-07, + 3.35435E-07, 2.31045E-07, 6.9187E-07, 4.7727E-07, 5.7104E-07, + 5.09243E-07, 3.81214E-07, 6.22616E-07, 7.94663E-07, 3.12949E-07, + 2.71216E-07, 6.87413E-07, -1.4409E-07, 3.70349E-07, 6.53573E-07, + 5.12776E-07, 2.22411E-07, 5.92519E-07, 7.13681E-07, 3.68427E-07, + 5.25275E-07, 6.98196E-07, 6.83187E-07, 3.57218E-07, 2.30839E-07, + 6.43597E-07, 8.20524E-07, -1.62657E-08, 8.59613E-07, 5.81985E-07, + 4.52627E-07, 4.52554E-07, 7.61057E-07, 8.07896E-07, 7.03395E-07, + 5.82255E-07, 6.81718E-07, 7.28229E-07, 4.717E-07, 3.71095E-07, + 5.80015E-08, 7.12153E-07, 4.79455E-07, 6.36934E-07, 6.26606E-07, + 5.09777E-07, 3.92585E-07, 3.6786E-07, 1.26685E-07, 8.83978E-07, + 1.73773E-07, 7.57403E-07, 7.22322E-07, 4.01634E-07, 7.74588E-07, + 8.19117E-07, 2.84339E-07, 4.24787E-07, 7.22572E-07, 6.04852E-07, + 1.01788E-07, 6.53814E-07, -3.2755E-08, 6.87165E-07, 6.03928E-07, + 2.63609E-07, 3.48812E-07, 4.68404E-07, 5.09688E-07, 5.71163E-07, + 7.63253E-07, 5.50409E-07, 5.15328E-07, 6.67003E-07, 5.16048E-07, + 2.10456E-07, 4.32267E-07, 6.61492E-07, 3.61794E-07, 2.66644E-07, + 3.82955E-07, 3.55708E-07, 9.87041E-07, 5.78699E-07, 9.01598E-07, + 3.95467E-07, 7.06755E-07, 3.3168E-07, 6.74315E-07, 8.35158E-07, + 8.33303E-07, 3.27687E-07, 7.22572E-07, 5.0582E-07, 4.84095E-07, + 2.96555E-07, 2.76895E-07, 4.76635E-07, 6.7625E-07, 6.57486E-07, + 8.60145E-07, 5.61585E-07, 1.6521E-07, 5.71664E-07, 7.14619E-07, + 5.12783E-07, 6.66017E-08, 3.8933E-07, 5.33127E-07, 6.83998E-07, + 6.08649E-07, 4.8867E-07, 1.06914E-06, 4.41095E-07, 3.12009E-07, + 9.09385E-07, 1.50159E-07, 5.87673E-07, 6.06796E-07, 5.59761E-07, + 2.97967E-07, 3.62042E-07, 4.79037E-07, 3.58792E-07, 1.81651E-07, + 4.37691E-07, 3.98959E-07, 1.01068E-06, 7.33268E-07, 9.71471E-07, + 7.56725E-07, 1.05442E-06, 2.96232E-07, 3.20394E-07, 2.75402E-07, + 5.98413E-07, 9.47218E-07, 3.15337E-07, 8.47428E-07, 3.44367E-07, + 6.84433E-07, 6.05551E-07, 7.03481E-07, 3.08964E-07, 4.7095E-07, + 5.1271E-07, 5.50433E-07, 4.78849E-07, 6.83504E-07, 8.00266E-07, + 6.17849E-07, 6.80301E-07, 3.1633E-07, 8.41341E-07, 5.20786E-07, + 2.27087E-07, 3.21207E-07, 3.06591E-07, 6.94168E-07, 5.08057E-07, + 2.96556E-07, -6.73466E-08, 3.57986E-07, 8.54299E-07, 6.76559E-07, + 4.98335E-07, 6.31299E-07, 6.65502E-07, 5.01349E-07, 9.39548E-07, + 3.53597E-07, 2.06021E-07, 3.96238E-07, 4.13217E-07, 8.16658E-07, + 4.61265E-07, 5.29631E-07, 5.1819E-07, 7.07341E-07, 9.00059E-07, + 6.80614E-07, 9.81769E-07, 6.42255E-07, 4.79011E-07, 7.64033E-07, + 3.83575E-07, 5.14271E-07, 5.8324E-07, 7.51059E-07, 7.01172E-07, + 1.94809E-07, 6.03129E-07, 4.9366E-07, 7.63842E-07, 4.57237E-07, + 8.59058E-07, 5.87943E-07, 7.9433E-07, 2.56729E-07, 5.98815E-07, + 1.07798E-06, 8.2063E-07, 5.69521E-07, 5.60207E-07, 5.83383E-07, + 4.39279E-07, 2.55421E-07, 6.33668E-07, 1.22436E-07, 4.00587E-07, + 9.08059E-07, 4.76726E-07, 7.06674E-07, 5.34291E-07, 2.1121E-07, + 4.20983E-07, 5.37036E-07, 4.65662E-07, 1.37602E-07, 3.19143E-07, + 6.33814E-07, 5.80594E-07, 5.57141E-07, 7.00927E-07, 5.25485E-07, + 5.54261E-07, 2.65493E-07, 6.59737E-07, 3.51037E-07, 5.35126E-07, + 7.61726E-07, 7.26301E-07, 5.22346E-07, 8.72359E-07, 9.69091E-07, + 2.98928E-07, 1.10481E-06, 6.12839E-07, 5.44351E-07, 3.37904E-07, + 8.68476E-07, 1.00438E-06, 8.5139E-07, 8.45094E-07, 8.2242E-07, + 9.81487E-07, 4.68897E-07, 7.45395E-07, 5.75876E-07, 4.41358E-07, + 6.28174E-07, 8.87356E-07, 3.59556E-07, 7.36999E-07, 9.82377E-08, + 3.82654E-07, 4.79728E-07, 1.47964E-07, 4.85844E-07, 6.22145E-07, + 5.82505E-07, 4.66394E-07, 4.2551E-07, 6.28908E-07, 6.35802E-07, + 4.946E-07, 6.89955E-07, 5.86421E-07, 6.07741E-07, 3.15998E-07, + 3.52035E-07, 3.64328E-07, 5.12186E-07, 8.35698E-07, 4.15805E-07, + 5.63711E-07, 7.28527E-07, 6.7841E-07, 7.80969E-07, 6.63385E-07, + 2.28157E-07, 8.77636E-07, 1.05348E-06, 6.80084E-07, 1.98753E-07, + 7.97042E-07, 4.84903E-07, 8.55167E-07, 5.57934E-07, 6.97996E-07, + 3.52109E-07, 6.26773E-07, 5.3312E-07, 7.4876E-07, 8.00969E-07, + 8.82651E-07, 7.85148E-07, 1.17512E-06, 3.6744E-07, 8.55814E-07, + 4.99202E-07, 1.01832E-06, 5.022E-07, 8.21731E-07, 1.04283E-06, + 1.59986E-07, 5.69402E-07, 7.98816E-07, 8.98257E-07, 8.22677E-07, + 1.1691E-06, 6.54726E-07, 6.55E-07, 5.94028E-07, 6.89328E-07, + 8.55687E-07, 2.63729E-07, 6.74874E-07, 4.99291E-07, 4.99376E-07, + 5.47195E-07, 7.08862E-07, 4.98164E-07, 7.34018E-07, 5.55972E-07, + 6.36141E-07, 6.74781E-07, 6.93496E-07, 5.28119E-07, 5.89503E-07, + 8.12319E-07, 3.48921E-07, 7.35968E-07, 7.6714E-07, 9.76346E-07, + 5.94384E-07, 4.55885E-07, 3.03447E-07, 3.12256E-07, 9.07714E-07, + 7.85438E-07, 4.83577E-07, 5.58356E-07, 1.0008E-06, 5.1497E-07, + 6.85606E-07, 6.27599E-07, 6.50068E-07, 6.35332E-07, 1.14241E-06, + 3.45645E-07, 9.65422E-07, 4.19979E-07, 5.73274E-07, 5.8287E-07, + 4.036E-07, 7.81148E-07, 6.12824E-07, 1.26142E-06, 3.13189E-07, + 8.64758E-07, 7.11363E-07, 7.13097E-07, 7.82466E-07, 5.18369E-07, + 5.50909E-07, 6.29709E-07, 6.99076E-07, 4.10409E-07, 8.34343E-07, + 4.24549E-07, 1.04369E-06, 9.28835E-07, 5.38838E-07, 9.29732E-07, + 7.20093E-07, 5.58478E-07, 6.56105E-07, 8.47952E-07, 2.87647E-07, + 5.06522E-07, 5.41565E-07, 6.89327E-07, 8.61232E-07, 5.07766E-07, + 6.19447E-07, 7.11609E-07, 5.87218E-07, 4.1928E-07, 5.03905E-07, + 6.1528E-07, 1.01844E-06, 5.2767E-07, 8.17664E-07, 9.38428E-07, + 7.79238E-07, 8.02621E-07, 5.10544E-07, 4.11051E-07, 4.25927E-07, + 7.21417E-07, 4.47019E-07, 4.7425E-07, 7.83066E-07, 6.71848E-07, + 7.09852E-07, 5.6588E-07, 2.08407E-07, 5.14476E-07, 5.88994E-07, + 8.00861E-07, 5.34024E-07, 7.02772E-07, 7.52933E-07, 2.9196E-07, + 9.33582E-07, 6.53747E-07, 7.07025E-07, 7.40819E-07, 8.82499E-07, + 8.99769E-07, 4.75446E-07, 6.56858E-07, 6.65791E-07, 5.0845E-07, + 8.68989E-07, 6.92259E-07, 5.13645E-08, 5.07448E-07, 4.77084E-07, + 6.35104E-07, 5.36342E-07, 5.14937E-07, 9.39478E-07, 6.9814E-07, + 4.66611E-07, 4.46305E-07, 9.73917E-07, 4.43573E-07, 5.86071E-07, + 4.25134E-07, 5.40401E-07, 2.79352E-07, 2.45074E-07, 8.10726E-07, + 5.71619E-07, 6.8158E-07, 3.70016E-07, 6.48764E-07, 7.73522E-07, + 6.21811E-07, 5.79637E-07, 5.6783E-07, 3.58305E-07, 5.05352E-07, + 2.98723E-07, 5.88613E-07, 8.72919E-07, 5.3377E-07, 5.12491E-07, + 6.03532E-07, 9.41264E-07, 5.33109E-07, 3.73368E-07, 9.78579E-07, + 9.33609E-07, 4.85875E-07, 7.28735E-07, 9.79759E-07, 1.09736E-06, + 6.24609E-07, 7.8959E-07, 1.38738E-07, 4.11922E-07, 5.34056E-07, + 7.96481E-07, 5.33397E-07, 7.02661E-07, 5.54345E-07, 3.07605E-07, + 9.27642E-07, 4.02612E-07, 4.4826E-07, 8.77177E-07, 5.46556E-07, + 7.56858E-07, 8.12908E-07, 1.12244E-06, 6.81324E-07, 4.46519E-07, + 9.89779E-07, 7.84284E-07, 6.91761E-07, 2.5327E-07, 5.00255E-07, + 1.24998E-06, 4.20599E-07, 7.35953E-07, 9.88749E-07, 5.94785E-07, + 7.35253E-07, 9.03222E-07, 8.81024E-07, 1.03997E-06, 6.68177E-07, + 6.11756E-07, 8.20331E-07, 5.43699E-07, 6.48492E-07, 4.58018E-07, + 5.76385E-07, 6.15443E-07, 8.17724E-07, 1.30614E-06, 4.94546E-07, + 2.73049E-07, 1.9201E-07, 6.72776E-07, 4.91968E-07, 1.00504E-07, + 3.50675E-07, 7.5466E-07, 8.31081E-07, 6.20516E-07, 8.61186E-07, + 7.47773E-07, 7.30435E-07, 6.39607E-07, 6.5334E-07, 7.7868E-07, + 7.37545E-07, 9.48707E-07, 5.98457E-07, 3.87945E-07, 9.48542E-07, + 8.54379E-07, 6.16224E-07, 9.40769E-07, 9.24717E-07, 8.40893E-07, + 7.20019E-07, 9.45572E-07, 5.6666E-07, 9.13419E-07, 4.20845E-07, + 4.75831E-07, 6.07023E-07, 4.44465E-07, 8.4033E-07, 5.90143E-07, + 9.53968E-07, 6.04483E-07, 8.59412E-07, 6.69747E-07, 4.79243E-07, + 1.03603E-06, 7.96978E-07, 6.9984E-07, 4.33286E-07, 9.44909E-07, + 9.14619E-07, 4.5106E-07, 6.38047E-07, 9.40743E-07, 5.63481E-07, + 4.37821E-07, 6.40688E-07, 6.21255E-07, 7.85131E-07, 8.24598E-07, + 5.92203E-07, 7.57019E-07, 6.70287E-07, 8.66138E-07, 6.0037E-07, + 4.61995E-07, 7.49631E-07, 8.29553E-07, 4.13557E-07, 8.01052E-07, + 5.59415E-07, 5.29615E-07, 9.32314E-07, 7.32218E-07, 1.15559E-06, + 6.91908E-07, 8.51348E-07, 4.86207E-07, 4.44089E-07, 8.38212E-07, + 7.5097E-07, 1.08536E-06, 5.70427E-07, 6.22885E-07, 4.58644E-07, + 5.88858E-07, 7.71919E-07, 9.09943E-07, 3.34829E-07, 5.17068E-07, + 6.38138E-07, 9.79417E-07, 6.37088E-07, 1.15143E-06, 4.41621E-07, + 7.89098E-07, 7.49192E-07, 4.17254E-07, 6.14801E-07, 7.23981E-07, + 6.26778E-07, 7.52833E-07, 8.98562E-07, 8.63662E-07, 6.28047E-07, + 5.82346E-07, 5.39143E-07, 1.03085E-06, 9.58594E-07, 6.27659E-07, + 8.02338E-07, 4.67431E-07, 4.33071E-07, 6.48388E-07, 7.44739E-07, + 5.8263E-07, 8.05556E-07, 7.8856E-07, 8.58293E-07, 5.10707E-07, + 5.92764E-07, 8.54274E-07, 7.14182E-07, 1.00597E-06, 8.22293E-07, + 6.6971E-07, 6.22159E-07, 7.57922E-07, 9.92379E-07, 8.51902E-07, + 1.18208E-06, 1.03043E-06, 9.98909E-07, 6.10743E-07, 4.3555E-07, + 9.19269E-07, 6.85437E-07, 3.40787E-07, 9.08677E-07, 7.11441E-07, + 6.59073E-07, 3.96671E-07, 6.26773E-07, 6.07048E-07, 5.90329E-07, + 7.57068E-07, 1.042E-06, 3.04829E-07, 6.3825E-07, 1.47423E-07, + 2.70017E-07, 1.33766E-06, 5.69429E-07, 3.40015E-07, 5.53223E-07, + 1.07237E-06, 9.5196E-07, 4.96368E-07, 8.31239E-07, 9.5463E-08, + 1.16382E-07, 9.3383E-07, 6.28538E-07, 7.54373E-07, 7.6029E-07, + 4.51705E-07, 7.57947E-07, 6.76715E-07, 7.38647E-07, 1.27647E-06, + 3.26588E-07, 9.01306E-07, 4.00086E-07, 7.82981E-07, 4.35197E-07, + 6.34925E-07, 8.70151E-07, 4.47978E-07, 4.96528E-07, 8.75344E-07, + 5.89439E-07, 5.09808E-07, 6.4829E-07, 6.79625E-07, 6.87151E-07, + 1.00326E-06, 7.06529E-07, 5.83762E-07, 9.30897E-07, 7.37148E-07, + 5.76077E-07, 3.46547E-07, 4.74715E-07, 7.22023E-07, 8.99295E-08, + 4.74161E-07, 5.36119E-07, 4.34514E-07, 8.6132E-07, 8.82588E-07, + 7.08668E-07, 4.5464E-07, 4.79263E-07, 8.80604E-07, 8.49341E-07, + 5.2641E-07, 1.01802E-06, 7.11718E-07, 5.3727E-07, 8.69342E-07, + 8.1106E-07, 8.61435E-07, 8.23442E-07, 7.99519E-07, 4.00435E-07, + 7.63173E-07, 1.36186E-06, 8.72092E-07, 5.65605E-07, 5.16438E-07, + 5.23424E-07, 6.35271E-07, 5.1946E-07, 3.58714E-07, 7.15944E-07, + 1.19467E-06, 7.57973E-07, 1.17217E-06, 7.86832E-07, 8.22139E-07, + 9.93314E-07, 6.90562E-07, 1.04823E-06, 6.93166E-07, 5.88137E-07, + 6.9595E-07, 9.23774E-07, 7.79845E-07, 1.29296E-06, 6.93531E-07, + 7.71556E-07, 5.01213E-07, 8.29332E-07, 7.84292E-07, 7.23142E-07, + 6.09091E-07, 6.50793E-07, 7.1758E-07, 8.63491E-07, 7.37199E-07, + -7.4089E-08, 8.35999E-07, 7.12919E-07, 7.28359E-07, 6.51327E-07, + 5.94988E-07, 7.89168E-07, 8.05735E-07, 1.00568E-06, 6.05964E-07, + 7.05999E-07, 3.28728E-07, 6.90501E-07, 2.57336E-07, 6.88954E-07, + 7.39458E-07, 6.48236E-07, 6.58993E-07, 5.08107E-07, 9.7468E-07, + 6.21876E-07, 7.21331E-07, 8.35957E-07, 8.72166E-07, 8.0847E-07, + 5.29404E-07, 1.09702E-06, 7.79662E-07, 1.02473E-06, 8.00136E-07, + 4.81251E-07, 1.01676E-06, 8.98495E-07, 7.17994E-07, 8.16323E-07, + 3.31749E-07, 5.96504E-07, 6.4248E-07, 6.97075E-07, 7.83474E-07, + 7.96138E-07, 1.03076E-06, 9.36527E-07, 4.04232E-07, 8.34154E-07, + 7.54609E-07, 7.08022E-07, 9.90108E-07, 9.05433E-07, 1.0775E-06, + 5.55569E-07, 8.79434E-07, 6.35791E-07, 1.17494E-06, 9.24654E-07, + 6.31248E-07, 7.36805E-07, 9.7465E-07, 6.37868E-07, 1.36838E-06, + 6.24483E-07, 6.71469E-07, 7.95298E-07, 1.01337E-06, 3.32969E-07, + 6.4223E-07, 6.76885E-07, 8.48766E-07, 7.71914E-07, 7.88367E-07, + 7.66755E-07, 8.859E-07, 6.41637E-07, 6.5983E-07, 8.49487E-07, + 7.83005E-07, 1.18978E-06, 6.48237E-07, 7.33127E-07, 7.52263E-07, + 3.65409E-07, 1.07283E-06, 6.17127E-07, 6.44099E-07, 1.09572E-06, + 7.05965E-07, 5.53396E-07, 9.20541E-07, 9.2099E-07, 6.66099E-07, + 6.96492E-07, 5.09556E-07, 7.80707E-07, 9.01011E-07, 9.83518E-07, + 5.30961E-07, 6.45435E-07, 1.18203E-06, 9.92244E-07, 3.46591E-07, + 7.00707E-07, 7.3569E-07, 5.98252E-07, 9.49421E-07, 9.39943E-07, + 1.19374E-06, 7.64183E-07, 1.13768E-06, 1.03588E-06, 3.39583E-07, + 7.40852E-07, 5.91631E-07, 7.90072E-07, 1.0413E-06, 8.69654E-07, + 5.7419E-07, 9.97073E-07, 8.40171E-07, 7.86044E-07, 9.80443E-07, + 7.81505E-07, 9.80078E-07, 8.12703E-07, 6.15003E-07, 4.40749E-07, + 7.18715E-07, 1.11068E-06, 6.64199E-07, 9.92143E-07, 4.55201E-07, + 8.96104E-07, 8.33538E-07, 9.21129E-07, 7.94242E-07, 2.67711E-07, + 9.56535E-07, 9.5759E-07, 3.12927E-07, 1.0718E-06, 6.57949E-07, + 1.03175E-06, 2.65932E-07, 8.61841E-07, 5.81272E-07, 7.92501E-07, + 8.5889E-07, 6.00189E-07, 7.66688E-07, 6.95267E-07, 1.04901E-06, + 3.83731E-07, 1.02278E-06, 4.50037E-07, 3.54633E-07, 7.80923E-07, + 1.14843E-06, 5.97326E-07, 5.17678E-07, 8.25672E-07, 8.89943E-07, + 1.34484E-06, 7.31577E-07, 7.96753E-07, 6.90724E-07, 7.99833E-07, + 6.13949E-07, 8.61232E-07, 8.04032E-07, 6.21858E-07, 1.13479E-06, + 7.33542E-07, 6.93347E-07, 7.4396E-07, 7.10217E-07, 5.4328E-07, + 1.04889E-06, 7.39458E-07, 7.16415E-07, 1.05761E-06, 6.23901E-07, + 6.95321E-07, 9.37053E-07, 4.96784E-07, 8.33675E-07, 8.71297E-07, + 7.50956E-07, 9.76181E-07, 9.12762E-07, 6.74734E-07, 5.87794E-07, + 9.34256E-07, 7.49617E-07, 6.91564E-07, 1.15599E-06, 9.27458E-07, + 8.64946E-07, 6.34393E-07, 1.163E-06, 1.2083E-06, 6.71584E-07, + 7.467E-07, 4.67167E-07, 8.6799E-07, 8.26236E-07, 6.49105E-07, + 1.0288E-06, 5.66028E-07, 1.16242E-06, 7.43714E-07, 8.04109E-07, + 8.77172E-07, 8.45287E-07, 1.08061E-06, 8.73257E-07, 8.47922E-07, + 1.14064E-06, 9.66321E-07, 1.19805E-06, 9.52882E-07, 8.76441E-07, + 8.62864E-07, 8.6858E-07, 7.10705E-07, 6.89993E-07, 8.01967E-07, + 9.8396E-07, 9.39822E-07, 6.5701E-07, 6.55685E-07, 8.16724E-07, + 6.98231E-07, 1.17226E-06, 1.11701E-06, 6.85002E-07, 6.3326E-07, + 7.50889E-07, 6.32072E-07, 8.64675E-07, 4.77722E-07, 7.86743E-07, + 8.9173E-07, 8.66493E-07, 8.04294E-07, 1.24342E-06, 1.09745E-06, + 5.67288E-07, 1.13759E-06, 8.25677E-07, 1.00339E-06, 1.14477E-06, + 8.73911E-07, 1.0357E-06, 7.7091E-07, 9.66002E-07, 7.34938E-07, + 7.99203E-07, 7.81485E-07, 1.07401E-06, 8.30632E-07, 9.87711E-07, + 1.16139E-06, 4.99503E-07, 7.02415E-07, 8.73305E-07, 7.63255E-07, + 8.64658E-07, 9.11422E-07, 9.45615E-07, 1.43242E-06, 1.25111E-06, + 7.67838E-07, 1.19231E-06, 6.47251E-07, 4.74402E-07, 8.84412E-07, + 1.0143E-06, 7.21798E-07, 9.24691E-07, 5.86634E-07, 9.31902E-07, + 1.58816E-06, 7.52255E-07, 5.33458E-07, 1.30467E-06, 7.73324E-07, + 9.40731E-07, 7.49181E-07, 5.95982E-07, 6.21722E-07, 9.59335E-07, + 6.67062E-07, 6.82339E-07, 6.64114E-07, 7.28678E-07, 5.85814E-07, + 7.72103E-07, 6.1889E-07, 6.93982E-07, 1.03393E-06, 9.68661E-07, + 9.60956E-07, 7.34083E-07, 4.17893E-07, 8.15143E-07, 8.06252E-07, + 5.4366E-07, 8.84008E-07, 9.6047E-07, 1.07322E-06, 7.64625E-07, + 7.75806E-07, 9.98409E-07, 7.6006E-07, 1.04122E-06, 7.72776E-07, + 1.06111E-06, 7.26022E-07, 9.28308E-07, 4.00797E-07, 8.3272E-07, + 6.90645E-07, 1.16462E-06, 8.95027E-07, 9.37293E-07, 5.18907E-07, + 7.84881E-07, 6.93101E-07, 6.94006E-07, 9.24326E-07, 8.82409E-07, + 1.07037E-06, 9.72377E-07, 7.02753E-07, 2.44265E-07, 4.3405E-07, + 6.89691E-07, 1.11954E-06, 1.05286E-06, 1.07122E-06, 6.6617E-07, + 8.53269E-07, 9.21634E-07, 1.01228E-06, 9.29855E-07, 9.5902E-07, + 1.05409E-06, 6.97731E-07, 9.7755E-07, 1.17406E-06, 6.69577E-07, + 8.42354E-07, 9.8906E-07, 1.10245E-06, 8.72108E-07, 5.01064E-07, + 8.82596E-07, 6.92193E-07, 5.6957E-07, 6.37495E-07, 6.34359E-07, + 1.19115E-06, 9.54097E-07, 6.88443E-07, 6.99306E-07, 7.07803E-07, + 1.10638E-06, 1.36627E-06, 7.52874E-07, 1.03324E-06, 8.94366E-07, + 7.61462E-07, 1.17439E-06, 8.66062E-07, 5.62692E-07, 6.92464E-07, + 8.99409E-07, 6.22799E-07, 7.37371E-07, 8.60126E-07, 8.44594E-07, + 9.99771E-07, 9.74801E-07, 7.56842E-07, 5.3138E-07, 8.38478E-07, + 1.09159E-06, 7.72521E-07, 1.99399E-07, 5.03006E-07, 9.67002E-07, + 8.48901E-07, 9.89654E-07, 1.22541E-06, 8.38004E-07, 8.44552E-07, + 4.56493E-07, 6.85664E-07, 7.44531E-07, 1.06591E-06, 7.49924E-07, + 1.09124E-06, 1.18163E-06, 1.20446E-06, 4.76519E-07, 8.94345E-07, + 3.14884E-07, 1.11941E-06, 1.0202E-06, 1.36631E-06, 7.56107E-07, + 8.67986E-07, 7.06493E-07, 6.09909E-07, 7.77416E-07, 1.2596E-06, + 8.47531E-07, 1.05479E-06, 8.76432E-07, 9.786E-07, 1.1614E-06, + 1.11506E-06, 7.43139E-07, 6.56986E-07, 1.1081E-06, 1.14742E-06, + 6.2705E-07, 8.48619E-07, 1.24041E-06, 1.0665E-06, 1.14463E-06, + 6.66733E-07, 6.7127E-07, 6.36273E-07, 7.7304E-07, 9.8662E-07, + 8.03777E-07, 8.20806E-07, 9.16395E-07, 1.06008E-06, 9.78505E-07, + 3.52237E-07, 1.04273E-06, 6.67692E-07, 9.15508E-07, 7.92001E-07, + 6.41589E-07, 8.59196E-07, 1.25006E-06, 1.11173E-06, 9.19134E-07, + 8.973E-07, 1.20206E-06, 1.32887E-06, 5.40778E-07, 1.1285E-06, + 7.57389E-07, 1.09803E-06, 8.02909E-07, 7.09518E-07, 1.04255E-06, + 9.8428E-07, 9.58411E-07, 9.93108E-07, 7.19764E-07, 5.31746E-07, + 8.58355E-07, 8.34268E-07, 7.4916E-07, 1.21932E-06, 1.00119E-06, + 7.658E-07, 1.30277E-06, 8.82274E-07, 1.07979E-06, 2.39519E-07, + 6.67309E-07, 1.23185E-06, 6.14517E-07, 8.85193E-07, 9.6302E-07, + 7.79428E-07, 9.42905E-07, 8.39067E-07, 8.437E-07, 7.28212E-07, + 6.73206E-07, 1.04563E-06, 1.00965E-06, 1.15922E-06, 8.95904E-07, + 7.71634E-07, 1.31439E-06, 1.15785E-06, 9.30824E-07, 1.1291E-06, + 9.40332E-07, 9.58844E-07, 1.21479E-06, 9.83935E-07, 1.14012E-06, + 1.05523E-06, 8.65039E-07, 9.7072E-07, 9.53568E-07, 1.17753E-06, + 1.08051E-06, 3.88371E-07, 1.22734E-06, 7.92292E-07, 1.15768E-06, + 1.03087E-06, 1.20672E-06, 8.2451E-07, 6.88979E-07, 1.18245E-06, + 4.78418E-07, 9.02653E-07, 1.03626E-06, 9.68673E-07, 1.10886E-06, + 1.25685E-06, 1.0482E-06, 9.69588E-07, 1.0638E-06, 1.95597E-07, + 1.05237E-06, 6.5608E-07, 9.70894E-07, 7.71207E-07, 7.54129E-07, + 1.32389E-06, 7.53319E-07, 8.64409E-07, 1.18315E-06, 1.19807E-06, + 8.81027E-07, 1.01591E-06, 9.45173E-07, 7.17715E-07, 9.39903E-07, + 9.53732E-07, 1.13461E-06, 8.63744E-07, 1.06467E-06, 7.68493E-07, + 1.32415E-06, 1.11344E-06, 8.6706E-07, 1.00143E-06, 1.26624E-06, + 9.36024E-07, 1.12295E-06, 6.42617E-07, 9.78988E-07, 1.10093E-06, + 8.53099E-07, 6.57678E-07, 8.94819E-07, 6.08003E-07, 8.92252E-07}; -const float SIMULATED_LON[GPS_DATA_SIZE] = {41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809412f, 41.809465f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809496f, 41.809488f, 41.809433f, 41.809379f, 41.809324f, 41.809269f, 41.809215f, 41.809160f, 41.809106f, 41.809051f, 41.809034f, 41.809020f, 41.809002f, 41.808992f, 41.808992f, 41.808981f, 41.808964f, 41.808964f, 41.808936f, 41.808915f, 41.808908f, 41.808882f, 41.808880f, 41.808936f, 41.808936f, 41.808880f, 41.808926f, 41.808936f, 41.808826f, 41.808824f, 41.808796f, 41.808838f, 41.808852f, 41.808801f, 41.808782f, 41.808768f, 41.808875f, 41.808852f, 41.808739f, 41.808679f, 41.808656f, 41.808624f, 41.808600f, 41.808600f, 41.808712f, 41.808558f, 41.808572f, 41.808628f, 41.808646f, 41.808656f, 41.808747f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808768f, 41.808550f, 41.808432f, 41.808386f, 41.808399f, 41.808432f, 41.808399f, 41.808404f, 41.808404f, 41.808404f, 41.808326f, 41.808320f, 41.808320f, 41.808303f, 41.808292f, 41.808292f, 41.808303f, 41.808320f, 41.808320f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808208f, 41.808227f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808236f, 41.808264f, 41.808238f, 41.808223f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808180f, 41.808208f, 41.808208f, 41.808208f, 41.808208f, 41.808196f, 41.808180f, 41.808147f, 41.808180f, 41.808159f, 41.808129f, 41.808103f, 41.808076f, 41.808068f, 41.808068f, 41.808096f, 41.808072f, 41.808081f, 41.808068f, 41.808051f, 41.808038f, 41.808036f, 41.808033f, 41.808031f, 41.808028f, 41.808026f, 41.808023f, 41.808021f, 41.808018f, 41.808016f, 41.808013f, 41.808011f, 41.808008f, 41.808006f, 41.808003f, 41.808001f, 41.807998f, 41.807996f, 41.807993f, 41.807991f, 41.807988f, 41.807986f, 41.807983f, 41.807981f, 41.807978f, 41.807976f, 41.807973f, 41.807971f, 41.807968f, 41.807966f, 41.807963f, 41.807961f, 41.807958f, 41.807956f, 41.807956f, 41.807939f, 41.807928f, 41.807906f, 41.807900f, 41.807900f, 41.807900f, 41.807872f, 41.807852f, 41.807848f, 41.807853f, 41.807858f, 41.807863f, 41.807868f, 41.807872f, 41.807851f, 41.807844f, 41.807818f, 41.807831f, 41.807816f, 41.807796f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807782f, 41.807704f, 41.807727f, 41.807754f, 41.807843f, 41.807844f, 41.807844f, 41.807844f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807816f, 41.807788f, 41.807812f, 41.807804f, 41.807788f, 41.807788f, 41.807760f, 41.807760f, 41.807771f, 41.807734f, 41.807732f, 41.807760f, 41.807760f, 41.807732f, 41.807756f, 41.807760f, 41.807732f, 41.807732f, 41.807704f, 41.807704f, 41.807704f, 41.807624f, 41.807635f, 41.807648f, 41.807628f, 41.807620f, 41.807644f, 41.807648f, 41.807648f, 41.807666f, 41.807676f, 41.807676f, 41.807687f, 41.807677f, 41.807676f, 41.807704f, 41.807704f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807655f, 41.807659f, 41.807650f, 41.807648f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807664f, 41.807648f, 41.807648f, 41.807648f, 41.807648f, 41.807648f, 41.807648f, 41.807662f, 41.807648f, 41.807648f, 41.807676f, 41.807676f, 41.807676f, 41.807676f, 41.807676f}; +const float SIMULATED_LON[GPS_DATA_SIZE] = { + -1.74842E-07, -1.43698E-09, -5.54666E-07, -4.02391E-07, -2.23117E-07, + 1.88671E-07, -3.5403E-08, -1.23698E-07, 2.33816E-07, -5.19344E-09, + 2.14989E-08, 3.13253E-08, -2.13007E-07, -2.74618E-07, -4.58129E-07, + -1.61634E-07, -3.10375E-08, 5.97976E-09, 1.56018E-07, 2.19836E-08, + -1.92872E-07, 1.46669E-08, -8.02885E-08, 2.40283E-07, -1.97468E-07, + -2.68672E-08, 2.47379E-08, 1.85072E-08, 5.08885E-08, 2.07897E-08, + 7.31081E-08, -8.44016E-08, 2.01954E-07, -2.39677E-07, 3.69962E-07, + 1.87407E-09, 3.13371E-07, -1.88277E-08, 1.87107E-07, -5.41445E-08, + 6.0013E-07, -5.26029E-08, 2.13072E-07, 1.01267E-07, -9.02709E-08, + -6.70766E-08, 4.90507E-07, 5.97063E-07, 2.50172E-07, 4.12252E-07, + 3.72828E-07, -7.99828E-08, 1.17874E-07, -2.82005E-07, -1.14941E-07, + 2.92413E-07, 2.57534E-07, 2.33206E-07, 5.2684E-08, 8.1069E-08, + 3.28281E-07, 6.21948E-08, 3.3226E-07, 4.5595E-07, 4.14124E-08, + 8.70226E-08, 2.86791E-07, 3.99446E-07, 1.59651E-07, 1.47298E-07, + 3.34205E-07, 3.30733E-07, 6.29568E-07, 3.09161E-07, 4.44505E-07, + 3.7364E-08, 3.08953E-07, 7.83087E-07, 3.31264E-07, 4.13066E-07, + 3.7659E-07, 5.83559E-07, 8.84798E-07, 6.47322E-07, 2.77282E-07, + 5.46164E-07, 6.27263E-07, 7.88335E-07, 4.78571E-07, 5.64501E-07, + 8.07642E-07, 1.08091E-06, 5.57284E-07, 7.53156E-07, 6.9771E-07, + 1.19875E-06, 3.90133E-07, 7.25626E-07, 8.89069E-07, 8.15802E-07, + 5.37213E-07, 1.14432E-06, 9.36041E-07, 8.81542E-07, 9.5323E-07, + 8.30789E-07, 9.85588E-07, 9.94458E-07, 1.1319E-06, 1.02166E-06, + 1.212E-06, 1.15386E-06, 1.46501E-06, 1.34377E-06, 1.47415E-06, + 1.43883E-06, 1.27025E-06, 1.37272E-06, 1.27638E-06, 1.51444E-06, + 1.23756E-06, 1.11896E-06, 1.31868E-06, 1.14909E-06, 1.72765E-06, + 1.06403E-06, 1.72671E-06, 1.58306E-06, 1.98953E-06, 1.51145E-06, + 1.62799E-06, 1.85744E-06, 1.71914E-06, 2.01847E-06, 1.55683E-06, + 2.13283E-06, 1.78452E-06, 1.81388E-06, 2.01537E-06, 2.77443E-06, + 2.69883E-06, 2.35388E-06, 2.37209E-06, 2.38904E-06, 2.57429E-06, + 2.11987E-06, 2.39973E-06, 2.79557E-06, 2.97796E-06, 2.82446E-06, + 2.67604E-06, 2.73506E-06, 2.6423E-06, 2.79535E-06, 2.671E-06, + 2.72992E-06, 2.68549E-06, 3.04277E-06, 2.84237E-06, 3.21573E-06, + 3.52807E-06, 3.52327E-06, 3.21216E-06, 3.48578E-06, 3.38792E-06, + 3.03178E-06, 3.34051E-06, 3.3565E-06, 3.22759E-06, 3.63669E-06, + 3.83651E-06, 3.50946E-06, 3.50718E-06, 3.76854E-06, 3.95235E-06, + 3.80879E-06, 4.18231E-06, 3.76084E-06, 3.97083E-06, 4.24936E-06, + 4.50894E-06, 4.5313E-06, 4.22036E-06, 4.38518E-06, 4.73348E-06, + 4.58713E-06, 4.76896E-06, 4.85375E-06, 4.50045E-06, 4.93459E-06, + 4.60257E-06, 4.48873E-06, 5.06708E-06, 5.14712E-06, 4.96803E-06, + 5.17089E-06, 5.13006E-06, 5.19507E-06, 5.31662E-06, 5.10885E-06, + 5.32887E-06, 5.64757E-06, 5.85663E-06, 5.19602E-06, 5.70449E-06, + 5.89747E-06, 5.9334E-06, 6.22835E-06, 5.759E-06, 6.08503E-06, + 6.41906E-06, 6.58022E-06, 6.49082E-06, 6.04903E-06, 6.79989E-06, + 6.72814E-06, 6.56948E-06, 6.37929E-06, 6.0753E-06, 7.05238E-06, + 6.74556E-06, 7.11518E-06, 6.74694E-06, 7.17498E-06, 7.00337E-06, + 7.4202E-06, 7.22857E-06, 7.17565E-06, 7.87569E-06, 7.31919E-06, + 7.83351E-06, 7.67951E-06, 7.76318E-06, 7.77836E-06, 8.42574E-06, + 7.79381E-06, 8.03714E-06, 8.27594E-06, 7.9897E-06, 8.53773E-06, + 8.31707E-06, 8.55452E-06, 8.89383E-06, 8.7788E-06, 8.75077E-06, + 8.65245E-06, 9.15622E-06, 9.59952E-06, 9.20581E-06, 9.28821E-06, + 8.67433E-06, 9.77508E-06, 9.81606E-06, 9.71414E-06, 9.89993E-06, + 1.00336E-05, 9.92448E-06, 9.93046E-06, 1.01085E-05, 9.94495E-06, + 9.81141E-06, 1.02572E-05, 1.03388E-05, 1.04371E-05, 1.06578E-05, + 1.07632E-05, 1.0801E-05, 1.08781E-05, 1.06862E-05, 1.08676E-05, + 1.12333E-05, 1.09313E-05, 1.14511E-05, 1.16046E-05, 1.10289E-05, + 1.11995E-05, 1.20586E-05, 1.17584E-05, 1.18106E-05, 1.20865E-05, + 1.19624E-05, 1.25514E-05, 1.26332E-05, 1.23805E-05, 1.27278E-05, + 1.29555E-05, 1.30787E-05, 1.27129E-05, 1.30108E-05, 1.27719E-05, + 1.29303E-05, 1.31739E-05, 1.36754E-05, 1.35607E-05, 1.35108E-05, + 1.37622E-05, 1.3541E-05, 1.40747E-05, 1.37348E-05, 1.39242E-05, + 1.40053E-05, 1.40008E-05, 1.43779E-05, 1.45548E-05, 1.47008E-05, + 1.48928E-05, 1.51637E-05, 1.47202E-05, 1.4892E-05, 1.51119E-05, + 1.5356E-05, 1.52083E-05, 1.55988E-05, 1.58973E-05, 1.58581E-05, + 1.54842E-05, 1.56569E-05, 1.63407E-05, 1.61269E-05, 1.6541E-05, + 1.64121E-05, 1.62688E-05, 1.65154E-05, 1.66618E-05, 1.6762E-05, + 1.77232E-05, 1.74112E-05, 1.74674E-05, 1.76711E-05, 1.697E-05, + 1.76833E-05, 1.78388E-05, 1.788E-05, 1.78846E-05, 1.82848E-05, + 1.82281E-05, 1.86676E-05, 1.78981E-05, 1.86583E-05, 1.84184E-05, + 1.85799E-05, 1.88185E-05, 1.88588E-05, 1.90517E-05, 1.88702E-05, + 1.95612E-05, 1.95541E-05, 1.96544E-05, 1.99635E-05, 1.98705E-05, + 1.98518E-05, 1.9822E-05, 1.99906E-05, 2.05492E-05, 2.04496E-05, + 2.04899E-05, 2.04777E-05, 2.05575E-05, 2.07719E-05, 2.09589E-05, + 2.14999E-05, 2.0848E-05, 2.11653E-05, 2.16718E-05, 2.15925E-05, + 2.18043E-05, 2.18126E-05, 2.17071E-05, 2.21502E-05, 2.2353E-05, + 2.1964E-05, 2.24749E-05, 2.24457E-05, 2.24815E-05, 2.26374E-05, + 2.27907E-05, 2.31454E-05, 2.29867E-05, 2.32327E-05, 2.37127E-05, + 2.34223E-05, 2.35978E-05, 2.38124E-05, 2.34624E-05, 2.35823E-05, + 2.40711E-05, 2.41665E-05, 2.42042E-05, 2.43833E-05, 2.46394E-05, + 2.46247E-05, 2.50904E-05, 2.49873E-05, 2.47532E-05, 2.54394E-05, + 2.49237E-05, 2.56787E-05, 2.57265E-05, 2.56735E-05, 2.55599E-05, + 2.60245E-05, 2.59436E-05, 2.61673E-05, 2.63082E-05, 2.62199E-05, + 2.66409E-05, 2.65758E-05, 2.66739E-05, 2.69805E-05, 2.64178E-05, + 2.72706E-05, 2.70751E-05, 2.75905E-05, 2.71027E-05, 2.78494E-05, + 2.78847E-05, 2.7709E-05, 2.8331E-05, 2.76243E-05, 2.83226E-05, + 2.81676E-05, 2.82096E-05, 2.79862E-05, 2.84133E-05, 2.86782E-05, + 2.93485E-05, 2.93124E-05, 2.93678E-05, 2.92139E-05, 2.97182E-05, + 2.96879E-05, 2.96585E-05, 2.95046E-05, 2.98102E-05, 3.01522E-05, + 3.05819E-05, 3.06885E-05, 3.07289E-05, 3.05099E-05, 3.02663E-05, + 3.08782E-05, 3.07609E-05, 3.07522E-05, 3.09181E-05, 3.14313E-05, + 3.1436E-05, 3.1234E-05, 3.17747E-05, 3.18473E-05, 3.16627E-05, + 3.17932E-05, 3.22052E-05, 3.23369E-05, 3.23138E-05, 3.23427E-05, + 3.24528E-05, 3.3053E-05, 3.27163E-05, 3.27328E-05, 3.29383E-05, + 3.31338E-05, 3.29583E-05, 3.33267E-05, 3.29829E-05, 3.37763E-05, + 3.34858E-05, 3.36354E-05, 3.42679E-05, 3.39776E-05, 3.41204E-05, + 3.48733E-05, 3.40027E-05, 3.45474E-05, 3.45785E-05, 3.50267E-05, + 3.47721E-05, 3.52764E-05, 3.51182E-05, 3.50768E-05, 3.53526E-05, + 3.53002E-05, 3.5907E-05, 3.57188E-05, 3.57604E-05, 3.58546E-05, + 3.58492E-05, 3.62641E-05, 3.59599E-05, 3.64845E-05, 3.65055E-05, + 3.65364E-05, 3.64551E-05, 3.72926E-05, 3.69839E-05, 3.72464E-05, + 3.71167E-05, 3.78754E-05, 3.69113E-05, 3.77719E-05, 3.72778E-05, + 3.77936E-05, 3.81969E-05, 3.76221E-05, 3.8367E-05, 3.8226E-05, + 3.88528E-05, 3.78126E-05, 3.85755E-05, 3.87919E-05, 3.87765E-05, + 3.92154E-05, 3.88931E-05, 3.95637E-05, 3.9513E-05, 3.91258E-05, + 3.95504E-05, 3.96765E-05, 3.93899E-05, 3.98829E-05, 3.9923E-05, + 3.99587E-05, 4.05694E-05, 4.02209E-05, 4.02156E-05, 4.03727E-05, + 4.0604E-05, 4.07866E-05, 4.0823E-05, 4.13979E-05, 4.11889E-05, + 4.13418E-05, 4.10883E-05, 4.14174E-05, 4.16921E-05, 4.17177E-05, + 4.19397E-05, 4.14112E-05, 4.19968E-05, 4.23231E-05, 4.16951E-05, + 4.28704E-05, 4.24014E-05, 4.23066E-05, 4.3051E-05, 4.26863E-05, + 4.31986E-05, 4.2999E-05, 4.34881E-05, 4.33575E-05, 4.30503E-05, + 4.35587E-05, 4.30826E-05, 4.34715E-05, 4.37939E-05, 4.43174E-05, + 4.39998E-05, 4.45862E-05, 4.431E-05, 4.44014E-05, 4.44652E-05, + 4.46025E-05, 4.46263E-05, 4.46279E-05, 4.49759E-05, 4.47751E-05, + 4.53018E-05, 4.51535E-05, 4.54441E-05, 4.57844E-05, 4.58643E-05, + 4.57071E-05, 4.57996E-05, 4.59663E-05, 4.64306E-05, 4.63392E-05, + 4.60829E-05, 4.64844E-05, 4.67352E-05, 4.66102E-05, 4.6818E-05, + 4.69511E-05, 4.66908E-05, 4.71509E-05, 4.72848E-05, 4.75273E-05, + 4.72681E-05, 4.74569E-05, 4.75588E-05, 4.75426E-05, 4.80719E-05, + 4.79121E-05, 4.82426E-05, 4.85834E-05, 4.85861E-05, 4.85626E-05, + 4.83646E-05, 4.90569E-05, 4.89256E-05, 4.88528E-05, 4.87774E-05, + 4.87468E-05, 4.93922E-05, 4.92511E-05, 4.96346E-05, 4.97331E-05, + 4.96802E-05, 5.02926E-05, 4.99139E-05, 4.99846E-05, 5.01785E-05, + 5.03402E-05, 5.00806E-05, 5.02935E-05, 5.03918E-05, 5.09243E-05, + 5.08605E-05, 5.10304E-05, 5.07244E-05, 5.15329E-05, 5.15249E-05, + 5.14659E-05, 5.13756E-05, 5.13327E-05, 5.13873E-05, 5.19527E-05, + 5.16721E-05, 5.20402E-05, 5.21446E-05, 5.22459E-05, 5.26861E-05, + 5.24207E-05, 5.26073E-05, 5.22335E-05, 5.28373E-05, 5.30041E-05, + 5.30021E-05, 5.2717E-05, 5.32482E-05, 5.30845E-05, 5.31812E-05, + 5.37672E-05, 5.35874E-05, 5.35896E-05, 5.39094E-05, 5.36587E-05, + 5.38527E-05, 5.44327E-05, 5.45892E-05, 5.41071E-05, 5.43866E-05, + 5.49208E-05, 5.48037E-05, 5.49447E-05, 5.53776E-05, 5.52516E-05, + 5.49569E-05, 5.53238E-05, 5.55869E-05, 5.53022E-05, 5.55632E-05, + 5.53718E-05, 5.60733E-05, 5.56712E-05, 5.60158E-05, 5.6214E-05, + 5.58406E-05, 5.6417E-05, 5.67829E-05, 5.70215E-05, 5.66799E-05, + 5.66628E-05, 5.70953E-05, 5.67459E-05, 5.75115E-05, 5.70824E-05, + 5.70472E-05, 5.71147E-05, 5.76551E-05, 5.77437E-05, 5.80687E-05, + 5.76778E-05, 5.79731E-05, 5.7565E-05, 5.80392E-05, 5.8317E-05, + 5.84187E-05, 5.87538E-05, 5.88184E-05, 5.90267E-05, 5.91042E-05, + 5.88851E-05, 5.9574E-05, 5.91351E-05, 5.93028E-05, 5.92032E-05, + 5.90531E-05, 5.96031E-05, 5.96927E-05, 5.99122E-05, 6.01678E-05, + 6.05515E-05, 6.01938E-05, 6.02091E-05, 6.02936E-05, 6.03631E-05, + 6.05829E-05, 6.06069E-05, 6.04865E-05, 6.12117E-05, 6.12292E-05, + 6.05379E-05, 6.10265E-05, 6.18976E-05, 6.18814E-05, 6.19784E-05, + 6.17416E-05, 6.14828E-05, 6.18693E-05, 6.22055E-05, 6.22036E-05, + 6.19394E-05, 6.22341E-05, 6.23318E-05, 6.26086E-05, 6.25904E-05, + 6.23753E-05, 6.28976E-05, 6.32297E-05, 6.28663E-05, 6.33194E-05, + 6.30031E-05, 6.34179E-05, 6.35567E-05, 6.38062E-05, 6.39398E-05, + 6.35886E-05, 6.3862E-05, 6.37822E-05, 6.41548E-05, 6.40523E-05, + 6.43069E-05, 6.41644E-05, 6.44279E-05, 6.46926E-05, 6.45171E-05, + 6.44539E-05, 6.49285E-05, 6.48881E-05, 6.52714E-05, 6.49178E-05, + 6.54191E-05, 6.56162E-05, 6.56923E-05, 6.55846E-05, 6.60856E-05, + 6.60651E-05, 6.57057E-05, 6.60446E-05, 6.61061E-05, 6.62905E-05, + 6.63266E-05, 6.65456E-05, 6.6485E-05, 6.67239E-05, 6.65219E-05, + 6.69511E-05, 6.67189E-05, 6.6919E-05, 6.69672E-05, 6.71336E-05, + 6.7401E-05, 6.74825E-05, 6.745E-05, 6.76238E-05, 6.79005E-05, + 6.77092E-05, 6.78976E-05, 6.82092E-05, 6.86002E-05, 6.81367E-05, + 6.88185E-05, 6.86054E-05, 6.87078E-05, 6.91751E-05, 6.88129E-05, + 6.88867E-05, 6.935E-05, 6.91528E-05, 6.94951E-05, 6.93939E-05, + 6.90065E-05, 6.94156E-05, 6.96123E-05, 6.95861E-05, 6.9767E-05, + 7.00486E-05, 7.01037E-05, 7.01033E-05, 7.04899E-05, 7.01639E-05, + 7.04945E-05, 7.01751E-05, 7.04892E-05, 7.10621E-05, 7.10951E-05, + 7.10594E-05, 7.08086E-05, 7.08717E-05, 7.14405E-05, 7.1252E-05, + 7.14563E-05, 7.19803E-05, 7.19249E-05, 7.15269E-05, 7.19118E-05, + 7.23974E-05, 7.19321E-05, 7.19468E-05, 7.21616E-05, 7.23578E-05, + 7.25625E-05, 7.21701E-05, 7.26515E-05, 7.29559E-05, 7.27545E-05, + 7.27504E-05, 7.31683E-05, 7.32067E-05, 7.35382E-05, 7.34771E-05, + 7.36716E-05, 7.38431E-05, 7.34911E-05, 7.36841E-05, 7.39104E-05, + 7.43433E-05, 7.40525E-05, 7.40761E-05, 7.4413E-05, 7.41736E-05, + 7.52072E-05, 7.45261E-05, 7.47582E-05, 7.49798E-05, 7.45996E-05, + 7.48501E-05, 7.50166E-05, 7.48161E-05, 7.52879E-05, 7.51987E-05, + 7.53722E-05, 7.58225E-05, 7.57622E-05, 7.58545E-05, 7.61049E-05, + 7.59664E-05, 7.63501E-05, 7.60327E-05, 7.601E-05, 7.63797E-05, + 7.67704E-05, 7.676E-05, 7.68394E-05, 7.6889E-05, 7.73226E-05, + 7.69956E-05, 7.69341E-05, 7.67913E-05, 7.73366E-05, 7.73796E-05, + 7.70388E-05, 7.77687E-05, 7.75884E-05, 7.72375E-05, 7.7562E-05, + 7.82232E-05, 7.80391E-05, 7.82003E-05, 7.85904E-05, 7.84134E-05, + 7.84694E-05, 7.85027E-05, 7.8855E-05, 7.89383E-05, 7.89223E-05, + 7.90722E-05, 7.91763E-05, 7.91493E-05, 7.93268E-05, 7.9599E-05, + 7.93344E-05, 7.95676E-05, 7.96048E-05, 7.98594E-05, 7.98712E-05, + 8.01175E-05, 8.01325E-05, 7.97035E-05, 8.02953E-05, 8.0169E-05, + 8.06882E-05, 8.09835E-05, 8.08845E-05, 8.09728E-05, 8.05278E-05, + 8.0959E-05, 8.09602E-05, 8.16246E-05, 8.10656E-05, 8.15875E-05, + 8.13485E-05, 8.12411E-05, 8.17574E-05, 8.14807E-05, 8.17573E-05, + 8.20176E-05, 8.17697E-05, 8.25216E-05, 8.23022E-05, 8.24016E-05, + 8.23206E-05, 8.2494E-05, 8.27879E-05, 8.28925E-05, 8.26265E-05, + 8.3487E-05, 8.33099E-05, 8.32545E-05, 8.31823E-05, 8.33141E-05, + 8.34378E-05, 8.32539E-05, 8.43139E-05, 8.35052E-05, 8.37188E-05, + 8.44156E-05, 8.40837E-05, 8.44789E-05, 8.44372E-05, 8.40002E-05, + 8.41137E-05, 8.46393E-05, 8.48501E-05, 8.47134E-05, 8.4936E-05, + 8.44708E-05, 8.48256E-05, 8.53661E-05, 8.48658E-05, 8.4871E-05, + 8.53654E-05, 8.57668E-05, 8.5634E-05, 8.55549E-05, 8.58815E-05, + 8.56756E-05, 8.61648E-05, 8.61027E-05, 8.63716E-05, 8.59768E-05, + 8.64122E-05, 8.67038E-05, 8.69841E-05, 8.6372E-05, 8.65631E-05, + 8.69486E-05, 8.66044E-05, 8.68605E-05, 8.7265E-05, 8.77564E-05, + 8.74281E-05, 8.71789E-05, 8.7587E-05, 8.77832E-05, 8.75239E-05, + 8.7508E-05, 8.79789E-05, 8.78462E-05, 8.81884E-05, 8.82138E-05, + 8.8648E-05, 8.89153E-05, 8.88117E-05, 8.86022E-05, 8.89003E-05, + 8.88431E-05, 8.87245E-05, 8.88994E-05, 8.92278E-05, 8.87199E-05, + 8.92607E-05, 8.95076E-05, 8.93227E-05, 8.98147E-05, 8.97439E-05, + 8.97411E-05, 8.98322E-05, 8.9803E-05, 9.01968E-05, 9.02114E-05, + 9.03658E-05, 9.02286E-05, 9.01706E-05, 8.9996E-05, 9.11892E-05, + 9.04786E-05, 9.06867E-05, 9.06153E-05, 9.06483E-05, 9.1574E-05, + 9.09279E-05, 9.12144E-05, 9.14887E-05, 9.18096E-05, 9.17803E-05, + 9.13073E-05, 9.13352E-05, 9.1845E-05, 9.1538E-05, 9.19866E-05, + 9.23044E-05, 9.20543E-05, 9.26326E-05, 9.23586E-05, 9.25261E-05, + 9.28034E-05, 9.24722E-05, 9.24912E-05, 9.32681E-05, 9.32993E-05, + 9.31663E-05, 9.32453E-05, 9.30912E-05, 9.31702E-05, 9.33058E-05, + 9.31825E-05, 9.36462E-05, 9.32959E-05, 9.36245E-05, 9.38519E-05, + 9.42948E-05, 9.37456E-05, 9.40234E-05, 9.44975E-05, 9.399E-05, + 9.4332E-05, 9.46454E-05, 9.43883E-05, 9.51645E-05, 9.48064E-05, + 9.51989E-05, 9.50716E-05, 9.50848E-05, 9.50654E-05, 9.53231E-05, + 9.52265E-05, 9.53674E-05, 9.56336E-05, 9.57968E-05, 9.57107E-05, + 9.59629E-05, 9.58575E-05, 9.61211E-05, 9.61882E-05, 9.632E-05, + 9.66913E-05, 9.65983E-05, 9.6861E-05, 9.64922E-05, 9.69722E-05, + 9.69253E-05, 9.70342E-05, 9.71645E-05, 9.71095E-05, 9.76141E-05, + 9.74688E-05, 9.72902E-05, 9.7741E-05, 9.7345E-05, 9.78263E-05, + 9.76857E-05, 9.83573E-05, 9.79769E-05, 9.83281E-05, 9.83002E-05, + 9.83298E-05, 9.83446E-05, 9.81482E-05, 9.83431E-05, 9.8386E-05, + 9.85918E-05, 9.88738E-05, 9.89461E-05, 9.90952E-05, 9.93306E-05, + 9.87831E-05, 9.93083E-05, 9.94452E-05, 9.97238E-05, 9.95871E-05, + 9.96113E-05, 9.95488E-05, 9.96361E-05, 9.99507E-05, 0.000100022, + 0.000100194, 0.000100093, 0.000100038, 0.000100003, 0.000100228, + 0.000100546, 0.000100711, 0.000100595, 0.000100599, 0.000101023, + 0.000101112, 0.00010087, 0.000101348, 0.000101303, 0.00010146, + 0.000101277, 0.000101524, 0.000101624, 0.0001015, 0.000101798, + 0.00010181, 0.000101459, 0.000102195, 0.000101981, 0.000102125, + 0.000102283, 0.000102971, 0.000102912, 0.000102856, 0.000103006, + 0.000103206, 0.000102899, 0.000103265, 0.000103335, 0.000103445, + 0.000103402, 0.000103433, 0.000103356, 0.000103936, 0.00010363, + 0.000103861, 0.000104033, 0.000104142, 0.00010406, 0.00010416, + 0.000104405, 0.000104151, 0.000104628, 0.000104381, 0.000104339, + 0.000104705, 0.000104713, 0.00010495, 0.000104998, 0.000105018, + 0.000104898, 0.00010533, 0.000105172, 0.00010487, 0.000106115, + 0.000105545, 0.000105385, 0.000105428, 0.000105651, 0.000105919, + 0.000106017, 0.000105788, 0.000105947, 0.000106304, 0.000105985, + 0.000106554, 0.000106425, 0.000106391, 0.000106852, 0.000107028, + 0.000106971, 0.000107038, 0.000107096, 0.000107439, 0.000107319, + 0.00010713, 0.00010758, 0.00010708, 0.00010774, 0.000107722, + 0.000107631, 0.00010869, 0.000108556, 0.000108403, 0.00010814, + 0.000108115, 0.000108398, 0.00010858, 0.000108666, 0.000108905, + 0.000108795, 0.000108697, 0.000109171, 0.000108974, 0.000109598, + 0.000109409, 0.000109221, 0.000109532, 0.000109295, 0.000109853, + 0.000109545, 0.000109443, 0.0001095, 0.000109935, 0.000110097, + 0.000110062, 0.000110695, 0.000110453, 0.00011057, 0.000110599, + 0.000110525, 0.000110656, 0.000110575, 0.000111184, 0.000111084, + 0.000111058, 0.000111337, 0.000111014, 0.000110935, 0.00011099, + 0.000111254, 0.000111314, 0.000111701, 0.000111588, 0.000111582, + 0.000111932, 0.00011204, 0.000112176, 0.000112038, 0.000112545, + 0.000112739, 0.000112618, 0.000112344, 0.000112534, 0.000112507, + 0.000113126, 0.000112899, 0.000112967, 0.00011305, 0.00011304, + 0.00011317, 0.000113575, 0.000113358, 0.000113559, 0.000113588, + 0.000112968, 0.000113938, 0.000113831, 0.00011397, 0.000114312, + 0.00011443, 0.000113934, 0.000114458, 0.00011458, 0.000114507, + 0.000114828, 0.000115114, 0.000114414, 0.000114997, 0.000115027, + 0.000114647, 0.000114904, 0.000115538, 0.000115286, 0.000115445, + 0.000115798, 0.000115585, 0.000115629, 0.000115609, 0.000116293, + 0.000115968, 0.000116284, 0.000116329, 0.000116094, 0.000116492, + 0.000116616, 0.000116204, 0.000116148, 0.000116572, 0.000116475, + 0.000116943, 0.000117096, 0.00011719, 0.000116634, 0.00011663, + 0.000117188, 0.000117805, 0.000117579, 0.000117608, 0.000117931, + 0.000117772, 0.00011774, 0.00011802, 0.000117677, 0.000118072, + 0.000118137, 0.000118114, 0.000118212, 0.000118027, 0.000118538, + 0.000118514, 0.000118507, 0.000118909, 0.000118884, 0.000118631, + 0.000119074, 0.000119104, 0.000118684, 0.000119409, 0.000119479, + 0.000119595, 0.000119509, 0.000119972, 0.000119884, 0.000119581, + 0.000120132, 0.000120012, 0.000119884, 0.000120071, 0.000120379, + 0.000120155, 0.000120317, 0.000120468, 0.000120444, 0.000120522, + 0.000120824, 0.000120813, 0.000121154, 0.000121278, 0.000120965, + 0.000120866, 0.000121549, 0.000121148, 0.000121877, 0.000121519, + 0.000121789, 0.000121721, 0.000121526, 0.000121893, 0.00012175, + 0.000122, 0.000122392, 0.00012228, 0.000122241, 0.000122478, + 0.000122712, 0.000122576, 0.000122991, 0.000122786, 0.000122641, + 0.000122834, 0.000123295, 0.000123204, 0.00012337, 0.000123323, + 0.000123537, 0.000123584, 0.000123164, 0.000123075, 0.00012356, + 0.00012377, 0.000124265, 0.000123988, 0.000124141, 0.000124005, + 0.000124407, 0.0001245, 0.000124616, 0.000124172, 0.000124677, + 0.000124833, 0.000124812, 0.000124727, 0.000125278, 0.000125017, + 0.000124718, 0.000125466, 0.000125341, 0.000125097, 0.00012497, + 0.000125439, 0.000125901, 0.000125333, 0.000126408, 0.000125859, + 0.000126248, 0.000126085, 0.000126706, 0.000126271, 0.000126237, + 0.000126502, 0.000126643, 0.000126313, 0.00012642, 0.000126825, + 0.000126739, 0.00012678, 0.000127098, 0.000127298, 0.000126856, + 0.000126995, 0.000127149, 0.000127357, 0.000127536, 0.000127291, + 0.000127489, 0.000127719, 0.000127739, 0.000128051, 0.000127628, + 0.000128167, 0.000128089, 0.000128337, 0.000128418, 0.000128217, + 0.000128272, 0.000128745, 0.000128905, 0.000128552, 0.000128961, + 0.000128948, 0.000129073, 0.000129106, 0.000129348, 0.000129462, + 0.000129213, 0.000129236, 0.000129422, 0.00012961, 0.000130121, + 0.000129987, 0.000129766, 0.000129878, 0.00012999, 0.00013024, + 0.000130123, 0.000130552, 0.000130641, 0.000130298, 0.000130286, + 0.000130607, 0.000130831, 0.000130878, 0.000130921, 0.000131, + 0.000130846, 0.000131325, 0.000131028, 0.000131736, 0.000131321, + 0.000131755, 0.000131268, 0.000131499, 0.000131905, 0.00013166, + 0.000131896, 0.000131997, 0.000132215, 0.000132316, 0.00013252, + 0.000132033, 0.000132533, 0.000132248, 0.000132045, 0.000132512, + 0.000132365, 0.000132827, 0.000132756, 0.00013279, 0.000132882, + 0.000132962, 0.000132965, 0.000133239, 0.000133244, 0.000133672, + 0.000133556, 0.000133836, 0.000133873, 0.000134165, 0.000134059, + 0.000134178, 0.000133812, 0.000134159, 0.000134283, 0.000134128, + 0.000134329, 0.000134073, 0.000134498, 0.000134933, 0.000134524, + 0.000134895, 0.000134483, 0.000134842, 0.000134903, 0.000135252, + 0.000135527, 0.000135959, 0.00013519, 0.000135505, 0.00013577, + 0.000135754, 0.000135502, 0.000135688, 0.000135993, 0.000136136, + 0.000136015, 0.000136229, 0.000136391, 0.000135676, 0.000136093, + 0.000136664, 0.000136609, 0.000136842, 0.000136516, 0.000137013, + 0.000136956, 0.000136965, 0.000137115, 0.000137056, 0.000137018, + 0.000137432, 0.000137575, 0.000137492, 0.000137283, 0.000137783, + 0.000137622, 0.00013762, 0.000138033, 0.000138115, 0.000138255, + 0.000137852, 0.000138161, 0.000138247, 0.000138253, 0.000138476, + 0.000138301, 0.000138975, 0.00013836, 0.000138931, 0.000138961, + 0.000139191, 0.000139015, 0.000139095, 0.000139423, 0.00013974, + 0.000139344, 0.000139523, 0.000139857, 0.000139685, 0.000139842, + 0.000140308, 0.000140064, 0.000139726, 0.000139535, 0.000140272, + 0.000140261, 0.00014011, 0.000140163, 0.000140704, 0.000140547, + 0.000140476, 0.000140919, 0.00014079, 0.000141151, 0.000140724, + 0.000140939, 0.00014113, 0.000141357, 0.00014123, 0.000141538, + 0.000141348, 0.000141681, 0.000141821, 0.000141798, 0.000141393, + 0.000141982, 0.000141734, 0.000142244, 0.000142145, 0.000141963, + 0.000142055, 0.000142265, 0.000142665, 0.000142358, 0.000142457, + 0.000143157, 0.000142869, 0.000142852, 0.00014288, 0.00014252, + 0.000143593, 0.000143024, 0.000143169, 0.000143196, 0.000143483, + 0.000143676, 0.000143821, 0.000143635, 0.000143701, 0.000143907, + 0.000143921, 0.00014374, 0.00014384, 0.000143807, 0.000144342, + 0.000144662, 0.000144433, 0.000144409, 0.000144448, 0.000144449, + 0.000145057, 0.000145062, 0.000144861, 0.000144992, 0.000145052, + 0.000145429, 0.000145682, 0.000145439, 0.000145181, 0.000145522, + 0.000145698, 0.000145873, 0.000146043, 0.000145463, 0.000145871, + 0.000146068, 0.000146021, 0.000145992, 0.000146015, 0.000146621, + 0.000146446, 0.000145913, 0.000146311, 0.000146757, 0.000146243, + 0.000146863, 0.000146668, 0.000146902, 0.000147098, 0.000146945, + 0.000146883, 0.000146972, 0.000147571, 0.000147082, 0.000147331, + 0.00014755, 0.000147756, 0.000147786, 0.000147581, 0.000147609, + 0.000148169, 0.000147862, 0.000148157, 0.000148403, 0.000148398, + 0.000148, 0.000148353, 0.000148456, 0.000148236, 0.00014831, + 0.000148512, 0.000148794, 0.000149022, 0.000148536, 0.00014911, + 0.000148912, 0.00014903, 0.00014934, 0.000149462, 0.000149687, + 0.000149808, 0.000149688, 0.000149877, 0.000149908, 0.000149737, + 0.000149928, 0.000150196, 0.000150071, 0.000150585, 0.000149831, + 0.000150162, 0.000150416, 0.000150173, 0.000150383, 0.000150603, + 0.000150631, 0.000150803, 0.000150994, 0.000151079, 0.000151063, + 0.00015098, 0.000150814, 0.000151323, 0.000151362, 0.000151131, + 0.00015143, 0.000151759, 0.000151697, 0.000151539, 0.000151977, + 0.000152119, 0.000151495, 0.000151962, 0.000152396, 0.000152118, + 0.000152324, 0.000152484, 0.000152583, 0.000152979, 0.000152257, + 0.000153027, 0.000152961, 0.00015291, 0.000153141, 0.000152745, + 0.000153268, 0.000152807, 0.000152981, 0.000153498, 0.000153227, + 0.000153815, 0.000153605, 0.000153557, 0.000154066, 0.000153627, + 0.00015419, 0.000154136, 0.000153978, 0.000154182, 0.000154179, + 0.000154456, 0.000154726, 0.000154643, 0.000154265, 0.00015434, + 0.000154854, 0.000154887, 0.000155159, 0.000155056, 0.000155339, + 0.000155051, 0.000155363, 0.000155108, 0.000155529, 0.000155551, + 0.00015595, 0.000155978, 0.000155941, 0.000155444, 0.000155924, + 0.000156187, 0.000155939, 0.00015596, 0.000156, 0.000156138, + 0.000156006, 0.000156434, 0.000156697, 0.000156307, 0.000156541, + 0.00015686, 0.000156416, 0.000156659, 0.000156885, 0.000156774, + 0.000157484, 0.000157672, 0.000157453, 0.000157114, 0.000157972, + 0.000157926, 0.000157596, 0.000157968, 0.000157611, 0.000158167, + 0.000157726, 0.000158002, 0.000157799, 0.000158158, 0.000158364, + 0.000158451, 0.0001588, 0.000158662, 0.00015878, 0.000158389, + 0.0001589, 0.000158791, 0.000158754, 0.000158862, 0.000158933, + 0.000159243, 0.000158947, 0.000159354, 0.000159058, 0.000159479, + 0.000159579, 0.000159315, 0.000159632, 0.000159793, 0.00015993, + 0.000159841, 0.000159952, 0.000159932, 0.000160374, 0.000160116, + 0.000160206, 0.00016041, 0.000160679, 0.000160619, 0.000161142, + 0.000160815, 0.000160637, 0.00016067, 0.000160974, 0.000160934, + 0.000161029, 0.000161064, 0.000161164, 0.000161267, 0.000161623, + 0.000161913, 0.000161642, 0.00016132, 0.000161615, 0.000161738, + 0.000162161, 0.000161832, 0.000161986, 0.000162591, 0.000162346, + 0.000161885, 0.00016238, 0.000162804, 0.000162372, 0.00016286, + 0.000162647, 0.000162593, 0.000162541, 0.000162885, 0.000163218, + 0.000162897, 0.000163034, 0.000162948, 0.000163072, 0.000163473, + 0.000163075, 0.000163558, 0.000163628, 0.000163971, 0.000164212, + 0.000163799, 0.000164451, 0.000164174, 0.000163639, 0.000164236, + 0.00016443, 0.000164077, 0.000164015, 0.000164706, 0.0001646, + 0.00016474, 0.00016472, 0.000164892, 0.000165054, 0.000164851, + 0.000165147, 0.000164847, 0.000165194, 0.000165218, 0.000165441, + 0.000165325, 0.000165637, 0.000165234, 0.000165974, 0.000165942, + 0.000165754, 0.00016602, 0.000166046, 0.000166184, 0.00016601, + 0.000165709, 0.000165915, 0.000166902, 0.000166605, 0.000166699, + 0.000166456, 0.000166855, 0.000166708, 0.000167269, 0.000166718, + 0.000166921, 0.000167105, 0.000167041, 0.000167387, 0.000166787, + 0.000167431, 0.000167399, 0.000167603, 0.000167186, 0.000167693, + 0.000167762, 0.000167578, 0.000168108, 0.000168488, 0.000168389, + 0.000167947, 0.000167938, 0.000168185, 0.0001687, 0.000168383, + 0.0001685, 0.000168796, 0.000168593, 0.000168702, 0.000168999, + 0.000169293, 0.000169044, 0.000169258, 0.000169173, 0.000169179, + 0.000169627, 0.000169403, 0.000169712, 0.000169569, 0.000169812, + 0.000169644, 0.000169971, 0.000170127, 0.000169731, 0.000169787, + 0.000170014, 0.000170098, 0.000170174, 0.000170448, 0.000169889, + 0.000170341, 0.000170332, 0.000170479, 0.000170425, 0.000170757, + 0.000170575, 0.000170601, 0.000171096, 0.000170868, 0.000171132, + 0.000171386, 0.000171474, 0.00017131, 0.000171733, 0.000171245, + 0.000171505, 0.000171742, 0.000172022, 0.000171943, 0.000171938, + 0.000171855, 0.000172392, 0.000172107, 0.000172342, 0.000172446, + 0.000172261, 0.000172862, 0.000172636, 0.000172525, 0.000172761, + 0.00017302, 0.000172968, 0.000173233, 0.000172913, 0.000173483, + 0.000173138, 0.000173146, 0.000173014, 0.000173317, 0.000173665, + 0.000173944, 0.000173608, 0.000173821, 0.000173792, 0.000173866, + 0.000173939, 0.00017411, 0.000174248, 0.000174148, 0.000174268, + 0.000174209, 0.000174301, 0.000174369, 0.000174613, 0.000174378, + 0.000174902, 0.000175014, 0.000174751, 0.000174988, 0.000175154, + 0.000175053, 0.000174936, 0.000175556, 0.000175381, 0.000175287, + 0.00017574, 0.000175935, 0.000175691, 0.000175845, 0.000175484, + 0.000175617, 0.000176023, 0.000176472, 0.000175989, 0.000175929, + 0.000176325, 0.000176537, 0.000176449, 0.000176273, 0.000176943, + 0.000176882, 0.000176396, 0.000176695, 0.0001768, 0.000176889, + 0.000176849, 0.000177473, 0.000177218, 0.000177556, 0.000177391, + 0.000177764, 0.000177393, 0.000177536, 0.000177696, 0.000177552, + 0.000177727, 0.000178209, 0.000177863, 0.000177814, 0.000178154, + 0.000178288, 0.00017793, 0.000178132, 0.000178495, 0.000178506, + 0.000178632, 0.000178705, 0.000178973, 0.000178832, 0.000178805, + 0.000178985, 0.000178903, 0.00017906, 0.000178938, 0.000178755, + 0.000179452, 0.000179253, 0.000179406, 0.000179514, 0.000179495, + 0.000180054, 0.000179965, 0.000179524, 0.000179695, 0.000180206, + 0.000179653, 0.000180092, 0.000180881, 0.00018016, 0.000180215, + 0.000180288, 0.000180583, 0.000180502, 0.000180632, 0.000180586, + 0.00018067, 0.0001807, 0.000180775, 0.000180895, 0.000181111, + 0.000180869, 0.000181282, 0.000181078, 0.000181677, 0.000181526, + 0.000181468, 0.000181577, 0.00018155, 0.000181635, 0.000182169, + 0.000181897, 0.000181933, 0.000181674, 0.000182407, 0.000182123, + 0.000182468, 0.000182169, 0.000182317, 0.000182826, 0.000182515, + 0.000182752, 0.000182759, 0.000182677, 0.000183296, 0.000183161, + 0.000183269, 0.000183473, 0.000182815, 0.000183368, 0.000183369, + 0.000183484, 0.000183276, 0.000183754, 0.000183544, 0.000183935, + 0.000183457, 0.000184221, 0.000184415, 0.000183981, 0.000184269, + 0.000184202, 0.000184358, 0.000184337, 0.000183918, 0.000184149, + 0.000184864, 0.00018474, 0.000184832, 0.000184895, 0.000184443, + 0.000184828, 0.000185533, 0.000185135, 0.000184673, 0.000185565, + 0.000185127, 0.000185527, 0.000185342, 0.000185668, 0.000185662, + 0.000185519, 0.000185784, 0.000186153, 0.000186136, 0.000186158, + 0.00018615, 0.000185922, 0.000186678, 0.000186487, 0.000186309, + 0.000186221, 0.000186836, 0.000186536, 0.000186309, 0.00018697, + 0.000186936, 0.000186518, 0.000186816, 0.000186831, 0.000186751, + 0.000187421, 0.00018746, 0.000187352, 0.000187204, 0.000187538, + 0.000187621, 0.000187546, 0.000187597, 0.000187921, 0.000187665, + 0.000187594, 0.000187755, 0.000188155, 0.00018809, 0.000188551, + 0.000188565, 0.000188298, 0.000188406, 0.000188524, 0.000188482, + 0.00018883, 0.00018909, 0.000189373, 0.000188547, 0.000188915, + 0.000188648, 0.000189166, 0.000189068, 0.000189557, 0.000189299, + 0.000189577, 0.00018948, 0.000189226, 0.000189471, 0.000189795, + 0.00018972, 0.000190287, 0.000190055, 0.000190061, 0.000190428, + 0.000190065, 0.000190402, 0.000190256, 0.000190006, 0.000190657, + 0.00019025, 0.000190721, 0.000190551, 0.000190987, 0.000190311, + 0.000191317, 0.000191238, 0.00019125, 0.00019124, 0.000191004, + 0.000191556, 0.000191574, 0.000191771, 0.000190945, 0.000191786, + 0.000191338, 0.000191684, 0.000191968, 0.000192126, 0.000192154, + 0.000191919, 0.000191732, 0.000192018, 0.000192226, 0.000192127, + 0.000192054, 0.000192944, 0.00019261, 0.000192514, 0.000192806, + 0.000192377, 0.000192899, 0.000192826, 0.000193226, 0.000193154, + 0.0001932, 0.000193128, 0.000193061, 0.000193325, 0.000193724, + 0.000193282, 0.000193756, 0.000194111, 0.000193947, 0.00019365, + 0.000194337, 0.000194056, 0.000193778, 0.000194125, 0.000194214, + 0.000194474, 0.000194523, 0.000194703, 0.000194452, 0.000194598, + 0.000194413, 0.000194865, 0.000194887, 0.000194951, 0.000195033, + 0.000195074, 0.000195288, 0.000195096, 0.00019539, 0.000195038, + 0.000195259, 0.000195512, 0.000195596, 0.000195821, 0.000195093, + 0.000195309, 0.000196041, 0.000195918, 0.000195944, 0.000196524, + 0.000196333, 0.000195685, 0.000196103, 0.000196779, 0.00019648, + 0.000196249, 0.000196316, 0.000196935, 0.000196353, 0.000196703, + 0.000196724, 0.000196934, 0.000197085, 0.000197261, 0.000197354, + 0.000197351, 0.000197331, 0.000197116, 0.000197693, 0.000197573, + 0.0001974, 0.000197961, 0.000197901, 0.000197998, 0.000197982, + 0.000198268, 0.000198383, 0.000198324, 0.000198112, 0.000198286, + 0.000198422, 0.000198098, 0.000198313, 0.000198275, 0.000198938, + 0.000198979, 0.0001993, 0.000198666, 0.000198844, 0.000199255, + 0.000199148, 0.000199398, 0.000198945, 0.00019903, 0.000199264, + 0.000199493, 0.000199295, 0.000199444, 0.000199977, 0.000199567, + 0.000199458, 0.000200074, 0.000200012, 0.000200007, 0.00019972, + 0.000200342, 0.000200295, 0.000200192, 0.000200241, 0.000200405, + 0.000200319, 0.000200646, 0.000201118, 0.000200526, 0.000200638, + 0.000200805, 0.000201327, 0.000201288, 0.000200846, 0.000201322, + 0.000201575, 0.000201533, 0.000201377, 0.00020153, 0.000201628, + 0.000201931, 0.000201811, 0.000201777, 0.000201923, 0.000201624, + 0.000202196, 0.000202466, 0.000201979, 0.000202177, 0.000202013, + 0.000202486, 0.00020305, 0.000202604, 0.000202438, 0.000202754, + 0.000202833, 0.00020302, 0.00020246, 0.000202853, 0.000203155, + 0.000203379, 0.000202699, 0.000203371, 0.000203288, 0.000203628, + 0.000203132, 0.000203507, 0.000203286, 0.000204256, 0.000203808, + 0.000203819, 0.000204131, 0.000204011, 0.00020374, 0.000204365, + 0.000204161, 0.00020424, 0.000204149, 0.000204552, 0.000204438, + 0.000203991, 0.000204584, 0.000204351, 0.000205068, 0.00020508, + 0.000204759, 0.000204981, 0.000205225, 0.000205203, 0.000205406, + 0.000205405, 0.000204973, 0.000205566, 0.000205209, 0.00020557, + 0.00020577, 0.000205441, 0.000206037, 0.000205844, 0.000205684, + 0.000206081, 0.000205828, 0.000206342, 0.000206331, 0.000206208, + 0.000206651, 0.000206378, 0.000206545, 0.000206793, 0.000206251, + 0.000206543, 0.000206414, 0.000206666, 0.000207184, 0.000207137, + 0.000207241, 0.000206859, 0.000207272, 0.00020718, 0.000207415, + 0.000206881, 0.000207671, 0.000207321, 0.000207528, 0.00020762, + 0.000207919, 0.000208007, 0.000208086, 0.000207944, 0.000208318, + 0.000207979, 0.000207908, 0.000208329, 0.000208802, 0.000208524, + 0.000208583, 0.000208374, 0.000208448, 0.000208445, 0.000209072, + 0.000209027, 0.000208642, 0.00020831, 0.000208948, 0.000209155, + 0.000209056, 0.000209099, 0.00020919, 0.000209363, 0.000209793, + 0.000209266, 0.000209357, 0.000209243, 0.000209401, 0.000209981, + 0.000210148, 0.000209922, 0.000209919, 0.000210436, 0.000210418, + 0.000210133, 0.000210512, 0.000210331, 0.000210774, 0.000210842, + 0.000210616, 0.000210566, 0.000211083, 0.00021111, 0.000210927, + 0.000211038, 0.000210986, 0.000211097, 0.000211374, 0.000211366, + 0.000211093, 0.00021141, 0.00021145, 0.000211462, 0.000211652, + 0.000211708, 0.000212047, 0.000211726, 0.000211804, 0.000212197, + 0.000212103, 0.000212139, 0.000212591, 0.000212525, 0.000212171, + 0.000212575, 0.000212867, 0.000213036, 0.000213095, 0.000213141, + 0.000212999, 0.000213001, 0.000212899, 0.000213322, 0.000212964, + 0.000213118, 0.000213111, 0.000213218, 0.000213268, 0.000213382, + 0.000213554, 0.000213368, 0.000213681, 0.000213389, 0.000213582, + 0.000213766, 0.00021404, 0.000213829, 0.000214348, 0.000214414, + 0.000214275, 0.000214353, 0.000214506, 0.000214483, 0.000214227, + 0.000214599, 0.000215184, 0.00021464, 0.000215007, 0.000214721, + 0.000214652, 0.000215345, 0.0002149, 0.000215168, 0.000215604, + 0.00021572, 0.000214934, 0.000215976, 0.00021586, 0.000215687, + 0.000216085, 0.000215481, 0.000215592, 0.000216069, 0.000216216, + 0.000216199, 0.000216717, 0.000216684, 0.000216517, 0.000216377, + 0.000216373, 0.000216484, 0.00021633, 0.000216643, 0.000216514}; + +const float SIMULATED_VNORD[GPS_DATA_SIZE] = { + 1.15585745, 2.647533222, 1.633673835, 2.440768363, -0.224579152, + 0.555666515, 0.305991203, 1.609186897, -0.988004271, 2.173881475, + 1.027173233, 2.038831934, 0.157196085, 1.491739423, 2.626959942, + 0.707236476, 0.165023409, 2.012414442, -0.268334118, -0.290011332, + 2.259172135, 1.618978071, 2.005067687, 2.334486131, 2.988992842, + 3.113279939, 3.672095799, 2.108945202, 4.283241154, 1.300542348, + 4.720951071, 2.081800207, 2.877127389, 6.674254624, 1.492449822, + 2.430972769, 1.761014537, 3.470831897, 4.763776693, 3.33372637, + 2.470452213, 1.928342135, 3.091402897, 4.007698156, 4.395192915, + 2.933121526, 2.070452492, 4.875346461, 1.611071282, 3.382661195, + 3.988769454, 2.117752108, 4.978095616, 5.178880433, 7.121581955, + 6.32689645, 4.261496552, 6.191930351, 3.542679238, 7.692246641, + 7.1409436, 7.038212109, 4.93506517, 8.554804769, 7.557956548, + 7.277787649, 6.819191612, 5.589444209, 4.214224696, 7.608443299, + 7.641846605, 7.875224607, 8.433248252, 7.04182257, 6.154524485, + 11.95796085, 10.14533385, 8.026891459, 10.26669566, 8.858089595, + 8.983353872, 10.89104647, 8.35536853, 8.38241596, 9.371915541, + 10.43285196, 11.60773717, 10.71407429, 12.63654397, 11.08923186, + 11.50781576, 12.47098857, 12.93866449, 11.80151878, 15.5563638, + 12.65029149, 14.44810845, 11.6510697, 14.461477, 15.76450435, + 14.19918536, 16.04263321, 16.47534173, 15.26166015, 17.49047395, + 14.15033867, 16.56990801, 16.3792729, 16.97776586, 16.9990111, + 19.58805361, 20.9056177, 18.3877234, 18.54591925, 20.29777508, + 23.76524555, 23.61457645, 21.17792138, 22.67055555, 20.74194287, + 23.11367136, 21.3255154, 20.10205358, 21.8817294, 22.70360344, + 23.45641554, 26.04735146, 21.99535836, 24.61412757, 22.73566029, + 24.93503656, 23.1813962, 24.28223407, 24.8466966, 26.6511174, + 26.59720529, 26.40348136, 27.15562764, 27.26100736, 26.59110911, + 27.30542375, 24.41607209, 28.60368352, 27.22431562, 28.02744339, + 29.42342397, 26.25759766, 28.17442339, 27.81422419, 29.63388113, + 29.69386067, 29.67150489, 30.02256501, 29.63519141, 30.24012858, + 34.27908155, 29.91127814, 34.20447844, 30.90724067, 30.12376771, + 32.20333768, 31.13130708, 33.97611373, 32.5779354, 36.52409547, + 35.02567693, 33.43619495, 34.81006352, 34.11995299, 35.78626168, + 32.28249578, 35.69005902, 33.5581562, 34.98717251, 34.58819228, + 34.4349169, 39.28707226, 38.03271686, 35.1414805, 36.39625394, + 35.70833086, 37.44807624, 37.03748635, 37.15166738, 35.83117728, + 40.04227332, 38.94890846, 39.60926355, 37.73149902, 39.47911602, + 36.4460395, 42.60299859, 40.39884414, 38.11947945, 40.53529207, + 38.93676839, 42.29785284, 42.96324452, 41.10141164, 45.63066766, + 43.21353524, 44.32496471, 43.7405277, 43.00101222, 45.07336498, + 43.90530672, 43.53767108, 44.76185389, 43.19098, 45.67561082, + 45.89343077, 45.87835599, 47.85125661, 47.42620914, 44.15008873, + 45.92901614, 46.01439111, 50.06218182, 48.70180614, 47.64184366, + 46.98759299, 48.95881914, 47.70906795, 47.977445, 49.41934701, + 49.63940159, 51.54129376, 51.76250824, 50.52679439, 49.90780586, + 52.77287047, 45.71987946, 50.35323213, 51.13573022, 52.85336431, + 50.62144186, 51.10940628, 53.30882757, 54.59536793, 55.48964014, + 52.97319184, 51.62643297, 52.68690051, 54.26775617, 53.20877221, + 53.95906549, 55.34828392, 56.76341234, 54.69423734, 56.48401697, + 57.29801772, 53.46352735, 56.5422492, 58.20345059, 57.12004164, + 59.30098567, 58.44107525, 56.25200596, 59.00594408, 59.50046022, + 57.29462657, 58.3524092, 58.52901546, 59.12330128, 57.68166733, + 61.6427304, 61.16233907, 61.53802678, 59.22141548, 60.16682856, + 62.53572909, 59.89328596, 61.34305639, 62.16506401, 63.42104904, + 61.94392353, 62.27035396, 63.34709837, 63.7702423, 63.13497201, + 63.23393317, 63.94294497, 63.96725013, 66.55061823, 66.61920594, + 66.83930943, 65.76591082, 65.42274617, 65.36775457, 64.80686847, + 66.63117461, 65.33436192, 64.42102413, 65.88624711, 66.43458292, + 66.30085275, 67.53340601, 66.80546729, 69.71956784, 68.16892852, + 67.99334308, 70.2310648, 67.01034184, 68.82338857, 69.99487743, + 69.86740044, 70.89019929, 71.5994174, 71.31321112, 67.69659066, + 70.18275553, 68.52444675, 69.83903237, 72.38846082, 73.01392244, + 72.44185492, 68.34762835, 71.40449505, 72.65702559, 72.35964244, + 71.83605999, 71.08736201, 73.21579639, 73.99435291, 72.36428626, + 75.64255727, 73.11395841, 74.59722057, 73.2591383, 74.36179845, + 73.54987464, 74.82481476, 73.62638979, 74.43589685, 75.29467993, + 73.82601023, 76.36820795, 74.03369948, 73.32047987, 74.39709283, + 74.50402097, 75.39340526, 73.54127518, 77.59747255, 74.38301479, + 76.13683827, 75.44100843, 74.98024237, 75.0735294, 77.94859909, + 73.19195139, 75.3954696, 74.97800095, 75.83556289, 76.05372928, + 73.08056813, 75.53638265, 74.15750034, 73.26781844, 75.74929151, + 74.76864666, 76.02782123, 76.18566877, 76.50625818, 74.75802764, + 77.13307356, 77.71482307, 75.73933853, 75.95401388, 75.24060415, + 75.81747124, 77.18974985, 76.70753047, 76.02080601, 77.42603557, + 76.13531751, 76.83810738, 73.9633067, 76.99510161, 77.29847935, + 77.78203857, 79.04655076, 77.56202805, 78.7377561, 77.35072352, + 76.5717882, 76.15741685, 77.55836151, 77.3516651, 76.26932943, + 77.43257092, 74.73226488, 77.24084866, 75.37298761, 79.75721558, + 77.54871751, 79.12615084, 78.29046938, 78.49329988, 78.25273212, + 76.65027842, 74.76613811, 77.07232587, 78.05849965, 80.12515372, + 75.93682613, 74.74828104, 75.67215363, 76.7351655, 78.39950277, + 78.34962537, 76.13441469, 77.28327022, 76.55483696, 75.38986469, + 77.78060645, 78.07637543, 75.94155638, 76.74610275, 75.4909619, + 77.32160076, 77.37450362, 77.25553425, 77.16446042, 77.60310542, + 76.68814451, 76.77786689, 74.77562702, 75.78699034, 75.34429875, + 76.8683999, 79.549265, 78.54301552, 79.38982041, 74.78362825, + 75.0779789, 75.82356974, 75.02429286, 78.49091782, 76.4932699, + 76.41659587, 73.83152331, 77.73677494, 75.56910047, 73.8839607, + 73.96573537, 74.69086915, 78.0137119, 75.28458625, 75.55138471, + 75.38130719, 75.22102212, 76.80128636, 76.83220998, 75.93115587, + 75.05687109, 74.95654951, 79.03163996, 75.89676925, 74.5272369, + 74.97973482, 73.64555241, 75.98528321, 75.18110421, 74.81253983, + 74.47907039, 75.51657341, 76.45153187, 75.82539348, 73.55867513, + 73.46464288, 76.44096717, 75.10450099, 72.95222878, 75.17468584, + 75.1068783, 74.95677152, 75.10449727, 75.70520186, 74.2803231, + 74.2443118, 73.68395474, 75.24109405, 74.2571342, 74.88153597, + 75.17068346, 75.79864853, 76.14970876, 74.90805888, 76.35649063, + 72.96335162, 74.80592276, 76.19134328, 72.90512979, 72.13825143, + 76.6120901, 73.61039096, 75.53510584, 73.15969612, 73.37091497, + 75.7222139, 76.17043437, 74.1615178, 76.57041232, 74.05204865, + 74.28777403, 73.33920095, 73.32087641, 74.80418857, 72.91587925, + 75.88530626, 71.92349202, 73.06891304, 73.59358493, 75.01424631, + 73.79324418, 73.90504796, 74.23715714, 72.76547829, 74.92804893, + 71.61463472, 74.59838429, 71.99078556, 72.03242216, 72.33034792, + 71.69663043, 73.99669254, 75.3526135, 71.72645885, 76.23799148, + 73.63978428, 73.2878603, 73.72544464, 73.61002546, 74.2741748, + 71.36826774, 72.49439287, 73.1690653, 72.70995669, 73.57717672, + 74.6790249, 72.79072302, 73.54586682, 71.13449559, 73.63791211, + 73.08465566, 70.8469633, 72.08980727, 73.69844312, 71.1524809, + 71.3383815, 74.57769191, 71.2581346, 71.87553852, 71.15980274, + 70.6091649, 71.55671494, 72.52578437, 69.86983284, 70.23822074, + 71.03121715, 71.43148232, 71.94143532, 69.6998805, 70.44945668, + 71.92178271, 71.36934355, 71.14850777, 72.14536498, 70.10278797, + 72.68736479, 70.44474159, 72.25641771, 72.48415874, 71.99337322, + 71.47806659, 73.80552756, 69.94625879, 69.85928401, 70.13856729, + 71.61738468, 69.32056042, 71.46582481, 70.45302537, 73.39158875, + 71.96381822, 69.58974158, 71.94068167, 67.52300928, 68.85319126, + 71.06770095, 69.70757978, 71.52010128, 71.77937028, 69.83472779, + 69.79899317, 70.23177589, 69.61458741, 71.18053482, 70.29947675, + 70.21690922, 71.12242153, 70.57951463, 71.35529828, 70.22823771, + 70.21971418, 70.47337087, 69.73845951, 70.71907322, 69.91173287, + 69.68827517, 69.91716806, 69.34883153, 74.1904669, 68.70172436, + 70.70605023, 71.49519684, 69.90130057, 71.91092983, 69.8793647, + 71.00553175, 71.38746345, 70.31246361, 69.58918228, 71.71143929, + 72.0009619, 67.96677776, 67.67593883, 70.64376999, 69.06632908, + 71.35338002, 69.20444953, 68.47196482, 68.34393504, 66.6882325, + 68.48333252, 68.75238101, 70.49675295, 71.30019954, 71.66991249, + 70.43493326, 70.40101285, 68.77405068, 69.30526061, 69.0513704, + 67.76555772, 70.71581855, 69.81049261, 67.80866213, 71.30545498, + 67.1656762, 69.98907962, 70.39518186, 68.74593009, 68.61020089, + 71.03167902, 68.29810648, 69.72544358, 70.48229533, 70.50340765, + 66.89961777, 71.91552777, 68.74188531, 67.65350683, 68.74043104, + 69.48534305, 67.79288437, 68.27904704, 71.48170455, 69.39566133, + 67.24983107, 70.955554, 68.98462001, 70.43415005, 70.78106405, + 65.67736491, 68.26464645, 68.39726166, 67.87329749, 67.60126761, + 68.35444885, 68.27567876, 68.40880048, 66.73935043, 66.4997693, + 67.80348808, 70.2578506, 67.80705791, 66.89611794, 67.58021201, + 68.69587304, 69.64697624, 68.42934325, 67.90204001, 69.41140094, + 69.0409256, 68.28583182, 66.78909138, 68.53360522, 67.87303666, + 68.48964377, 69.36127295, 70.69842318, 67.64891864, 64.86510759, + 68.15832758, 63.93142639, 68.46825255, 67.69202514, 64.03453377, + 68.26491449, 66.49058202, 63.93651866, 67.74016404, 66.65459079, + 66.82566665, 66.83837435, 68.03435903, 69.21212657, 67.0096958, + 68.51664412, 68.42363656, 67.61130003, 68.57499402, 69.2705466, + 68.97381037, 66.388133, 67.30990653, 66.43902007, 66.70160026, + 66.6584105, 68.36695994, 66.81643564, 66.00805942, 69.03420169, + 67.61190815, 66.13504711, 65.59244565, 66.32730322, 67.02387866, + 68.05816053, 66.04719432, 70.70224595, 66.55623571, 64.49063272, + 65.51351211, 66.9698342, 65.30847356, 66.02919264, 65.53104404, + 67.29083353, 65.26513791, 69.13561518, 65.27148741, 66.72396203, + 67.91487347, 65.04060386, 66.30319173, 65.29757218, 65.66148828, + 66.86795257, 64.71526606, 67.68715978, 68.06554605, 65.62695667, + 68.08595355, 67.95374093, 66.22642701, 62.85735427, 66.63094358, + 67.90641896, 67.2683348, 65.35766248, 64.51605953, 67.49407092, + 65.54383737, 66.53007655, 65.59524148, 64.63515241, 64.4544024, + 67.59166192, 63.68823632, 62.7394781, 68.5115317, 67.3426382, + 64.6512592, 67.67670823, 66.41807768, 65.54423298, 64.82603184, + 65.95292973, 65.82737648, 65.74635622, 64.82393295, 64.91427946, + 65.27702955, 64.33362896, 64.61094286, 63.83004288, 64.25235067, + 64.09787119, 65.36913656, 65.95287661, 65.90461838, 63.18024737, + 64.29430188, 65.89729286, 65.13439702, 66.75227486, 63.93682337, + 65.10021209, 66.72139419, 65.7911315, 65.67928855, 65.4620239, + 62.4203603, 65.11585701, 65.98333437, 64.7610827, 63.97985941, + 65.16900877, 66.63215907, 66.25084848, 65.93801275, 64.01565354, + 65.27610374, 63.04634605, 63.57519467, 65.98679996, 65.34232711, + 64.06975385, 63.91963796, 63.48190118, 64.68797288, 66.07107703, + 64.1556208, 62.31266691, 63.45781723, 64.16002317, 62.76733885, + 62.20682292, 64.82997138, 64.79508798, 61.08693693, 63.94586797, + 62.59127303, 62.02119275, 64.7105879, 61.83776901, 62.81968213, + 61.95383325, 62.86339116, 64.63454762, 64.91542686, 63.15528884, + 64.67456734, 64.75712045, 65.17399974, 65.07708752, 63.79215694, + 63.88159111, 64.32922595, 62.42247112, 62.58907834, 62.12561669, + 63.47771625, 64.01002369, 64.59800032, 65.61309425, 63.91514067, + 62.35287707, 63.27415876, 64.3260363, 64.83443866, 65.61973504, + 62.87014986, 61.47404704, 62.67269238, 64.52928061, 63.85010161, + 62.06893351, 62.43313519, 60.62381769, 60.42649972, 64.72509125, + 63.1748496, 63.94057892, 60.99701945, 63.57616695, 62.91335257, + 62.71965775, 61.72759875, 64.44319618, 65.17783264, 65.31842783, + 60.97953355, 62.35646669, 63.29296135, 63.61372004, 59.4992666, + 59.60062782, 63.67118577, 62.982435, 65.66206276, 61.66981619, + 62.2959332, 64.73975497, 61.34257779, 62.34753218, 63.31225531, + 63.6468068, 62.23660119, 62.29632785, 63.49894026, 62.2499156, + 61.41528544, 64.09107752, 59.62302367, 63.40271774, 61.93124684, + 61.96989167, 61.07952588, 59.46912436, 59.3287135, 62.58440822, + 63.14269719, 62.70728198, 61.81239616, 60.83673511, 64.79089048, + 62.60743261, 63.28169151, 60.2249967, 62.7995922, 63.52389421, + 63.68725819, 62.709139, 61.03840652, 62.90977741, 63.46411083, + 63.67368164, 63.38042352, 59.91435247, 64.98330217, 61.64182526, + 63.73147585, 62.24602574, 58.87122196, 60.85683956, 60.95715128, + 60.70062944, 63.96648758, 61.81679795, 61.89719074, 61.5008703, + 61.8969134, 61.08673538, 58.89519933, 62.02894101, 58.06230773, + 59.07723551, 62.32949987, 60.6895586, 63.67079867, 60.93568117, + 60.78516237, 62.06076399, 63.01556564, 60.0589714, 63.66114314, + 63.40526453, 64.5747104, 61.09988043, 62.35403704, 61.50936419, + 61.77290478, 63.08813599, 61.77644321, 62.11665883, 62.38608126, + 61.28488651, 58.58354395, 61.31326009, 63.20482456, 62.90451935, + 59.03300234, 60.24419107, 63.38769542, 60.56071143, 60.482896, + 61.02168856, 60.79869185, 65.30070646, 62.1810499, 60.82203362, + 60.96354579, 63.04537257, 60.13716998, 62.8293777, 62.32895002, + 62.98387389, 59.21121109, 59.35789336, 61.7064356, 59.03217666, + 62.08611069, 61.83041469, 61.1673883, 57.6514031, 61.66755141, + 57.79531899, 58.76236536, 61.1522658, 59.25180219, 60.04890468, + 59.0333338, 59.41941367, 60.91328133, 61.99773912, 59.59568798, + 59.45919094, 62.1051842, 57.89554123, 59.50358342, 60.09099722, + 60.92624691, 61.28761015, 60.76332899, 60.14557689, 58.51122248, + 60.44057738, 59.96549824, 59.87087273, 59.95132298, 59.48202396, + 61.33779783, 59.4914262, 59.50082544, 61.59434729, 61.64020497, + 59.85694188, 58.47766643, 58.33846183, 60.25473251, 62.55780949, + 62.57786434, 59.75254602, 61.35586922, 61.26170262, 58.05106014, + 61.13476804, 62.03419092, 60.30971491, 60.4204663, 58.47577204, + 58.39362031, 57.28299175, 62.13974138, 59.94935751, 60.08819871, + 57.93823578, 62.46128683, 61.94650719, 60.91270909, 58.69018622, + 58.60882423, 58.55903531, 58.58742118, 61.24094777, 59.5836443, + 59.80424023, 60.09597848, 59.1053668, 58.29026371, 59.53621322, + 58.38574331, 60.44497481, 59.30404694, 58.78324325, 59.51588447, + 60.16787498, 58.46656642, 59.07306472, 59.7195711, 58.8684741, + 58.24056049, 59.77918712, 59.48428973, 58.74112168, 57.71659181, + 56.45603349, 59.78996392, 57.84377943, 56.61567467, 57.48708959, + 58.42415221, 57.5204844, 61.93724399, 58.45617318, 59.94161156, + 62.05144769, 58.90958278, 60.24435515, 58.57918735, 59.77878279, + 57.4231158, 59.6407741, 58.80253621, 57.1883754, 60.35764808, + 58.77259766, 58.22478141, 58.99650737, 59.23191176, 61.13187064, + 58.68731995, 59.86685634, 58.44432886, 58.7628867, 60.4235064, + 57.40457042, 59.69968269, 57.01097459, 61.34352356, 59.16534188, + 58.3987454, 59.48910233, 57.81774599, 60.74114549, 57.42743864, + 57.87355333, 58.0695507, 58.71418979, 61.79645823, 59.69941574, + 57.82909004, 58.22340449, 59.81088908, 58.84501368, 58.62162432, + 59.74913176, 57.61771918, 59.3938011, 56.01509094, 57.98268279, + 59.14032382, 55.09875178, 58.18297823, 58.00268799, 57.30593085, + 57.56637532, 58.18386979, 56.18731239, 58.44955987, 60.68241139, + 56.91405476, 57.77932588, 59.68421971, 57.61160106, 61.11772639, + 59.91941609, 58.51719764, 55.49456926, 57.55388183, 58.75304689, + 56.07785778, 58.80628624, 57.52154544, 58.16462518, 55.73670039, + 59.65960473, 56.11444842, 58.51882447, 57.17127062, 59.45437159, + 60.7460867, 56.27048472, 56.06136871, 54.80469729, 57.04541557, + 57.29206032, 57.45584207, 55.667039, 56.24477948, 60.33494805, + 56.38989933, 57.17901563, 60.16774967, 56.59829213, 56.79840756, + 57.0273846, 57.00635182, 58.26153077, 59.02864088, 56.46336681, + 54.9311482, 54.44794028, 57.90735397, 59.06245751, 59.19846477, + 57.2586643, 56.82002103, 58.2498309, 60.25501626, 58.28337268, + 57.64086719, 58.30701542, 58.76272363, 57.58948387, 57.26245249, + 59.36561616, 56.65068795, 56.91977516, 58.12364737, 56.97463795, + 56.67248744, 57.88741241, 55.89422588, 58.29877005, 57.84199065, + 57.25926989, 57.03856426, 55.75822268, 56.83751614, 57.72205937, + 56.60577173, 54.2667815, 58.45274184, 57.97238117, 57.22844942, + 55.70059902, 53.52317101, 55.98202935, 55.32021937, 55.80706187, + 56.1481161, 57.30662913, 56.46884104, 56.63907212, 58.13362101, + 59.35158807, 56.08406545, 57.3063078, 57.6850355, 58.91762634, + 58.59708079, 56.54617969, 56.50973046, 56.44629826, 57.28071141, + 58.4167253, 55.5730407, 56.8496542, 57.14179833, 57.82461961, + 56.08745484, 57.97729638, 56.73080157, 55.59096547, 57.0605365, + 56.31713068, 56.23460339, 56.35656199, 56.31618108, 56.95960519, + 56.50709068, 56.33236645, 58.90554512, 56.55464285, 56.15753068, + 56.46102099, 56.89366726, 58.03248964, 54.84068847, 57.90492131, + 57.77709421, 53.37849399, 55.79066439, 54.7424831, 54.40327502, + 56.36917496, 55.7205906, 54.62733587, 55.27242724, 58.71689563, + 55.86027486, 57.12328952, 55.33737619, 54.25028262, 54.87313106, + 56.33447006, 56.94631128, 56.25214146, 55.39485128, 59.09698617, + 57.09947836, 56.41116974, 54.98497441, 56.78872075, 56.23337276, + 55.91383606, 55.17225838, 54.26693468, 57.70876036, 56.9804999, + 56.85652361, 57.03000629, 54.48491965, 54.83239185, 54.80594262, + 54.8219576, 57.55448093, 57.12988134, 59.10643787, 54.60246794, + 56.59846501, 57.79339191, 54.17306282, 56.29451228, 54.30043212, + 56.53920835, 54.72616204, 53.78750718, 54.94973804, 53.42229919, + 59.83067904, 54.31326276, 56.81144128, 56.68352834, 56.0481564, + 56.12240655, 55.07084268, 56.7094825, 54.56983612, 53.2788382, + 56.49558653, 55.03339006, 56.22968168, 55.15403225, 53.92132064, + 59.54531865, 52.98410921, 54.65958655, 56.62100495, 55.49697307, + 55.17151051, 53.77031307, 55.57765936, 53.63198003, 56.64795999, + 56.55572681, 56.0318899, 55.15342028, 57.58697409, 55.39896643, + 52.08235869, 52.43976384, 56.36572197, 55.20849221, 58.19254473, + 56.24224272, 55.27196942, 54.20645382, 54.96004685, 56.67502483, + 54.11653066, 54.10435174, 53.64389486, 56.46750262, 57.28397422, + 55.76208362, 54.29448003, 58.18078563, 55.40233682, 56.04404664, + 54.51877343, 54.44971519, 55.76001543, 53.5203576, 52.50958367, + 53.72655175, 53.94545728, 53.83211407, 52.44991489, 54.84983439, + 51.715566, 54.73290331, 54.25627596, 52.49409128, 54.2549528, + 52.1335208, 54.09211293, 55.9047755, 55.14037929, 53.20878345, + 56.15451316, 52.36379065, 51.68534819, 55.58212564, 55.2009634, + 55.95434575, 52.90978974, 54.08831841, 55.39296363, 53.45200507, + 56.50372549, 53.61458295, 54.78993596, 51.15624165, 53.74322805, + 55.48157757, 52.75259888, 54.54829714, 53.68102696, 53.9721637, + 56.85290296, 56.86143177, 54.10633483, 53.91030057, 53.83360755, + 54.26472009, 54.14483056, 56.54109897, 56.63492476, 54.24670983, + 55.01395861, 53.17388112, 56.64999286, 54.32878376, 53.3195045, + 51.01621461, 53.65254661, 52.97634043, 54.9678073, 53.04647754, + 53.86518485, 53.68890308, 52.97314776, 52.79469532, 54.99354852, + 55.73969594, 53.04711831, 52.97414634, 55.92721334, 55.56239478, + 56.37749467, 53.18475169, 52.81640664, 54.5887424, 55.29834146, + 51.55884652, 53.73215075, 52.43731494, 54.98259042, 55.94830859, + 52.95825746, 54.96438871, 54.98361916, 54.33590534, 53.17406902, + 52.82454506, 53.32913079, 52.0721292, 53.55374383, 54.71234203, + 52.61150156, 51.2211317, 53.62862954, 54.04352851, 52.01405375, + 56.39384616, 53.99892033, 53.27917992, 51.68955393, 53.81234085, + 53.41769273, 52.15436137, 52.58078658, 54.81327587, 51.9964659, + 56.8495859, 53.32568754, 51.27590625, 53.85325772, 51.05514768, + 54.09410325, 55.42830296, 52.25303911, 53.78944969, 52.93632671, + 50.46318079, 54.28256863, 54.9485591, 53.06498828, 51.33894239, + 52.97897802, 53.78248498, 53.85118601, 51.96829007, 52.79086763, + 54.56060686, 52.76721477, 51.91524224, 51.70803706, 52.43610776, + 54.05864338, 52.38570473, 53.1737549, 51.8004268, 53.44671068, + 50.6512485, 54.707901, 52.19234327, 53.09783876, 52.08664216, + 52.32017058, 52.13439656, 51.27736353, 55.28786727, 55.04092739, + 53.71142704, 53.52460067, 53.21010271, 53.12573388, 54.10377155, + 52.742695, 53.83846541, 52.68094029, 55.41788717, 52.08147991, + 52.49086918, 51.68232635, 51.68631683, 54.60943078, 53.14884065, + 51.53552774, 51.2201983, 53.75991158, 53.31126171, 52.12161275, + 52.118373, 50.5248803, 53.36849479, 54.65199275, 55.94962329, + 51.3790091, 52.16131094, 53.46365641, 54.36719747, 52.25449808, + 51.74828839, 54.55281623, 52.49680517, 53.3923283, 53.20552285, + 52.39606461, 53.81090869, 51.55467408, 54.36373577, 52.17164144, + 50.88749862, 49.87696111, 52.74010884, 49.84002041, 53.81016372, + 52.6133462, 50.24595676, 52.61735983, 56.03569255, 53.51176325, + 53.47080511, 52.61486606, 50.91610583, 53.3226644, 53.84154322, + 51.9499374, 53.39505441, 51.89959411, 53.27207463, 54.54451445, + 53.94357493, 52.48389145, 50.26436601, 51.58469616, 48.82941081, + 53.76515582, 54.11781397, 52.50936341, 53.7739929, 51.57633909, + 50.99952638, 51.9994075, 50.20495699, 49.88221686, 54.20791048, + 51.49757579, 53.24720179, 52.07932089, 51.70594323, 54.1870675, + 51.84684323, 51.76592761, 50.79137341, 52.31874039, 51.37465597, + 53.13589934, 53.07627339, 51.61524068, 54.31626504, 50.81287623, + 49.85730172, 53.1188765, 51.61211612, 52.61567511, 53.30671276, + 54.17355055, 52.82065509, 48.62882659, 47.94054626, 51.61919912, + 52.47067301, 50.18779171, 53.74378538, 51.08338206, 53.14909379, + 51.99321355, 51.04377831, 52.91436114, 53.7299382, 51.40650444, + 51.50432216, 48.43579872, 52.30460796, 53.27384103, 51.51955488, + 51.90471506, 51.61378538, 52.37333133, 55.20421989, 52.9777495, + 51.32038827, 52.80569656, 52.81560693, 50.66306396, 51.5472497, + 52.17055438, 53.40500556, 51.55221648, 54.35235976, 51.09918939, + 50.38828725, 49.43592493, 52.531144, 51.62608759, 52.45509605, + 53.76742791, 53.29656597, 49.78618613, 48.5203366, 52.2320687, + 51.2116226, 51.49246997, 51.52322008, 51.95067116, 49.50593681, + 52.68428264, 55.06196312, 49.69274319, 51.62852891, 51.76717319, + 51.04044188, 52.9822104, 54.88981451, 54.03821528, 49.61753843, + 52.59098926, 51.52476496, 53.4769185, 50.70400791, 51.5253717, + 52.06864626, 52.77743442, 51.19594424, 51.78063071, 50.48099678, + 53.44800893, 52.44462669, 48.12924828, 50.11543355, 51.98261463, + 51.09810543, 51.22573629, 51.00936891, 51.54463949, 52.60399837, + 50.99208685, 52.32099316, 51.33484121, 48.10862658, 52.19775507, + 50.62364952, 51.54374032, 50.93894146, 51.78941585, 50.3905159, + 50.73465709, 51.49848361, 51.1102004, 49.76645796, 50.19167866, + 51.69532709, 53.74283388, 50.8340685, 54.39165847, 53.27737095, + 49.15476391, 50.63974814, 50.48326611, 51.86201905, 50.13838621, + 50.92011548, 51.99634612, 50.31179079, 50.83592951, 55.12162165, + 51.74258393, 49.35575372, 52.19022018, 51.08720749, 52.0497061, + 50.12058384, 50.57826049, 50.30676951, 52.59815279, 49.40645192, + 50.51729151, 48.63107329, 52.68146863, 51.24905604, 51.60112559, + 51.60339927, 50.17375717, 51.78863909, 48.52189595, 50.29803665, + 51.52626243, 51.42677271, 51.31467334, 47.65225953, 50.78319287, + 48.87138898, 50.3981076, 49.2058683, 48.75368675, 51.86095856, + 48.61022329, 50.43953799, 48.45471394, 50.58783663, 53.38659709, + 49.36189437, 49.90967798, 51.20352523, 48.59521782, 49.82485719, + 50.89236537, 51.68528463, 50.81372603, 52.28870477, 49.52788925, + 49.77119353, 50.3376299, 50.76491807, 48.2970292, 50.99671595, + 51.55936764, 50.05950116, 48.85852124, 51.85266022, 51.5006031, + 49.76111716, 49.02337872, 50.25658777, 48.94450109, 51.22074824, + 49.41852146, 51.37234509, 52.74129401, 51.32241672, 50.30951169, + 52.3506678, 50.00237641, 50.46956439, 49.49679562, 52.02368409, + 52.00840758, 48.11574761, 51.56974458, 52.29982224, 49.64640702, + 51.75805136, 49.54765166, 49.96695634, 51.00619781, 47.9263993, + 48.59324828, 54.02031396, 51.85537934, 50.64181188, 52.38032751, + 49.07337457, 52.76331737, 50.78364762, 51.51359204, 48.29851345, + 50.45156191, 48.60250589, 47.49652071, 51.08858749, 50.37017836, + 48.96585564, 51.66668931, 50.43403636, 49.55074966, 51.17528373, + 48.646493, 50.29508404, 49.80925205, 49.68035976, 49.91670365, + 47.21912107, 50.93150271, 49.8462539, 51.47358518, 50.04053062, + 50.66848798, 50.96512812, 47.83424212, 51.074619, 50.9205553, + 48.68129483, 51.35786843, 48.32656463, 51.91123497, 49.5911589, + 49.71312591, 52.23435798, 49.54435529, 50.01496335, 49.95629841, + 49.90336451, 49.37411987, 51.67442667, 50.32949952, 50.48855263, + 49.68242482, 53.07603501, 45.99775939, 50.17714143, 50.78352834, + 48.36739597, 51.66583425, 47.56810045, 50.12963237, 47.70099105, + 51.32590012, 51.15861172, 51.70600683, 47.52344302, 48.96908595, + 49.8872012, 51.62510173, 48.61126644, 51.08746905, 51.65434643, + 49.75058731, 46.02035273, 47.47760714, 49.45573221, 49.84249778, + 49.14341689, 50.49799886, 48.31027414, 47.82737113, 51.23326584, + 49.05937476, 47.37579607, 48.28400861, 49.70046761, 51.13190256, + 49.35211177, 49.25452665, 48.22757306, 49.84350917, 48.37902535, + 50.25993233, 46.84149693, 48.52661618, 48.91242715, 50.6058431, + 48.02183812, 49.20977133, 50.12674573, 48.88122243, 49.30254354, + 50.24175522, 48.13594325, 49.89704255, 49.13698254, 46.14847664, + 50.9974204, 48.14950444, 47.73756921, 47.14284531, 49.66814215, + 48.63324316, 50.18253066, 48.82232156, 48.9528056, 51.12258884, + 46.97937413, 48.90859586, 48.078761, 50.87910313, 50.51298837, + 48.94736348, 50.46056681, 47.03734932, 49.88878016, 49.18173496, + 48.371876, 49.8570422, 50.3486331, 48.33283184, 50.89855763, + 48.43359649, 49.06556292, 47.82607215, 48.13477573, 47.26081906, + 48.79693584, 51.43157184, 49.38134665, 49.02504851, 51.47554705, + 48.94351266, 50.24940035, 47.21996907, 50.62074302, 48.71038689, + 49.61172928, 49.51119665, 46.86533315, 52.17898536, 46.41470216, + 48.83824506, 51.10080536, 47.68610786, 48.19002165, 53.25329176, + 47.15899348, 50.10955576, 50.85246046, 50.35858658, 47.53958299, + 50.00220699, 49.58345517, 49.38131622, 49.9552403, 51.67244513, + 49.39609772, 47.33083548, 49.60214437, 49.32387365, 49.33544588, + 47.96521476, 48.46712509, 49.29610904, 48.81633282, 50.29713922, + 50.05502429, 47.58283358, 47.87064155, 49.96921084, 47.84859141, + 46.92878661, 47.67439745, 49.27681919, 49.07294303, 48.22898062, + 50.61733466, 47.7189316, 48.19067194, 48.23381424, 48.38284268, + 48.5775388, 48.81268961, 50.26814271, 47.08296601, 50.48029145, + 47.4851254, 45.87236041, 48.02295976, 50.07151452, 48.46464226, + 50.36367153, 47.83584821, 49.10618446, 49.10265469, 50.4185086, + 47.15192772, 48.13237111, 48.33532715, 45.3153211, 49.23831397, + 49.31790475, 46.6383609, 48.76938039, 48.98999085, 48.93124383, + 48.90273597, 48.55262327, 48.31076794, 46.6916889, 51.59277358, + 47.8268692, 49.19577973, 48.26682164, 49.19152878, 47.44213378, + 48.47805185, 48.27639326, 48.9644763, 47.47787724, 50.29954586, + 48.26765234, 50.52738604, 51.1991314, 48.25261691, 47.60141451, + 48.54978419, 48.14680822, 50.89965802, 49.93528839, 46.2656863, + 50.68578916, 50.64060155, 49.1055874, 47.95383243, 51.49176531, + 47.02288199, 48.63848468, 49.80215084, 48.9937351, 46.49714686, + 48.21454999, 49.42652264, 46.97805169, 50.0695457, 52.37071327, + 46.44549979, 49.94083781, 49.92589961, 49.11896283, 49.19726675, + 48.8373052, 46.27375372, 50.07735452, 49.01043956, 48.831747, + 48.62724768, 48.88724168, 43.71057128, 47.52080541, 48.87730742, + 46.66953825, 48.03489947, 47.93013278, 46.93473556, 46.5612022, + 49.24567942, 48.11307283, 47.46535558, 49.69062771, 48.38576285, + 49.12204295, 45.22570023, 47.69942089, 47.90933048, 47.59743617, + 50.19240482, 48.47704663, 49.1101135, 49.64638339, 47.92403591, + 47.41180546, 50.52338424, 48.16587322, 46.67461557, 49.16335222, + 47.84441689, 49.03083145, 48.27285676, 46.24336362, 48.25389299, + 51.31752149, 46.74862513, 51.1693147, 47.8978836, 47.90709376, + 46.29908022, 46.17721173, 47.4089717, 48.31397761, 49.68277875, + 51.99437075, 47.24360887, 45.45215971, 46.2817328, 47.32813918, + 48.9271041, 48.42709031, 48.01701981, 48.84388508, 46.88849086, + 49.35684472, 46.182204, 46.6525444, 47.30963056, 48.20125466, + 48.73841812, 50.02372819, 45.85635522, 49.14544406, 49.90740559, + 46.08463023, 47.78581441, 46.54522004, 47.53810545, 50.17820039, + 47.71822933, 49.02277849, 46.30133758, 48.86444256, 46.00802067, + 45.83803388, 45.31529088, 46.22807795, 48.29491889, 47.40502217, + 47.02931806, 49.37463735, 46.29302506, 48.83639382, 47.04947176, + 48.89951064, 48.41738934, 46.05331967, 47.33507465, 48.96855365, + 48.78303395, 47.60737065, 46.90315461, 46.16867964, 47.99242064, + 49.6301018, 48.37438503, 46.81777148, 47.47262078, 47.69148055, + 45.02854792, 46.32347018, 46.85386698, 46.70799955, 46.51895617, + 47.44014772, 45.65866433, 50.17560801, 48.23812406, 46.38860977, + 47.50990316, 45.22491595, 46.78734552, 46.07455513, 47.32627594, + 46.52049433, 48.91388822, 49.37134994, 46.93988398, 48.08381196, + 46.99298396, 44.59855881, 47.31529203, 46.85967395, 48.88782424, + 45.55644483, 46.43875015, 45.06783561, 47.13336619, 47.39166498, + 48.95850534, 45.82770661, 47.78814395, 45.87340733, 47.79042687, + 47.20417906, 47.52862623, 46.87895752, 47.95924382, 45.62654178, + 45.84824009, 47.3693143, 48.29626289, 47.82453188, 46.22738151, + 45.75582566, 46.68768967, 46.52388609, 49.04599276, 49.3420453, + 49.62374966, 46.7286964, 45.82540313, 47.60506104, 47.35536499, + 47.1698668, 49.86558537, 44.15559291, 47.80788356, 49.13069025, + 47.39066889, 47.32058291, 49.38370913, 47.78814581, 47.72790779, + 47.37484336, 48.53174889, 46.89314994, 47.32849846, 48.18963226, + 49.34947666, 46.6823194, 44.55350791, 48.25812406, 47.75037012, + 45.9056424, 46.37197348, 47.32019127, 48.52970085, 48.84019016, + 47.42864679, 48.60401525, 48.4959618, 42.71099033, 47.99749963, + 47.11429438, 47.45238084, 46.68243998, 47.02397774, 46.73904732, + 46.28407128, 42.87367229, 47.2777852, 47.97150217, 47.47546872, + 47.82988241, 44.02934717, 46.00774567, 44.73794902, 47.42263441, + 46.89568035, 47.73856744, 46.69788575, 47.31625979, 47.36135648, + 45.42372243, 46.37245991, 47.36334758, 48.84048578, 45.55464241, + 46.10593582, 48.46040977, 46.57893333, 48.24397811, 46.26489521, + 47.44188555, 47.83101916, 48.15602988, 47.47870314, 47.69725973, + 46.03012023, 47.45267199, 45.84809532, 46.77537042, 47.81241082, + 46.5114458, 48.36007307, 46.65994596, 46.38590272, 49.53688092, + 46.08943171, 47.11794954, 47.62007326, 46.77363833, 46.46017129, + 47.78244837, 48.05027318, 47.12172928, 49.10122618, 44.74898731, + 44.13461826, 45.86859372, 47.02350571, 47.66420687, 46.23365158, + 48.97025813, 46.94072569, 48.78699892, 45.88260644, 45.17578008, + 48.32027428, 46.65464326, 45.42423998, 45.68884457, 45.11416846, + 50.64665161, 46.73820616, 44.60276907, 46.64439725, 47.19955709, + 46.36609819, 47.0413817, 45.12295717, 44.80186107, 49.00193353, + 47.976253, 46.14217462, 45.24139658, 46.96814406, 46.48947903, + 45.45483444, 47.37707199, 48.79510407, 44.74752298, 44.57291786, + 45.74759363, 45.94866229, 44.64113453, 45.43445381, 48.11458179, + 47.42690058, 46.9029463, 45.65942858, 46.85245684, 48.95101273, + 46.58683243, 45.84088048, 47.7773991, 46.39678091, 47.56579937, + 46.63719269, 51.15467375, 44.88160144, 49.2025143, 50.66840359, + 45.61101668, 47.66557041, 44.62406741, 47.26377156, 45.38735222, + 46.14266112, 47.64243355, 47.33878635, 46.74841959, 46.47681254, + 47.09399527, 46.68782735, 48.62113214, 46.48788694, 45.42268181, + 47.16763203, 44.70327301, 46.82300862, 47.13952842, 43.66012491, + 43.93676197, 47.40317503, 46.32725053, 46.49187467, 47.23056011, + 46.7613803, 49.95869996, 47.59461686, 49.63824186, 46.50441385, + 47.18015836, 44.3099505, 49.23870527, 44.58418426, 46.09863721, + 44.64833153, 46.74440667, 48.3018921, 46.80030809, 44.98428767, + 42.93431268, 47.82593913, 46.26167919, 48.0902586, 46.12866623, + 43.48032186, 47.33538622, 45.76988797, 46.29879358, 49.79043602, + 42.62085603, 45.71594682, 46.52186009, 47.13697014, 48.50388741, + 43.22042805, 44.93871623, 47.55933907, 47.78019495, 45.04956556, + 46.45838194, 48.29944958, 44.88860625, 46.54581886, 45.71152084, + 45.72296563, 46.49448033, 45.55039039, 46.22045458, 47.42782414, + 44.07255108, 48.26874715, 47.5204795, 47.1337446, 47.84326159, + 46.63224302, 46.61549044, 43.55608883, 45.84882753, 47.30500215, + 47.47601836, 45.20485351, 48.2945965, 46.80742963, 47.18170177, + 45.14698067, 44.95741067, 48.08841285, 45.60241605, 48.08675353, + 47.10104534, 46.27042907, 44.76196322, 44.85858866, 47.4266035, + 46.48647077, 46.7627719, 48.41688797, 47.01995636, 45.72807726, + 45.91887011, 48.45922398, 43.2301402, 47.05882433, 44.56947932, + 45.54738502, 46.0571001, 44.60466781, 49.27456269, 46.59190667, + 45.50003112, 44.504287, 42.57398237, 47.065903, 47.19546833, + 45.42504976, 47.87709923, 45.85797557, 47.2203195, 44.62287244, + 45.17060364, 44.9063807, 44.19356869, 47.52569177, 45.75253315, + 46.82864278, 44.91954823, 44.43950133, 46.66823264, 47.64431357, + 46.57875476, 46.47256268, 43.30172834, 45.09087795, 48.32023412, + 45.23973431, 47.09003625, 45.6177087, 48.20682562, 46.73706528, + 43.60833919, 47.07241511, 46.6706366, 46.03267592, 45.93587553, + 46.70148978, 49.20116114, 45.23934094, 46.75309841, 44.85915855, + 48.88880298, 44.9614572, 43.3868302, 45.40129593, 46.43889776, + 45.88336428, 45.1059271, 45.20395543, 44.82545692, 45.20751869, + 46.89540149, 42.94405937, 46.19252319, 46.53868137, 46.60512431, + 44.91742447, 46.9548162, 46.5482309, 45.40563964, 44.17918456, + 45.83888446, 47.29804539, 48.71969097, 43.83807917, 45.68657121, + 44.50284763, 45.13621865, 47.29664164, 43.58496476, 49.38687518, + 44.34423725, 46.59713179, 46.39881514, 46.89151227, 47.10282176, + 44.87918024, 43.04891339, 46.04773455, 45.49310318, 47.58866801, + 45.75686759, 47.89649419, 43.97431989, 46.94638184, 46.16877428, + 47.64785301, 45.89139306, 45.5507501, 45.48660009, 47.28411938, + 46.20095812, 45.93171489, 45.61300329, 43.95132512, 46.4593379}; + +const float SIMULATED_VEAST[GPS_DATA_SIZE] = { + 1.123925133, -1.120688682, 3.077857733, -1.97573061, -1.624232809, + -0.243835631, 0.604868232, -0.995493815, 0.74131936, -1.201807635, + -1.080352028, -0.848625068, -1.464741105, -0.803565782, -0.568754275, + 0.491804544, 2.147147298, -0.351163696, -0.222060087, 0.803585041, + 0.762989586, 2.330660768, 1.451482974, -0.240341058, -1.030122575, + 1.479906643, -2.308339787, 0.540638945, -1.259205664, -0.40142712, + 2.109759595, -0.341807264, -1.340056391, -0.762490265, -0.08466474, + 0.31631243, -1.249806725, 2.592886512, 0.593182867, 0.006891127, + 0.231485122, -2.530822571, -0.764450711, -2.35766534, -1.682172022, + 0.524918741, -0.871117864, 1.150666821, -1.983310449, -0.102733929, + -1.611250414, 1.11003025, 1.133274016, 0.192212178, -0.02447497, + 1.302048961, 1.760127826, 2.219839547, 1.146604176, 1.817096788, + 0.907950593, -1.699113212, 0.826883139, -0.430019737, 0.93261029, + 0.636172031, 0.204243267, 1.117425706, 0.731854873, 2.653820492, + -3.004174156, -0.084773495, 1.539299233, -0.739260309, -0.239592605, + 0.811554368, -1.675241808, 3.682571182, -1.090180536, 2.538447461, + 0.891217563, -0.720548638, -1.647123637, -1.418638042, 0.192431562, + 5.008987073, -0.483707112, -2.166579384, 2.339332231, 0.731724891, + -2.038842428, -0.560808188, -1.395576267, -0.672463363, -0.360154814, + -0.376780703, -0.824452193, 0.612824495, -2.13915436, 0.495738433, + 2.757305122, -0.963169877, 0.665738743, -2.241899989, -1.66327343, + -0.850889616, -0.246099374, 3.374650946, -1.284991136, -0.852165657, + 2.682144448, -0.735207943, -1.560123549, -1.11010547, 0.471665185, + -3.907441645, -0.992524383, 0.208134188, 1.248364079, 0.68021698, + -0.23844202, 0.498925016, -1.765530472, 0.797281356, -2.986063021, + 0.379582615, 0.132366784, -1.676147953, 1.599897131, 1.677280155, + -1.353206824, 3.892698525, -0.592720481, 1.619602166, -0.23232626, + 0.981976372, 0.331297644, 1.345503544, -0.700362291, -0.91253154, + -1.802800875, 0.323661298, 0.38721404, -0.32296837, 1.319951062, + -0.223409915, 1.114525166, -1.959925981, -0.133233418, 3.774554686, + 2.483916399, -0.159555899, -1.307826225, 1.268905046, -1.033859725, + -0.478664019, -1.44305206, -0.421515496, 0.284244701, -0.399355006, + -3.066429233, 0.062996776, 0.218999928, 1.959552241, -0.541084666, + -0.099679173, 0.628680116, -0.479385665, -1.354715944, -0.146433004, + 0.093946168, -0.06390901, 1.052413396, 1.968298014, 1.696012951, + -0.006671612, -1.841288495, -0.114286564, 0.575346371, -3.018347271, + 1.726893298, -1.608543002, 0.651820675, -0.599311177, -1.263546871, + -1.623935643, 1.413595168, 2.267897719, -1.112612345, -1.233317848, + -0.341568786, 1.336030287, -0.610093603, 1.62426153, -1.337912214, + -0.805964196, -0.953813731, 0.628590165, -1.206835177, -3.176868665, + -0.132373141, -0.004579647, 1.056617665, -1.561324964, 3.010497929, + -0.788169761, -0.042703416, 1.247037718, -0.978336561, -0.870887148, + 0.761751466, -2.029527982, 0.688403906, 1.606771084, -2.184468964, + -2.70154824, 2.150222559, -1.168684544, -0.870580755, -0.622226201, + -0.005271962, -1.305787183, -1.290702848, 0.965814133, 0.590940206, + -1.432834854, 1.971998036, -2.439830054, 1.428304645, 2.536641416, + -2.014655671, 0.40976155, 0.555717656, 0.399549594, 1.218619603, + -0.968256401, 2.076868069, -1.605360952, 0.294550963, -0.912458987, + 0.850484311, 2.01175084, 1.68982154, -0.096569938, -0.794252364, + -0.353232642, -0.757313016, -0.25156108, 2.654295711, 1.070192085, + 0.533094383, 1.432531405, -0.263281474, 0.570449426, -1.353791496, + 2.267686646, 0.257606369, -1.502040189, 0.369209174, -0.58327102, + -1.398750876, 0.17589707, 2.439454934, 2.33398271, -0.609801433, + 0.924947067, -1.327405469, 0.390345077, -2.233184938, -1.349370953, + -1.606178427, 0.566866939, 0.074954436, -1.671411752, 1.38804014, + -1.558586871, -0.852136058, 1.612909797, 1.640117049, 2.400075718, + 1.73825388, -2.293397308, -0.326435361, -1.807105589, 2.145946642, + 0.476526935, 0.423026367, -1.63883838, 0.005559238, -0.981641993, + -0.510066687, -0.629871704, 0.827877292, 0.995809113, -0.835897131, + 0.979927278, 0.5539637, -0.114651219, 1.769314461, 0.654822784, + 2.119309335, -0.483245068, 0.677621228, 0.762871088, 0.315761873, + 1.686426781, 1.681554196, 3.745268612, -0.042513933, -0.487579968, + 0.007881613, -2.351066252, 1.040764849, 1.354153666, -1.679230478, + 1.288142608, 3.304530587, 0.537471923, 0.878843113, 0.74047792, + 0.020563602, -2.48086971, -2.156691874, 1.654531667, -1.381700886, + -0.901112757, 0.821407141, -0.40894876, 0.156449172, 0.976583332, + 1.565190657, 0.041146375, -1.194159773, -0.636890941, -0.050882094, + -0.823680134, 2.583985676, -1.475096267, -0.497742483, -1.259989923, + -0.700364224, 0.909897812, -0.256345556, 1.881076329, 0.198843014, + -2.543632117, 0.282603017, -1.565964235, 0.540242433, -2.442810642, + 1.052655976, 0.855796713, -1.304813968, 2.049441175, -0.901334, + 0.650926792, 3.052402291, -0.134595519, 4.557092439, 0.340044355, + 0.504148406, 0.316137682, 2.471981227, -0.946784691, 1.134727822, + -1.052199523, -0.228289752, 2.720241779, -1.09233825, -0.147703414, + 0.607710877, 2.375894043, 2.792997879, -0.373082809, 2.692619087, + -1.248370123, 1.881366445, -0.698171853, 0.887125038, -0.998181409, + 0.879736718, 0.952582334, 0.838039217, -1.532832218, 1.293076076, + 0.216213255, 0.630645429, 0.964351044, 0.486550464, -0.40893838, + 0.704058796, 2.308576595, 1.745364273, 0.070156118, 1.376212892, + 1.403580591, 0.872611335, 2.475850228, -0.285277155, -3.375203644, + -0.439010814, 0.319099226, -0.605889823, 2.826890484, -0.498192648, + -0.11427501, -1.567864684, 2.599468457, -2.736439498, -0.455349938, + 1.136158044, 1.015355345, -0.1819495, -1.213311236, 0.906482084, + -0.631334048, 2.378749526, -1.547674301, 0.952674508, 1.752111562, + -0.305313789, -1.048184266, -1.394074807, 0.284413671, 1.599912886, + 1.623117751, -0.981474759, -0.550672409, -0.156453035, 0.682704427, + 0.067744249, -0.227488633, 1.459592959, 0.718391938, -3.520697093, + 1.026509808, 1.739419119, 0.466924469, -0.246135774, -0.26558037, + 0.504289225, -2.080141715, -0.382144179, 0.416433547, -1.19991614, + -1.058501986, 0.599342234, -0.742280703, 0.766842956, 1.868462358, + 1.06826954, 0.683594578, 1.539396212, 1.587681648, -0.457063646, + -0.499406581, 0.250472159, 2.633666891, 2.615310062, 0.924595709, + -1.40942825, -0.408417489, 2.225439799, -0.169201296, 2.054434775, + 1.994175433, 1.204409709, -2.160656836, 1.610625041, -0.545706574, + -0.496914054, -0.532906892, 2.32461998, -1.396479844, -1.433428652, + 2.067777341, -1.342353359, 2.541774562, -2.970596773, -2.354801324, + 1.359863621, -1.374497956, 1.62466443, 2.337483761, 0.359624203, + -1.057649003, 0.629078617, 0.306746741, 2.838632928, 1.848397869, + 0.385918115, 0.947797779, -0.852781344, -0.413483403, 4.209579997, + 2.124880184, -0.904679867, 0.195842495, 1.955357387, 1.448190984, + 1.580536482, 1.995192661, 1.623291868, -0.250025441, 0.882953557, + -1.04008737, -1.113430696, 2.012385177, 1.9848939, 0.720330636, + -0.073291214, 0.448748383, 1.287883174, -1.55272048, 1.683770932, + 2.476280878, 1.721055774, -1.500597776, 1.045158004, 0.205754678, + 2.021914418, -0.883572193, 1.201005953, 0.464065552, -0.879416867, + 0.459448793, 1.092206906, 0.62904761, 0.521218696, 1.619633249, + 1.266784107, -1.091717293, 0.197572129, -0.858095807, 1.821776965, + 0.327084324, 0.671747251, 1.354418259, 0.868807593, 3.757970303, + 0.382603982, 2.7155264, 1.244113893, 1.731844067, -0.804743109, + -1.089831474, 2.881109171, -0.233551021, 0.818685088, -2.357738669, + 2.89332174, 1.329017641, -2.183810283, 2.276912557, 0.897442733, + -0.491074652, 0.281183208, 0.458582956, 1.275034519, 0.345538206, + -0.114530969, 0.234870863, 0.673117863, -0.569001119, -0.027185051, + 1.444451293, -0.19551902, 1.485542978, 0.758941513, 2.211265275, + 0.519051529, -0.380528768, 2.384178234, -0.483936615, 2.232825747, + -0.453465462, -0.143803383, 2.002509985, 0.743340208, 0.117873524, + -0.742907139, 2.022066643, -1.004969366, 0.401323547, -2.653339188, + 0.017433264, -1.628695227, 1.540684438, -2.372450545, 0.349724954, + -3.801739542, 3.041271259, -0.819242516, 2.341596249, 0.449962259, + -2.156353165, -1.128436067, -0.305567237, 1.905003952, -0.223023949, + -0.973945462, 1.533210364, 0.046960593, -1.221497708, 0.786342537, + -0.450454991, -1.101489542, -1.700083038, -0.571914127, -0.592839139, + -0.959302184, 2.404372576, 1.152413825, 0.716993885, -1.73530736, + -1.366667038, 2.183531638, 0.111029784, 1.947492032, 1.591037477, + -2.08320759, 2.510154952, -1.865252263, 1.640625887, 0.778523879, + 1.596747249, 4.265666753, -0.773933648, -0.196789287, 1.098085217, + 0.903211325, 1.671128346, 1.557957056, 0.461027441, -0.495051796, + 0.390893179, 2.330250332, 0.171553868, -0.552222918, 1.607373581, + 1.641373528, -1.801295102, -1.238171385, 0.259752783, 1.738185943, + 0.927323989, 1.620667568, -1.733463344, -0.692715102, 1.175136392, + 0.200680761, 2.092231661, -1.814482877, 0.089461285, -1.182209334, + -0.027942983, -2.180687816, 0.779451197, 0.971713489, -3.236397044, + 0.580053004, 1.199200878, 2.01752181, 1.26400507, 2.713307027, + 1.540537168, -0.395101153, 0.763755701, 0.536524385, 1.048600249, + 0.162457145, -0.446961206, -0.010525594, -0.425524141, 2.832545509, + -1.169735189, -2.388113562, 0.91313736, 0.85833751, 0.444488164, + 2.590301262, 0.7528403, 0.40821949, 0.439925469, -1.313989786, + 0.656691861, -0.721659261, -1.711662432, -1.910479466, 0.628776373, + -0.649671431, 1.648903551, 2.04072536, 0.028256172, -1.728670379, + -1.006113804, 1.481389194, 1.798996369, -0.971189088, -2.02110358, + -0.742560866, -2.325365663, -0.569135145, -3.289952428, -0.89752459, + 2.243975879, -1.476407217, -1.140467288, -0.13433331, -0.577226016, + -0.866547422, -0.017672184, -1.017380726, 1.233805889, 1.307184003, + -1.310334455, -1.399679351, -1.144709677, -0.741616817, -0.620473253, + 0.446913226, -0.833832927, 1.909169248, 0.610277425, -1.495965604, + 2.253938895, 0.576886633, 3.432813653, 2.36483092, 1.853728966, + 0.854378135, 0.362114513, -1.435697164, -1.634454426, 1.008386064, + 0.799481574, -1.387059458, -0.692021241, 0.037467906, -0.709340056, + -0.280135259, -0.155187467, 0.249371939, 2.199769277, 1.691902384, + -0.416058505, 0.61560844, -1.436410387, 2.008539584, 2.544143055, + 0.529851323, -1.140906923, -0.591209136, 1.602469752, 2.771587908, + -0.756172103, 1.9658752, 1.386825924, -0.188944239, 1.84084632, + -1.149047963, -1.509359377, 2.715633108, 0.581051563, 0.707574237, + -2.006536861, 1.444919697, -3.19159248, -0.210402774, -0.865312849, + 0.839369875, -0.277539324, -1.577831793, -1.450301029, 0.51102979, + 3.039499234, 2.421107411, 1.05272487, 1.985062744, 0.416701688, + -2.138626124, -0.352304611, 0.77008402, 0.861958888, 1.812387162, + 2.012945327, 0.076608593, -0.286771798, -1.194613812, -0.952123561, + 0.222167598, 0.36407537, 0.669997978, -0.542146281, -0.326285013, + 0.842116284, -0.40789859, 0.501487819, 2.292870081, -1.595471139, + -1.434530815, 0.66235057, -0.497548247, -1.053051478, 1.001684093, + 3.116399367, 1.321067799, 1.588253458, 1.063354772, 1.830607934, + -1.31646588, 0.996233317, -0.13431018, 1.437859769, -2.744320043, + 3.02996071, 1.019639855, -0.859034916, 1.607730702, 0.199327806, + -0.593733131, -0.21645903, 1.575883713, 0.834035354, 2.272736206, + 0.643517562, 0.4333004, -0.41693513, 0.458183563, 1.003343549, + -0.90321986, 2.174933786, 0.839597802, -0.011354717, -1.557211144, + 0.18764625, 0.592608531, -0.955295621, 2.563267179, -2.688766231, + -0.338835259, -2.660866038, 2.138126698, 2.447938622, 1.853718315, + 1.44769605, 2.350090845, 2.122995314, -2.005671167, 0.173223808, + 0.022702781, 0.686049156, -1.413335359, 0.853796145, 0.413264431, + -0.682475257, -2.977961154, 3.307122428, -0.845409855, 1.069756317, + -1.51608332, -0.310163079, 0.602863063, 2.11540277, 1.900302931, + -0.330256401, -1.110319336, 0.269261347, -0.920847666, 2.414482244, + 4.805376066, -0.577482039, -0.056947263, -2.001917795, 0.119155967, + -0.47205224, 0.686414046, 1.872225186, 0.033384606, -1.806912943, + -0.007079513, 0.986837583, 0.509731588, -1.244768939, -0.214564888, + 3.367507072, 0.523367563, 2.006322392, 0.102059933, 1.278686201, + 0.098948297, 0.76056752, -1.109462893, -0.148037024, 1.002912808, + -1.824792222, 0.267142516, 3.113228914, 1.222834011, 0.175437375, + -1.854974019, 1.584990392, 1.14686631, 0.067193995, -0.019331696, + -0.18427938, 0.480956163, -0.315803402, 1.430525363, -0.594858497, + 1.412615444, -0.142429552, -1.550019885, 2.281793398, 2.383484067, + -1.229100874, -0.32105663, 2.84419721, 0.250579068, -1.177915978, + 0.200490245, 0.965732241, -0.541500851, 1.364462372, -0.645717628, + 0.678133902, -0.038582607, -1.216405105, 0.089920323, -0.238515973, + -0.789376995, -0.485312778, -1.377026505, -1.547735536, 0.073371865, + 0.008839298, -1.380370117, 1.252079681, 0.44539402, -1.564501365, + -0.012388971, 0.634724257, 1.022824146, -1.201086522, 0.533389699, + -0.769439627, 0.009543408, 3.109627473, 0.305617611, -1.055539048, + 1.925563005, 0.945593931, 0.911584031, 3.518608724, -0.678243209, + 0.637437813, -0.020296169, -2.405518172, 0.221560642, -2.060290873, + 2.844785617, 2.628139897, 0.846686057, -2.359092365, 0.199282562, + -0.38079297, 1.544117693, -0.023783432, 1.115552944, 0.678235144, + 0.731063323, 0.169845881, -0.34181282, -0.049160643, 0.469873703, + 1.336290888, 0.700942253, -2.44047181, 3.923963673, 1.171027921, + -2.103954526, 0.188902889, 1.483428355, -0.569223819, 0.680726555, + 1.503527713, -0.034392208, -0.195604247, -0.121002662, -1.909986842, + 0.180793046, 0.611906548, 2.477381784, 0.093561094, -2.162704367, + 0.679999017, 0.257088527, -2.029297735, 1.119056451, 0.083477466, + 3.363407883, 1.045975166, 1.491553909, 1.602136748, -0.094216367, + -1.556712777, -1.885536786, -2.759298981, 0.834808364, 1.871076003, + 2.808409381, -0.162975151, 1.615174642, 0.921825968, 2.888364098, + 0.235961318, -1.628865798, 1.545587101, 0.226612449, 1.270137874, + -1.569925137, -0.653925764, -1.774682385, 2.179149063, -0.983529965, + -0.029301711, 2.144270447, 1.566702312, -1.010491887, -0.851219571, + 0.849135255, 0.140053276, -0.003884728, 1.097611674, 0.869278922, + 2.238837305, 0.24465101, -0.20992344, -2.087410446, 0.266885181, + 0.915464279, 0.554165413, 1.62228536, 1.356218018, -1.668175494, + 2.305518793, 2.023943548, 1.009899154, 0.082677887, 1.29767767, + 0.61944694, 0.596659127, 0.237806751, -2.012317211, -1.618816039, + 1.801597027, 0.016660046, -0.583427335, -2.114943517, -1.347881675, + -2.885994509, -0.007004256, 1.623807924, 2.820585623, 1.35748284, + -2.624543765, 1.686777825, 0.708083581, -0.925952805, 1.850902442, + -0.614967883, -0.999406023, 0.533296873, -1.265418534, 1.009122853, + 0.683204851, 2.089819051, 0.724259613, -0.18594275, -0.613325895, + -3.45870145, 0.966181088, 1.717194883, 0.729864815, 3.008499754, + 1.228194282, 2.359845892, 0.940212028, -0.229548107, 1.632402175, + 2.209887243, 1.724582635, 0.769277426, -1.824048782, 0.007320447, + -0.582533919, -1.431635112, 1.235107671, -0.340820518, 1.265695392, + 1.986816896, 0.293241224, 1.236962056, 1.504130654, 2.388258902, + 0.564790498, -1.587556429, 2.783216032, 0.194935145, 0.424470985, + -0.54553692, 1.903584021, 2.32391633, -4.101877874, -1.21373924, + 0.063899649, -1.291012889, 2.524620851, -0.302191945, 2.481970074, + 1.144044823, 1.39193599, 2.112708426, 0.670169598, -0.765938533, + -0.791517007, -0.985290144, 3.820075311, -0.285770708, -1.51582182, + -0.998915403, 3.112780087, 1.842923285, 0.281852928, 1.274490217, + 0.053920865, -1.741479531, 1.286564254, 1.467623325, 0.527254309, + 2.314605709, 0.774966554, 0.875432356, -0.584788876, -0.095782347, + 1.548032481, 0.118550432, 1.331027452, 1.434627917, 2.63091738, + 0.526503647, 1.696594491, 1.207324686, 2.735659097, 0.885743563, + -1.327846221, 0.907681352, 2.182323763, 0.310932898, 0.648917955, + -1.322601889, 0.312228454, 1.103933545, -1.522846932, -0.616690377, + 0.627756229, 0.711281358, 0.700413742, 1.571916396, 1.229500822, + -1.857422729, -1.024613104, 0.428021907, 1.346762301, 0.793324472, + -0.414231445, 3.52352521, 0.717467573, 1.596509668, 0.595389715, + 1.195126441, 1.488087964, -0.759961733, 2.736437773, -0.052256774, + -0.30831256, -1.761745429, 0.775729409, 0.567398268, 0.868491332, + -0.320460361, -0.276052319, 0.802882844, -3.335469263, -2.366748986, + -1.050264965, -1.142877823, 2.333557578, -0.575255017, -1.776918191, + -0.249401631, 0.368228382, -0.646397252, 3.402838087, 0.800323383, + -0.447051815, 1.299119769, 0.197676406, -0.185570598, 0.165483161, + 1.134848285, 2.927159774, 1.153010932, -0.565297978, 1.920985442, + 3.145492917, 1.23640579, 1.955446758, -3.657266703, 1.935070276, + 1.092096281, 1.058721972, -0.029358872, 0.099383062, -0.835941561, + 2.593672885, 2.399392751, -0.449927151, 0.005546844, 0.198201187, + 1.172232453, 1.38797488, 0.661482384, -1.340632124, 0.480567569, + 1.790477977, 1.389858065, -1.673960997, 1.546369724, 0.152188723, + -1.097126477, 0.503022858, -0.448114363, -1.805615132, 0.060180725, + 0.800643307, 1.595802291, -0.75660658, 0.729096119, 1.301565667, + 1.062077279, -0.758823825, -0.135388761, -0.593216108, 0.428574209, + 0.066833057, -1.449294843, 0.628135845, -0.159418031, -0.55000601, + -0.285601006, 1.293826392, -2.114371094, 1.780373342, -1.631371817, + -2.703350057, -0.413514467, 0.189690975, -0.213151495, 1.656325236, + -0.05360659, 0.29300288, 2.172158185, 1.190813112, -0.652027261, + -2.40332705, 0.056930197, -1.836982512, -0.983402586, -0.761513057, + -0.358907652, -1.514462073, -0.347427505, 1.582562802, 3.452454681, + 2.456137107, 1.290356648, 0.123268415, 1.463220202, -0.17497932, + 0.232963373, 0.338533459, 0.669513935, 0.566869227, 0.827777935, + 2.214083556, 1.075964218, -0.359119787, -0.36353274, -1.02061679, + 1.292514497, 1.325485942, 0.193749925, 1.287520944, -1.147978707, + -0.245547886, 0.976648973, -0.461379498, -0.197190217, 1.132271083, + 0.080652484, -0.547565877, -1.499904668, 0.326322692, -0.72791293, + -0.816125452, 2.562778287, -0.388261746, -3.034550974, -1.884864923, + 0.566230288, 0.286592052, -1.985685193, 1.858981685, -0.030772587, + 1.980432396, -0.251097443, -0.059747521, 0.763078314, 0.0755844, + 0.791849005, 2.510046149, 0.517859899, -0.059894442, 0.808724046, + -1.099168396, 2.199436528, -0.604409791, 1.210341835, 1.058134083, + 1.088447822, 0.914621791, 2.317251582, -2.0824251, -1.613300533, + 0.722042585, -1.152425875, -0.036792785, -0.283623294, 0.36507122, + 1.966509829, -0.743496402, -1.070938003, -0.011383237, -0.597942061, + 0.938354114, 1.779062866, -1.432832288, -1.869878411, 0.240525811, + -0.99129146, 3.072613118, 1.300675083, 1.125817174, 1.011275482, + 0.886601371, -0.516365725, 1.550037203, 2.116749051, 0.23292923, + 0.155581406, 1.12702405, 0.231372271, -2.712564001, 0.368089782, + -0.662278638, -0.305311658, -0.204281991, -0.260957222, 0.630725683, + -0.060135938, -0.495011161, -1.42217047, -0.457938779, 3.127249921, + 0.964094177, -0.317302827, -2.205146865, -0.917860139, -1.543324573, + -0.020722091, 0.009926673, -0.163746266, 0.922349701, -1.251024404, + 1.42511299, 1.396743423, 1.400223278, -0.494173719, 0.158901096, + 1.25924283, 0.589486012, -1.537176094, -0.520506819, 1.194485066, + 0.372706525, -1.581999101, 2.46838069, 0.489175552, -1.186059859, + 1.488111776, 3.76339336, -0.445596702, -0.655262797, 1.31334385, + 0.87751287, -1.362033008, 2.485032141, -0.559170821, 0.046956705, + 0.131427651, -1.542227192, 0.353027499, 2.268824741, 2.316870461, + 0.088155684, -1.443347715, -1.021856763, 1.488715885, -0.441663799, + -0.954756371, 0.793711033, -0.03696681, 1.440621863, 0.304196746, + -1.76452531, 2.015333733, -2.139067939, -0.857557924, 0.406930951, + 2.400895278, 0.452117178, 0.121162137, 2.610249121, 0.300008888, + -0.604066235, -1.350652178, -1.687136006, -0.490258913, 1.458027209, + 5.289107198, -1.466948299, 0.412679753, 1.366175908, 2.172152359, + -0.538083145, 1.499054293, -1.451139797, 1.088292188, -0.093435674, + 0.759701754, 1.265919892, 1.663072618, 0.369630909, -1.002785698, + -1.350731781, -0.623074078, -0.493552034, 0.896680234, -2.147363324, + -0.49282741, 0.878126982, -1.382176753, 2.245369757, -0.725861534, + 1.70136147, 1.853109759, -1.279124706, -2.371670711, -0.578016305, + 2.392039938, 0.005963482, 1.409651627, 1.396533131, -1.808486231, + 0.766287797, 1.788877445, 1.445236548, -2.31146934, -3.316750241, + 0.167144782, 1.911461741, 0.341867179, -0.692709115, 0.469708286, + -0.328149129, 2.675635074, -2.448583386, -0.942981478, 0.420531871, + 0.0472463, 2.907244524, -0.582013031, -1.064828526, -0.84774717, + 1.979902183, 0.334935492, 1.257289405, -0.0905649, -0.76330356, + -0.826062174, -1.755858204, -1.061159054, -0.065143801, 0.401009003, + 3.503555358, 0.71511531, 1.825573341, -0.498099769, 0.754146445, + 0.905587029, 2.040290295, -1.981109477, 0.166393107, -1.789263813, + -0.884534464, 0.871336884, 1.860719385, 0.84082732, 1.866276818, + 0.407819081, 0.797413425, 0.969168063, 1.660251486, -0.610636177, + -1.47224957, 1.880596608, -1.546600473, 0.367880654, 0.219952818, + 2.503304998, -0.033202459, -0.322816661, -0.521791642, -2.725154094, + -0.245783324, 2.075939597, -0.576293831, 0.754107847, -0.747277442, + 1.577479468, -2.686322933, 1.949315905, -2.487528423, -0.697518804, + -0.972004924, 1.233150558, 0.76701988, -0.891585286, -1.546511986, + -0.003607555, -0.582195818, -1.322817853, 2.397346969, 0.270528217, + 0.523634785, 2.209909651, 1.537891044, 0.244241427, -0.907216415, + -0.095672762, -0.634258841, 1.057223397, 0.8953283, 2.279877487, + -1.494937042, -1.947909399, -0.447272675, 1.968050323, -1.166421019, + 1.724625941, 0.813347047, -2.750457696, 2.415328374, -1.279157099, + -1.575171978, 0.459912017, 1.045666027, 1.580295193, -0.514686288, + -0.744782283, 0.524319964, -0.213417921, -1.668887657, -3.947388306, + -0.194083981, 0.683288342, -1.831793001, 0.907471475, 0.483257502, + 0.95692032, -0.041081761, -0.630854, -1.074810279, -0.019258458, + 1.416719095, 3.417939932, -2.062220318, 0.430456877, 1.21766735, + -0.141318281, 0.505580878, 4.420514215, 1.024463712, 0.267547162, + -1.227936992, -0.991670056, -0.8936344, -0.091065641, 1.591750208, + 2.122530825, 0.666258014, -1.092880227, -0.204897047, 0.361693101, + 0.224188538, 0.57881251, 0.824471842, -0.312963607, 3.567410305, + -0.300219044, 2.099013534, 3.776727201, 0.973724414, -0.821893668, + 1.397723503, -2.165222485, 0.145957056, 1.561644871, 0.588455323, + 0.992526961, 0.018700155, -2.305011471, 1.188925805, 1.683880796, + 0.373947268, -0.120034566, -3.06551328, -0.826704597, 0.457881145, + 1.437263621, 2.484647434, -0.571774554, 1.843247667, 0.164353463, + 0.958227674, -0.156224487, 1.817900234, -0.002264615, -3.035174629, + -1.211970069, -0.520819679, 2.132070239, 0.923590125, 0.188409444, + -2.159797496, 1.216359868, 0.452391806, 3.243891632, -1.99609264, + 3.092169237, 1.139399734, 2.206082834, -0.132530667, 0.492112095, + -0.176906446, 1.978241958, 0.823919121, 2.729490431, 0.373026315, + -0.056335652, -1.468616493, 1.008203532, -0.293358319, 0.167804427, + 2.373939117, -1.450959143, 0.201844139, 0.84046227, 0.593609211, + 1.265362011, -0.385409769, 0.266458448, -0.515161552, -1.880138238, + -2.024077849, -1.299513099, 2.402613685, 2.222157657, -0.251500404, + -0.630069623, -0.628891903, -0.709660234, 0.851838803, -0.698597626, + 3.049287355, -1.6149106, -1.927081707, 1.918138555, -1.269452134, + -1.80420512, 1.141090727, 0.670000249, -1.91741656, 0.083464807, + -1.775744538, -0.297945026, 3.694692688, 2.084143517, 1.737330869, + 0.487444855, -0.60996227, 1.184610997, -2.14943899, -0.903895022, + 2.021254708, -0.059073692, -1.018550037, -1.557652553, 0.090863145, + 0.256465285, 0.727052019, -1.812877674, 0.969863002, -1.897692813, + -0.375852285, 0.22712039, -2.796802924, -0.821638024, -1.597260846, + 2.197220202, -0.159525394, 0.51668669, 0.147147491, -0.843365315, + -0.053503757, 1.363101478, 1.753133159, 1.754455246, -1.127151655, + 0.606370201, 1.082566604, 0.012637048, 0.211280462, -1.23692247, + 0.558462559, 0.070828279, 0.044307747, -0.259496689, -0.804888795, + 0.793374797, -2.419361894, 2.397771459, 0.813688551, 0.709392118, + 1.585978154, -0.714452416, 2.995573439, -0.648880185, 1.77432724, + 1.797405896, -1.812307803, -0.121980407, 1.070824747, 0.274422153, + 1.285574685, -0.219218111, 1.735338579, -0.226355349, -2.833132972, + 1.623264216, -0.429309569, -0.527687632, 2.04389432, -1.212411816, + -0.003291687, -1.475884332, -1.13999695, 2.607874853, 1.804612142, + 1.124069672, 1.927328928, 2.389264933, 1.362671732, -1.609573236, + -0.133528443, -0.138384005, 0.672561359, 1.312195519, -3.675499919, + -0.00216216, -1.183359036, -0.464207104, 0.691192147, 1.80666532, + 0.714666717, 0.405350211, -1.7621341, -0.846460527, 0.43822416, + 0.296008479, -2.215874128, -0.278254515, 1.218972292, 1.670299342, + -0.408137004, -0.285827372, -1.155245049, -0.955690706, 1.058421041, + 1.111706542, -1.701430466, 1.026677608, -0.175678155, -0.07494604, + -0.915953289, 0.63251776, -1.474537275, 2.853330932, 0.310785621, + 0.364663639, 0.848361991, -0.127431792, 0.416892679, -0.215719819, + 1.997654186, 1.35031807, -0.063042535, 0.727870745, -3.376893375, + 1.206018577, 0.515723828, -0.400636896, 1.453179958, 0.274872362, + 0.774340884, 1.648432495, 0.361709017, -0.609525767, 1.815449062, + 1.497443169, 1.230256027, 1.166972024, -0.14990035, -0.040401336, + -0.710003731, -1.416526773, -0.955993605, 1.65073934, 0.201491364, + -0.350695537, 2.615230539, -1.551784506, 0.375667912, 2.645946656, + 0.682155697, -2.894325873, -3.982720111, -0.419990401, 2.310531766, + -1.908981383, -2.09390227, -0.046004026, 0.868955889, 0.247102064, + -1.101351396, 1.225216123, 0.237778089, -1.402513632, 1.628312945, + 1.363855853, 1.317260285, 2.010694666, -0.42430637, 0.981780268, + 0.148340135, 0.289268163, 1.603728907, 0.345215102, 0.196336158, + 1.32727439, 1.265454825, -2.001170731, 0.399447087, -1.403738697, + -0.441049265, 0.747101963, 2.609234798, 2.508843239, 0.677854717, + 1.343707019, 0.062563543, -0.237599485, 3.364924592, -0.404672551, + 0.724344413, 0.715514569, 0.599782389, 0.010745858, 1.225901619, + 0.896168029, -0.409035438, 0.317583234, 0.226837627, 0.070142538, + 0.164437968, -1.110793885, 1.653966112, -1.379667164, -0.777256325, + -1.179518868, 1.39781246, 0.23600978, -1.048941178, 0.342508936, + -1.229353632, 0.549509167, -0.876597994, -1.266782932, 0.87277752, + 1.031138514, 1.54155358, 1.001400503, 0.283936255, -1.86546816, + -0.61605017, 0.481075388, -1.660543988, 1.751534339, 0.540899856, + 0.509294149, 1.78644306, -0.058001203, 0.245602149, 0.807037513, + 0.528749274, -0.587547022, 1.716926718, 2.301480201, 3.440973437, + -0.020516581, 0.928470415, -2.445302882, 1.717475321, 1.092803308, + 1.869049938, 3.014946231, -0.794056944, 2.01736103, 0.597613201, + 1.635747497, -0.225541041, -0.97828082, 0.742982621, 2.0546451, + 0.573111754, 2.169703639, -0.791468085, -0.306110546, -1.789587304, + 2.067655611, 1.504923309, -1.52286389, 1.695755641, -0.164157461, + -0.901043643, 0.264403752, -0.212823824, 1.539979262, 0.712952658, + 0.393523622, 1.136906817, 1.227850403, 0.103172014, -1.346179328, + 0.183656357, 0.437088043, 0.108778004, -0.481362792, 1.672347787, + 1.567111756, -0.620631196, 0.149633963, -2.06877202, 2.624683522, + -0.59513515, -3.495076176, -0.780071653, -0.10202848, 1.822852664, + -1.494294913, -0.405669511, -1.630174247, 0.057850587, 1.377075114, + 0.118763601, 1.115238847, -1.154151313, -2.151221389, -1.604743989, + 0.122237589, 1.700550553, 0.216717329, 0.367590526, 1.047867853, + -1.13339013, -0.030181953, -1.72867427, -0.611826217, -1.913771395, + -1.763564218, -0.161666547, -0.334339126, -0.763200547, 1.030559995, + 0.58798261, 1.093305059, -0.931104677, 0.907465064, 0.302792188, + -1.104880389, 2.671022486, -1.093517097, 2.147944675, 2.828718984, + -0.913522148, 3.432869063, 0.560067983, -1.401103015, 0.095025613, + 0.301602919, 2.092174969, 0.048264734, -1.079992248, -0.103264757, + 4.113418085, 1.146893845, -1.422481544, 0.93009919, -1.279897774, + 2.123972491, 0.563584813, 0.214626163, -1.273609865, -1.763108521, + -2.107955653, -1.415487513, 1.562198165, -1.113590919, -1.204285669, + 2.253840458, -0.008218922, -1.55096459, -0.422119434, 1.414220622, + -0.382153337, 2.787651495, 0.73184156, 0.245980326, 0.343996678, + -0.161954919, -1.221516539, 0.891469383, 0.452937192, -1.225358249, + 1.235552398, 0.446619284, 1.665574603, -0.893170049, 1.337596711, + 0.183757152, -0.61887269, 0.065192563, -2.123984533, -1.227810734, + 0.025162375, -0.651007732, 0.290569809, 0.854172515, -1.190188261, + 0.796943802, 0.792435549, -0.422167028, 0.024266268, 0.234692408, + 0.703284537, -0.214504255, -1.367520329, -1.612400537, -0.271011952, + 1.268275976, -1.176272185, 2.850113121, 4.009423465, 1.050897074, + 0.455787147, 1.186778102, 0.184213001, -0.000879537, -1.34014317, + 2.408974589, 0.269199612, -2.649608335, -2.516299847, -1.535931386, + 0.299984919, -2.264319552, 0.700768983, 0.556737982, 0.40325214, + -0.987952904, 0.996469322, 0.056938113, -1.48477869, 0.665648282, + 0.500494562, 0.71715095, -0.993503203, -1.279376596, -1.117481329, + -1.011410036, 0.474176803, 0.845707778, 1.001441284, 0.397955452, + -0.699286715, -1.687685557, -1.386879571, 2.27558231, 0.029092693, + -0.973558664, 0.91718233, 0.700856832, 2.550515426, -0.464792942, + -2.276326557, 2.201909379, -2.172840696, -1.219186837, 2.126802148, + 0.920387928, 1.474365161, -1.337279324, 0.522324677, -0.6292902, + 0.310686451, -1.262586009, 0.053683935, -1.775108966, 0.13443679, + 1.640400524, 0.752863736, 0.805356949, 0.453956969, 2.955811787, + 0.939734014, -0.898693403, -0.682942853, 0.522115493, 0.290743643, + -2.744873176, -0.650948914, -0.825822157, 0.036289286, 2.35851884, + -2.066822543, 1.706630528, 2.397176118, -1.830931368, -1.26049062, + 0.563661143, -1.206870531, 1.998078103, -0.511818947, -2.707018818, + -1.163787354, 0.483297455, 0.425049209, -0.835398937, -0.358465006, + 1.169932505, -2.894782496, 1.659873843, 1.588417846, 0.583777019, + 1.915019154, 2.343132919, 1.96863736, 0.745661389, 1.406556589, + -1.508826165, -1.142917171, 1.762597046, 0.56987792, 1.375523874, + -1.938998091, 0.40693206, 0.238203489, 0.976584803, 2.627005767, + -0.709392163, 1.123145034, 1.028645838, -0.001941209, -0.622606759, + 0.484872501, 0.774228962, -2.34688901, 0.999128756, 0.46127639, + -0.415641465, -0.805967562, 4.03481652, -1.919844046, -0.116683876, + -0.175496456, -1.019130486, -0.990293967, 2.146444112, -1.292401591, + -0.840575448, 2.060081962, -1.788947083, 2.530081414, 3.230873458, + 0.160244772, -0.530951483, 0.72794049, -2.260466314, 0.652572945, + 0.318972501, 1.291042044, -0.07155115, -2.798517996, 4.095462522, + 1.619520692, -0.417640937, 0.008003157, 1.726111894, -0.816381331, + -0.104527854, -1.074706544, -1.124537415, -0.186036562, 0.473611018, + -4.162339414, -3.035651066, 1.422241431, 1.084445861, 0.364127369, + -3.160183506, 0.59928583, -2.441105329, 1.173114846, 2.333246387, + -0.387920297, -2.223338459, 1.619011811, 0.401000065, -0.381396115, + -2.280394319, -1.354751273, -0.48520788, 1.309242675, 0.7322256, + 0.142046401, 2.255346027, -3.184031724, -1.116550968, -1.310222827, + 0.704560642, -0.673482634, -0.092914183, -0.112176567, 1.049523962, + 0.584804148, -1.440454665, -0.164614946, -0.086822089, 0.336898011, + -0.077126214, -1.245431979, 2.401252597, 0.177680644, 0.437918889, + 1.902307217, 1.988220333, -1.362948647, -0.200077393, 3.590309097, + 0.858918798, -0.019623438, 0.498353046, -2.103343871, -0.000154881, + 1.557796982, -0.43546615, 2.1832457, 0.152692029, 0.595102768, + 1.79166552, 1.454043, -0.185638046, 0.589250605, -0.272087614, + 1.884411299, 0.012884378, 0.710016883, 2.485399061, -1.308911069, + 0.104209337, 0.033886585, 0.78454266, 2.369837945, -0.960717052, + 0.371822457, 2.248967927, 0.55992114, 2.237446929, -0.824086218, + 2.246105985, 3.17607526, 1.2454744, 0.283162021, -1.369623558, + 0.702482598, -0.937351797, 1.299665639, -0.640907521, -0.620065725, + 0.945243243, -1.037456257, -0.698398585, 0.078864991, 2.988747365, + -0.308089093, 1.017701787, -1.425564592, -0.23549295, -0.645751715, + -1.433667028, -1.072376149, 2.121220993, -0.579776331, -1.643443172, + 2.085982482, -1.923063201, -0.321205048, 0.507524295, -2.712482382, + -3.064074894, 0.339948091, -0.962430717, 0.507077451, 1.699442501, + 1.449384543, 0.725804724, 0.602611108, 0.469401636, 0.529803224, + 0.262635649, -3.610754294, 0.323158222, 0.029692017, -1.178334135, + -0.270242172, 0.898974098, -0.134509516, -2.357446925, 0.856909527, + 1.364070448, 0.558424506, -0.356538345, -0.753820362, -0.379182667, + -1.740609329, -0.058055642, 1.554925386, 1.418515953, 1.287007965, + 0.448858743, -1.687837774, 1.050009195, 0.522292321, 3.025384263, + -0.280388008, -0.79212641, 2.458800809, 1.313987701, 0.703108899, + -1.397646223, -1.582677315, 1.312790017, -1.486910204, 0.633946741, + -0.256881439, 2.067897303, 2.481722326, -0.84814557, 0.28732032, + -0.284214887, -0.041828072, -1.161012902, -1.70169471, -0.074733256, + 0.125735971, 1.739011015, -1.255194305, -1.058735728, -0.973651084, + 0.488282848, 0.934815237, 0.486467365, -1.565648596, 3.441606598, + 0.211683453, -2.283196984, 0.184270804, 0.417195623, -1.405034279, + -0.076054368, 1.584783224, -0.003135455, 1.409339729, -0.715932146, + -2.640649556, 1.525658051, 3.042343981, -1.373483909, 1.572633691, + 1.018089415, 3.971245117, 0.441086315, -0.062926057, 2.286728394, + 0.570473322, -0.681163756, 0.650330916, 0.922985516, -0.630953637, + -2.533212851, 2.592750505, -0.045107404, -1.392494488, -2.797720532, + 1.020783918, 1.671664788, -0.235019709, 0.744736002, -1.517601615, + 2.256849153, 2.090523962, 2.60398147, 0.378676667, -1.449549584, + 1.092139771, 2.899815325, -1.832637271, -2.776032219, 1.03135609, + -0.372958538, -1.090211998, 1.248494658, 0.58448001, 0.688642347, + -0.754263922, -2.885575712, 0.919086543, -3.841311531, -1.75178116, + 3.045766472, 2.589631709, 3.121029595, 2.073435349, 0.620360664, + -0.541385749, -0.042689635, -2.379938645, -0.057167837, -3.246115673, + 0.820910153, -0.384768879, 0.020306601, 1.186834386, 0.210583953, + 0.869384476, -0.768637368, 0.009513777, -0.891858299, 1.607001697, + 0.286264357, 0.132137018, -2.674998467, 0.257006206, 0.677868603, + 2.601310752, -0.144490105, 0.664659897, -2.085568319, 2.28760246, + 3.344528752, 1.729666589, 1.717457486, -0.125653334, 4.467710331, + 2.087064038, 0.367153221, -1.061759864, 0.390129963, 1.469903071, + 2.394681952, -1.149315042, 2.266347626, 1.030153451, -0.087997232, + -1.660320676, 0.202804671, 2.816287157, 3.632231121, 2.64266144, + 1.068537672, -0.168980476, 2.147994656, 0.34399136, 1.780024187, + 1.593336942, 2.155929444, 1.579955892, -1.241889337, 0.077217715, + 0.648339359, -1.057207702, 0.654047371, -1.405485163, 0.451083249, + 2.922196733, -1.161332255, 0.634733978, -0.052460799, 1.014153865, + 1.824880453, -0.049095127, -1.766937441, 0.9970873, -0.553968497, + 0.507521904, -0.976342078, 1.022960964, -1.879712418, 0.612033994, + -1.601029481, 3.605605168, 1.085561287, 1.791048571, 2.919575227, + -2.783826825, 0.746994973, 1.612013384, -2.114449766, 1.521916866}; + +const float ACCELEROMETER_DATA[MOTION_SENSOR_AXIS_NUM][IMU_DATA_SIZE] = { + {-9.7563F, 29.18325888, 68.1828354, 66.58149024, 69.71237136, + 75.1483278, 80.00262972, 84.37581, 88.12425024, 89.56043424, + 89.56043424, 90.93198996, 92.32508844, 93.72536784, 93.91925268, + 93.77563428, 93.61765404, 93.44531196, 93.39504552, 93.35914092, + 93.33759816, 93.30169356, 93.25142712, 93.19397976, 93.15807516, + 93.10780872, 93.15807516, 93.20116068, 93.30169356, 93.38068368, + 93.40222644, 93.48839748, 93.56020668, 93.6751014, 93.711006, + 93.81153888, 93.86180532, 93.9623382, 93.9982428, 94.070052, + 94.06287108, 94.1418612, 94.20648948, 94.24239408, 94.2854796, + 94.3931934, 94.37883156, 94.49372628, 94.50808812, 94.58707824, + 94.55117364, 94.59425916, 94.65170652, 94.68761112, 94.68761112, + 94.7163348, 94.73787756, 94.72351572, 94.63016376, 94.62298284, + 94.58707824, 94.58707824, 94.5009072, 94.42191708, 94.36446972, + 94.33574604, 94.20648948, 94.1059566, 94.04850924, 93.9264336, + 93.91925268, 93.87616716, 93.73972968, 93.64637772, 93.55302576, + 93.48839748, 93.35914092, 93.2442462, 93.16525608, 93.05036136, + 93.0647232, 92.9570094, 92.8852002, 92.84211468, 92.7774864, + 92.72003904, 92.67695352, 92.61950616, 92.61232524, 92.61950616, + 92.64822984, 92.64822984, 92.64104892, 92.69131536, 92.82057192, + 92.86365744, 92.91392388, 93.04318044, 93.10780872, 93.21552252, + 93.22988436, 93.41658828, 93.6032922, 93.7469106, 93.98388096, + 94.20648948, 94.45782168, 94.73069664, 94.94612424, 95.26208472, + 95.26208472, 95.5421406, 95.84373924, 96.17406156, 96.41103192, + 96.7269924, 96.97114368, 97.2655614, 97.53843636, 97.74668304, + 97.73950212, 97.93338696, 98.14881456, 98.31397572, 98.38578492, + 98.47195596, 98.53658424, 98.53658424, 98.53658424, 98.44323228, + 98.42168952, 98.36424216, 98.22780468, 98.0554626, 97.96211064, + 97.76104488, 97.60306464, 97.45944624, 97.301466, 97.09321932, + 97.10040024, 96.85624896, 96.71263056, 96.50438388, 96.37512732, + 96.16688064, 96.05916684, 95.9011866, 95.80065372, 95.6498544, + 95.66421624, 95.49187416, 95.36979852, 95.2908084, 95.21181828, + 95.1112854, 94.967667, 94.80250584, 94.67324928, 94.5368118, + 94.55117364, 94.35010788, 94.17058488, 94.0341474, 93.8187198, + 93.63201588, 93.3878646, 93.1365324, 92.98573308, 92.813391, + 92.7774864, 92.65541076, 92.60514432, 92.47588776, 92.43280224, + 92.36817396, 92.36099304, 92.37535488, 92.41125948, 92.454345, + 92.48306868, 92.53333512, 92.633868, 92.71285812, 92.813391, + 92.92828572, 93.05036136, 93.07908504, 93.12935148, 93.21552252, + 93.2442462, 93.22270344, 93.21552252, 93.12217056, 93.08626596, + 92.9570094, 92.79902916, 92.66259168, 92.48306868, 92.3107266, + 92.29636476, 92.15274636, 91.90141416, 91.67880564, 91.46337804, + 91.26231228, 91.01098008, 90.80991432, 90.66629592, 90.43650648, + 90.45804924, 90.19953612, 89.99847036, 89.81894736, 89.581977, + 89.33782572, 89.21575008, 88.99314156, 88.76335212, 88.54792452, + 88.59101004, 88.31813508, 88.09552656, 87.8585562, 87.607224, + 87.4636056, 87.26972076, 87.18354972, 87.02556948, 86.9250366, + 86.93939844, 86.86040832, 86.75269452, 86.69524716, 86.58753336, + 86.50854324, 86.40801036, 86.3146584, 86.25003012, 86.0633262, + 86.09204988, 85.96997424, 85.79763216, 85.65401376, 85.44576708, + 85.23033948, 85.10826384, 84.84975072, 84.6630468, 84.42607644, + 84.41889552, 84.15320148, 83.93059296, 83.65053708, 83.4063858, + 83.17659636, 82.87499772, 82.67393196, 82.40823792, 82.16408664, + 82.19281032, 81.91275444, 81.682965, 81.4316328, 81.26647164, + 81.0366822, 80.92178748, 80.72790264, 80.56274148, 80.41912308, + 80.38321848, 80.25396192, 80.14624812, 79.98826788, 79.80156396, + 79.67948832, 79.57895544, 79.40661336, 79.26299496, 79.14091932, + 79.11219564, 78.92549172, 78.73160688, 78.5592648, 78.1284096, + 77.733459, 77.36723208, 77.00818608, 76.56296904, 76.14647568, + 76.13211384, 75.74434416, 75.27040344, 74.88981468, 74.32970292, + 73.79113392, 73.23820308, 72.66372948, 72.11079864, 71.60813424, + 71.60813424, 71.091108, 70.552539, 70.07859828, 68.95119384, + 67.65144732, 66.3157962, 64.987326, 63.68757948, 62.3662902, + 62.39501388, 60.75776412, 59.19232356, 57.57661656, 55.9752714, + 54.3236598, 52.73667648, 51.22868328, 50.18744988, 49.17494016, + 49.17494016, 48.112164, 47.09965428, 46.02251628, 44.98128288, + 43.89696396, 42.87727332, 41.81449716, 40.75890192, 39.72484944, + 39.75357312, 38.58308316, 37.4125932, 36.263646, 35.10751788, + 33.90112332, 32.72345244, 31.53141972, 30.40401528, 29.24788716, + 29.23352532, 28.10612088, 26.9643546, 25.75796004, 24.57310824, + 23.45288472, 22.28239476, 21.10472388, 19.99168128, 18.8140104, + 18.76374396, 17.65070136, 16.48739232, 15.33844512, 14.11768872, + 12.97592244, 11.84133708, 10.6636662, 8.62428492, 8.03544948, + 8.0426304, 7.5040614, 7.01575884, 6.52745628, 6.03915372, + 5.48622288, 4.96919664, 4.49525592, 3.9854106, 3.41811792, + 3.47556528, 2.81492064, 2.18299968, 1.59416424, 0.95506236, + 0.33032232, -0.31596048, -0.88325316, -1.53671688, -2.154276, + -2.16145692, -2.78619696, -3.21705216, -3.50428896, -3.8417922, + -4.22238096, -4.55270328, -4.94047296, -5.27079528, -5.565213, + -5.59393668, -5.93862084, -6.29766684, -6.63517008, -6.95113056, + -7.30299564, -7.60459428, -7.96364028, -8.26523892, -8.66737044, + -8.71763688, -9.2633868, -8.95460724, -9.5506236, -10.09637352, + -10.67084712, -11.24532072, -11.848518, -12.4229916, -13.0692744, + -13.06209348, -13.63656708, -14.30439264, -14.98658004, -15.67594836, + -16.31505024, -17.01159948, -17.72251056, -18.36161244, -19.0653426, + -19.03661892, -19.51774056, -19.9629576, -20.38663188, -20.80312524, + -21.26988504, -21.67201656, -22.11005268, -22.55526972, -23.02202952, + -23.0148486, -23.0148486, -22.95022032, -22.93585848, -22.95740124, + -22.88559204, -22.87841112, -22.84250652, -22.799421, -22.77787824, + -22.799421, -22.73479272, -22.75633548, -22.74197364, -22.69888812, + -22.6917072, -22.63425984, -22.59835524, -22.59835524, -22.52654604, + -22.5480888, -22.5121842, -22.49064144, -22.45473684, -22.44755592, + -22.39728948, -22.37574672, -22.34702304, -22.31829936, -22.26803292, + -22.2967566, -22.260852, -22.28239476, -22.27521384, -22.21058556, + -22.1531382, -22.13159544, -22.12441452, -22.10287176, -22.1172336, + -22.11005268, -22.06696716, -21.99515796, -22.02388164, -21.96643428, + -21.94489152, -21.94489152, -21.88744416, -21.90898692, -21.85872048, + -21.85153956, -21.81563496, -21.78691128, -21.7581876, -21.75100668, + -21.70792116, -21.67201656, -21.66483564, -21.62893104, -21.38477976, + -21.3991416, -21.34887516, -21.3273324, -21.33451332, -21.26988504, + -21.26270412, -21.24116136, -21.19807584, -21.24116136, -21.16217124, + -21.19807584, -21.16217124, -21.09754296, -21.08318112, -21.08318112, + -21.04727652, -21.0400956, -21.01855284, -20.97546732, -20.9682864, + -20.98264824, -20.98982916, -20.93956272, -20.91083904, -20.88929628, + -20.8605726, -20.83902984, -20.83902984, -20.7887634, -20.77440156, + -20.77440156, -20.72413512, -20.7169542, -20.68823052, -20.6810496, + -20.65950684, -20.5733358, -20.58769764, -20.5733358, -20.53025028, + -20.58051672, -20.55179304, -20.50870752, -20.465622, -20.50870752, + -20.43689832, -20.4297174, -20.3938128, -20.3938128, -20.34354636, + -20.37227004, -20.32918452, -20.3220036, -20.29327992, -20.27173716, + -20.24301348, -20.26455624, -20.19274704, -20.17120428, -20.13529968, + -20.14966152, -20.1424806, -20.11375692, -20.07785232, -20.05630956, + -20.0706714, -19.98450036, -20.02040496, -19.9629576, -19.98450036, + -19.98450036, -19.9988622, -19.95577668, -19.91987208, -19.90551024, + -19.89832932, -19.86242472, -19.83370104, -19.84806288, -19.84088196, + -19.76907276, -19.79061552, -19.76189184, -19.74753, -19.73316816, + -19.75471092, -19.74753, -19.76189184, -19.69008264, -19.69008264, + -19.70444448, -19.66135896, -19.66853988, -19.61827344, -19.6039116, + -19.57518792, -19.58954976, -19.53928332, -19.55364516, -19.51055964, + -19.52492148, -19.4602932, -19.45311228, -19.45311228, -19.43875044, + -19.43875044, -19.40284584, -19.33103664, -19.3166748, -19.3166748, + -19.30949388, -19.32385572, -19.30231296, -19.26640836, -19.26640836, + -19.23050376, -19.19459916, -19.19459916, -19.16587548, -19.1730564, + -19.1730564, -19.14433272, -19.1371518, -19.08688536, -19.05816168, + -19.05098076, -19.05098076, -19.029438, -19.029438, -18.98635248, + -18.97917156, -19.01507616, -18.93608604, -18.93608604, -18.93608604, + -18.849915, -18.86427684, -18.83555316, -18.86427684, -18.80682948, + -18.85709592, -18.7781058, -18.79964856, -18.8140104, -18.78528672, + -18.76374396, -18.71347752, -18.69911568, -18.69193476, -18.64166832, + -18.64166832, -18.670392, -18.64884924, -18.61294464, -18.64166832, + -18.56985912, -18.51959268, -18.5267736, -18.50523084, -18.49804992, + -18.51241176, -18.48368808, -18.44778348, -18.490869, -18.4549644, + -18.40469796, -18.40469796, -18.40469796, -18.39751704, -18.36161244, + -18.34006968, -18.28262232, -18.31852692, -18.28262232, -18.2754414, + -18.28262232, -18.2754414, -18.29698416, -18.30416508, -18.25389864, + -18.26107956, -18.2395368, -18.26107956, -18.21799404, -18.23235588, + -18.2036322, -18.16054668, -18.14618484, -18.10309932, -18.12464208, + -18.11746116, -18.10309932, -18.08873748, -18.07437564, -18.03129012, + -18.03129012, -18.03847104, -18.00974736, -17.99538552, -17.95948092, + -17.97384276, -17.9523, -17.96666184, -17.93075724, -17.90921448, + -17.89485264, -17.90203356, -17.83740528, -17.8445862, -17.8086816, + -17.83740528, -17.83740528, -17.8086816, -17.772777, -17.75841516, + -17.70814872, -17.7009678, -17.68660596, -17.71532964, -17.65788228, + -17.69378688, -17.65788228, -17.62197768, -17.65070136, -17.60043492, + -17.61479676, -17.5573494, -17.58607308, -17.57889216, -17.51426388, + -17.52862572, -17.50708296, -17.52862572, -17.49272112, -17.50708296, + -17.47117836, -17.4496356, -17.44245468, -17.413731, -17.39218824, + -17.40655008, -17.39218824, -17.34910272, -17.35628364, -17.33474088, + -17.34910272, -17.34910272, -17.29883628, -17.3060172, -17.2701126, + -17.29883628, -17.25575076, -17.25575076, -17.234208, -17.234208, + -17.17676064, -17.19112248, -17.14085604, -17.1623988, -17.14803696, + -17.14803696, -17.11213236, -17.14085604, -17.07622776, -17.09777052, + -17.04750408, -17.01159948, -17.01159948, -16.99005672, -16.99005672, + -16.99723764, -16.97569488, -16.96851396, -16.99005672, -16.9110666, + -17.01159948, -17.00441856, -16.96851396, -16.9469712, -16.93260936, + -16.92542844, -16.89670476, -16.88952384, -16.88234292, -16.9110666, + -16.82489556, -16.86798108, -16.8392574, -16.81771464, -16.81771464, + -16.82489556, -16.78181004, -16.77462912, -16.73872452, -16.74590544, + -16.71718176, -16.68845808, -16.71718176, -16.68127716, -16.68127716, + -16.65255348, -16.68127716, -16.65255348, -16.61664888, -16.63101072, + -16.6238298, -16.60228704, -16.59510612, -16.53047784, -16.55920152, + -16.5520206, -16.50175416, -16.52329692, -16.47303048, -16.50175416, + -16.48739232, -16.46584956, -16.42276404, -16.42994496, -16.4443068, + -16.40122128, -16.39404036, -16.4084022, -16.39404036, -16.36531668, + -16.35095484, -16.36531668, -16.336593, -16.32941208, -16.28632656, + -16.31505024, -16.28632656, -16.23606012, -16.24324104, -16.25042196, + -16.2288792, -16.20733644, -16.21451736, -16.20733644, -16.1929746, + -16.18579368, -16.13552724, -16.16425092, -16.14988908, -16.13552724, + -16.10680356, -16.10680356, -16.07807988, -16.06371804, -16.05653712, + -16.0493562, -16.02063252, -16.02781344, -16.04217528, -16.00627068, + -15.98472792, -15.977547, -15.95600424, -15.92728056, -15.93446148, + -15.9416424, -15.97036608, -15.92009964, -15.89855688, -15.9057378, + -15.8698332, -15.8339286, -15.8339286, -15.84829044, -15.8339286, + -15.8339286, -15.85547136, -15.798024, -15.80520492, -15.78366216, + -15.77648124, -15.76930032, -15.73339572, -15.74057664, -15.71903388, + -15.74775756, -15.70467204, -15.7621194, -15.73339572, -15.73339572, + -15.73339572, -15.70467204, -15.71903388, -15.69749112, -15.63286284, + -15.68312928, -15.64722468, -15.66158652, -15.62568192, -15.58977732, + -15.62568192, -15.59695824, -15.58977732, -15.56823456, -15.63286284, + -15.55387272, -15.5466918, -15.55387272, -15.53232996, -15.5107872, + -15.51796812, -15.52514904, -15.49642536, -15.45333984, -15.42461616, + -15.45333984, -15.438978, -15.38871156, -15.39589248, -15.41743524, + -15.34562604, -15.37434972, -15.30972144, -15.33844512, -15.3312642, + -15.32408328, -15.30972144, -15.3312642, -15.28099776, -15.2953596, + -15.30254052, -15.24509316, -15.23073132, -15.259455, -15.20918856, + -15.23073132, -15.23073132, -15.20200764, -15.18046488, -15.20200764, + -15.17328396, -15.12301752, -15.1517412, -15.18046488, -15.13019844, + -15.14456028, -15.13019844, -15.12301752, -15.08711292, -15.05838924, + -15.05120832, -15.07275108, -15.05838924, -15.079932, -15.01530372, + -15.00094188, -15.01530372, -15.02966556, -14.98658004, -15.01530372, + -14.9722182, -14.99376096, -14.94349452, -14.96503728, -14.94349452, + -14.98658004, -14.91477084, -14.92195176, -14.87168532, -14.8645044, + -14.85732348, -14.85732348, -14.81423796, -14.84296164, -14.82141888, + -14.8285998, -14.80705704, -14.7926952, -14.76397152, -14.72806692, + -14.77833336, -14.7567906, -14.71370508, -14.72806692, -14.720886, + -14.69216232, -14.70652416, -14.67780048, -14.6849814, -14.67780048, + -14.66343864, -14.67780048, -14.65625772, -14.63471496, -14.59881036, + -14.62753404, -14.62035312, -14.62753404, -14.6131722, -14.55572484, + -14.59881036, -14.65625772, -14.6131722, -14.62035312, -14.59881036, + -14.57008668, -14.62035312, -14.56290576, -14.53418208, -14.54854392, + -14.55572484, -14.541363, -14.5054584, -14.51982024, -14.5054584, + -14.51263932, -14.49827748, -14.47673472, -14.49109656, -14.46237288, + -14.46237288, -14.44083012, -14.42646828, -14.41210644, -14.41210644, + -14.41210644, -14.4336492, -14.3977446, -14.3977446, -14.34747816, + -14.34747816, -14.36184, -14.31875448, -14.33311632, -14.31875448, + -14.31875448, -14.29721172, -14.31875448, -14.2541262, -14.2900308, + -14.2541262, -14.28284988, -14.2900308, -14.20385976, -14.2182216, + -14.23258344, -14.23976436, -14.24694528, -14.23976436, -14.182317, + -14.18949792, -14.1464124, -14.13923148, -14.16795516, -14.15359332, + -14.13923148, -14.13923148, -14.08896504, -14.09614596, -14.1105078, + -14.11768872, -14.06742228, -14.08178412, -14.1105078, -14.08178412, + -14.08896504, -14.05306044, -14.08896504, -14.08178412, -13.99561308, + -14.00997492, -14.01715584, -13.99561308, -13.94534664, -13.98125124, + -13.97407032, -14.002794, -13.9668894, -13.9309848, -13.88789928, + -13.95252756, -13.9309848, -13.92380388, -13.90944204, -13.8950802, + -13.88789928, -13.91662296, -13.85199468, -13.87353744, -13.8950802, + -13.81609008, -13.83763284, -13.823271, -13.79454732, -13.81609008, + -13.823271, -13.80890916, -13.80172824, -13.76582364, -13.7514618, + -13.80172824, -13.75864272, -13.73709996, -13.74428088, -13.7155572, + -13.70119536, -13.72273812, -13.6796526, -13.70837628, -13.7155572, + -13.70119536, -13.69401444, -13.62938616, -13.65810984, -13.67247168, + -13.65092892, -13.61502432, -13.62938616, -13.62220524, -13.65092892, + -13.65092892, -13.61502432, -13.60066248, -13.63656708, -13.65092892, + -13.60066248, -13.61502432, -13.61502432, -13.62220524, -13.55757696, + -13.56475788, -13.55039604, -13.56475788, -13.5360342, -13.52885328, + -13.54321512, -13.51449144, -13.5001296, -13.52885328, -13.49294868, + -13.50731052, -13.48576776, -13.47140592, -13.44268224, -13.464225, + -13.44268224, -13.44268224, -13.43550132, -13.41395856, -13.44268224, + -13.40677764, -13.42113948, -13.36369212, -13.40677764, -13.3565112, + -13.34933028, -13.34214936, -13.3565112, -13.34214936, -13.3565112, + -13.30624476, -13.32778752, -13.31342568, -13.27752108, -13.3206066, + -13.32778752, -13.30624476, -13.284702, -13.29906384, -13.2487974, + -13.25597832, -13.24161648, -13.22007372, -13.2487974, -13.24161648, + -13.22725464, -13.24161648, -13.19853096, -13.19135004, -13.20571188, + -13.23443556, -13.1769882, -13.18416912, -13.16980728, -13.16262636, + -13.19135004, -13.16980728, -13.16980728, -13.16262636, -13.15544544, + -13.11235992, -13.09799808, -13.09081716, -13.1410836, -13.09799808, + -13.09799808, -13.08363624, -13.09081716, -13.07645532, -13.05491256, + -13.09799808, -13.07645532, -13.05491256, -13.04055072, -12.99028428, + -13.04773164, -13.0333698, -13.0333698, -12.98310336, -13.01182704, + -12.9615606, -12.9974652, -12.96874152, -12.96874152, -12.95437968, + -12.94719876, -12.9615606, -12.94719876, -12.91129416, -12.94719876, + -12.94001784, -12.89693232, -12.87538956, -12.8538468, -12.87538956, + -12.82512312, -12.925656, -12.87538956, -12.84666588, -12.89693232, + -12.8538468, -12.83948496, -12.81076128, -12.82512312, -12.77485668, + -12.81076128, -12.78921852, -12.8179422, -12.78921852, -12.73177116, + -12.77485668, -12.76049484, -12.73895208, -12.746133, -12.75331392, + -12.746133, -12.73895208, -12.72459024, -12.7102284, -12.70304748, + -12.71740932, -12.68150472, -12.7102284, -12.71740932, -12.75331392, + -12.6743238, -12.70304748, -12.68868564, -12.65996196, -12.68868564, + -12.65996196, -12.68150472, -12.60969552, -12.6743238, -12.60969552, + -12.68150472, -12.58097184, -12.6025146, -12.59533368, -12.6025146, + -12.60969552, -12.57379092, -12.56661, -12.59533368, -12.55942908, + -12.57379092, -12.53788632, -12.5307054, -12.55224816, -12.55224816, + -12.4948008, -12.54506724, -12.50916264, -12.5307054, -12.48043896, + -12.4948008, -12.46607712, -12.43735344, -12.46607712, -12.43017252, + -12.48761988, -12.45171528, -12.45171528, -12.43017252, -12.43735344, + -12.43735344, -12.43017252, -12.4229916, -12.39426792, -12.35836332, + -12.40862976, -12.40144884, -12.41581068, -12.387087, -12.40862976, + -12.33682056, -12.3511824, -12.37990608, -12.35836332, -12.3511824, + -12.33682056, -12.34400148, -12.32963964, -12.33682056, -12.28655412, + -12.32245872, -12.29373504, -12.3152778, -12.30809688, -12.26501136, + -12.23628768, -12.25064952, -12.22910676, -12.26501136, -12.22910676, + -12.20038308, -12.22910676, -12.22910676, -12.21474492, -12.207564, + -12.17884032, -12.17884032, -12.1716594, -12.1716594, -12.1716594, + -12.15729756, -12.15729756, -12.15011664, -12.15729756, -12.14293572, + -12.12857388, -12.16447848, -12.15011664, -12.08548836, -12.11421204, + -12.0998502, -12.09266928, -12.0998502, -12.10703112, -12.0998502, + -12.08548836, -12.07112652, -12.07112652, -12.05676468, -12.05676468, + -12.05676468, -12.03522192, -12.04240284, -12.04958376, -12.04958376, + -12.028041, -11.99931732, -12.01367916, -11.99931732, -11.94905088, + -11.9562318, -11.94186996, -11.98495548, -11.98495548, -11.9562318, + -11.9562318, -11.91314628, -11.93468904, -11.93468904, -11.94186996, + -11.91314628, -11.91314628, -11.86287984, -11.89878444, -11.91314628, + -11.85569892, -11.87724168, -11.81979432, -11.82697524, -11.84133708, + -11.87006076, -11.87006076, -11.848518, -11.8126134, -11.79825156, + -11.8126134, -11.82697524, -11.82697524, -11.79825156, -11.79107064, + -11.8126134, -11.85569892, -11.848518, -11.78388972, -11.7767088, + -11.79107064, -11.78388972, -11.78388972, -11.76952788, -11.79107064, + -11.76234696, -11.7767088, -11.76234696, -11.72644236, -11.72644236, + -11.74798512, -11.72644236, -11.71926144, -11.7048996, -11.71208052, + -11.668995, -11.71208052, -11.71208052, -11.69771868, -11.71926144, + -11.67617592, -11.65463316, -11.65463316, -11.64027132, -11.64027132, + -11.61872856, -11.64745224, -11.65463316, -11.61154764, -11.58282396, + -11.5971858, -11.61872856, -11.56846212, -11.57564304, -11.59000488, + -11.57564304, -11.5971858, -11.51101476, -11.55410028, -11.54691936, + -11.5612812, -11.51101476, -11.5612812, -11.53973844, -11.5253766, + -11.55410028, -11.51819568, -11.53973844, -11.5253766, -11.51819568, + -11.51819568, -11.47511016, -11.489472, -11.46792924, -11.489472, + -11.47511016, -11.489472, -11.49665292, -11.49665292, -11.46074832, + -11.4535674, -11.46792924, -11.44638648, -11.44638648, -11.44638648, + -11.42484372, -11.44638648, -11.42484372, -11.4176628, -11.38893912, + -11.37457728, -11.36739636, -11.40330096, -11.42484372, -11.37457728, + -11.38893912, -11.3817582, -11.36739636, -11.35303452, -11.35303452, + -11.32431084, -11.31712992, -11.30276808, -11.30276808, -11.33149176, + -11.33867268, -11.29558716, -11.29558716, -11.28840624, -11.28122532, + -11.31712992, -11.28840624, -11.25250164, -11.24532072, -11.25968256, + -11.25968256, -11.24532072, -11.29558716, -11.2740444, -11.2022352, + -11.2022352, -11.22377796, -11.19505428, -11.18787336, -11.19505428, + -11.15914968, -11.17351152, -11.20941612, -11.17351152, -11.1663306, + -11.18787336, -11.18069244, -11.15196876, -11.15196876, -11.17351152, + -11.13760692, -11.13760692, -11.14478784, -11.130426, -11.14478784, + -11.13760692, -11.12324508, -11.130426, -11.130426, -11.06579772, + -11.08734048, -11.08734048, -11.07297864, -11.03707404, -11.0586168, + -11.07297864, -11.05143588, -11.05143588, -11.04425496, -11.04425496, + -11.07297864, -11.02989312, -11.06579772, -11.0227122, -11.0227122, + -11.00116944, -10.96526484, -10.99398852, -10.93654116, -10.96526484, + -11.00835036, -11.00835036, -10.9868076, -11.00835036, -10.99398852, + -11.00116944, -10.97962668, -10.950903, -10.92936024, -10.950903, + -10.9868076, -10.9149984, -10.96526484, -10.93654116, -10.950903, + -10.92217932, -10.94372208, -10.90063656, -10.8790938, -10.95808392, + -10.90781748, -10.93654116, -10.88627472, -10.85755104, -10.89345564, + -10.85755104, -10.86473196, -10.82882736, -10.8431892, -10.85037012, + -10.86473196, -10.85755104, -10.85037012, -10.82164644, -10.83600828, + -10.81446552, -10.75701816, -10.80010368, -10.77138, -10.80010368, + -10.76419908, -10.8072846, -10.76419908, -10.76419908, -10.78574184, + -10.76419908, -10.76419908, -10.75701816, -10.75701816, -10.72829448, + -10.72829448, -10.74265632, -10.74265632, -10.7354754, -10.72829448, + -10.6636662, -10.71393264, -10.68520896, -10.6995708, -10.67084712, + -10.68520896, -10.65648528, -10.6277616, -10.6636662, -10.6995708, + -10.67802804, -10.64212344, -10.6636662, -10.64930436, -10.64212344, + -10.62058068, -10.64212344, -10.57749516, -10.591857, -10.6277616, + -10.61339976, -10.57749516, -10.59903792, -10.61339976, -10.62058068, + -10.57749516, -10.57031424, -10.60621884, -10.54877148, -10.56313332, + -10.54877148, -10.5559524, -10.53440964, -10.52722872, -10.53440964, + -10.52722872, -10.5559524, -10.47696228, -10.4841432, -10.5200478, + -10.46260044, -10.49132412, -10.4482386, -10.46978136, -10.47696228, + -10.46260044, -10.42669584, -10.44105768, -10.45541952, -10.46260044, + -10.412334, -10.40515308, -10.44105768, -10.4482386, -10.39797216, + -10.40515308, -10.35488664, -10.39797216, -10.35488664, -10.41951492, + -10.3764294, -10.36206756, -10.34770572, -10.33334388, -10.38361032, + -10.3405248, -10.36206756, -10.33334388, -10.36924848, -10.31898204, + -10.29025836, -10.3405248, -10.33334388, -10.3046202, -10.35488664, + -10.29743928, -10.33334388, -10.32616296, -10.32616296, -10.28307744, + -10.29743928, -10.27589652, -10.29743928, -10.26153468, -10.29743928, + -10.2687156, -10.27589652, -10.23999192, -10.25435376, -10.1969064, + -10.23999192, -10.23999192, -10.21126824, -10.22563008, -10.21126824, + -10.23999192, -10.17536364, -10.17536364, -10.21126824, -10.18972548, + -10.18972548, -10.1969064, -10.16818272, -10.15382088, -10.1610018, + -10.1610018, -10.16818272, -10.1610018, -10.16818272, -10.1610018, + -10.16818272, -10.1250972, -10.11791628, -10.15382088, -10.14663996, + -10.10355444, -10.13945904, -10.13945904, -10.08201168, -10.14663996, + -10.0891926, -10.10355444, -10.08201168, -10.06764984, -10.053288, + -10.04610708, -10.04610708, -10.053288, -10.02456432, -9.9814788, + -10.01020248, -10.03892616, -10.0173834, -9.98865972, -10.0173834, + -9.99584064, -10.01020248, -10.0173834, -9.97429788, -10.0173834, + -9.99584064, -9.9814788, -9.99584064, -9.95275512, -9.95993604, + -9.97429788, -9.93121236, -9.95993604, -9.95275512, -9.95993604, + -9.9455742, -9.93121236, -9.89530776, -9.93839328, -9.91685052, + -9.90248868, -9.9096696, -9.85222224, -9.9096696, -9.85222224, + -9.88812684, -9.85940316, -9.83067948, -9.86658408, -9.84504132, + -9.88812684, -9.86658408, -9.83067948, -9.8378604, -9.80913672, + -9.83067948, -9.8378604, -9.8378604, -9.82349856, -9.78759396, + -9.82349856, -9.77323212, -9.78041304, -9.78041304, -9.77323212, + -9.7660512, -9.75168936, -9.75168936, -9.77323212, -9.74450844, + -9.75168936, -9.70860384, -9.7301466, -9.74450844, -9.71578476, + -9.72296568, -9.70142292, -9.75887028, -9.66551832, -9.694242, + -9.70860384, -9.68706108, -9.66551832, -9.70860384, -9.67269924, + -9.64397556, -9.65115648, -9.67988016, -9.62961372, -9.61525188, + -9.60807096, -9.60807096, -9.6224328, -9.63679464, -9.60089004, + -9.60089004, -9.61525188, -9.57216636, -9.55780452, -9.54344268, + -9.56498544, -9.54344268, -9.53626176, -9.57934728, -9.52908084, + -9.54344268, -9.5506236, -9.49317624, -9.54344268, -9.55780452, + -9.52189992, -9.53626176, -9.50035716, -9.47163348, -9.46445256, + -9.4429098, -9.46445256, -9.45009072, -9.47163348, -9.42136704, + -9.45727164, -9.43572888, -9.4429098, -9.4429098, -9.4429098, + -9.43572888, -9.45727164, -9.38546244, -9.41418612, -9.41418612, + -9.39264336, -9.39264336, -9.39982428, -9.37828152, -9.3711006, + -9.37828152, -9.3711006, -9.36391968, -9.36391968, -9.34955784, + -9.34955784, -9.34237692, -9.34955784, -9.29211048, -9.32083416, + -9.30647232, -9.24902496, -9.2992914, -9.2992914, -9.29211048, + -9.27056772, -9.27056772, -9.24902496, -9.24902496, -9.24184404, + -9.18439668, -9.24902496, -9.19875852, -9.24902496, -9.2274822, + -9.2274822, -9.23466312, -9.1915776, -9.19875852, -9.17721576, + -9.20593944, -9.16285392, -9.18439668, -9.1915776, -9.18439668, + -9.17003484, -9.16285392, -9.14849208, -9.155673, -9.12694932, + -9.12694932, -9.11258748, -9.12694932, -9.14131116, -9.12694932, + -9.10540656, -9.13413024, -9.1197684, -9.06232104, -9.09104472, + -9.0838638, -9.10540656, -9.07668288, -9.03359736, -9.06232104, + -9.07668288, -9.06950196, -9.02641644, -9.06232104, -8.99769276, + -9.0120546, -9.03359736, -9.01923552, -8.96896908, -8.96178816, + -8.99051184, -8.98333092, -8.95460724, -8.99051184, -8.96178816, + -8.99051184, -8.9402454, -8.96178816, -8.93306448, -8.91870264, + -8.9402454, -8.92588356, -8.91152172, -8.94742632, -8.91870264, + -8.88997896, -8.88997896, -8.8684362, -8.91152172, -8.86125528, + -8.88997896, -8.9043408, -8.87561712, -8.8325316, -8.84689344, + -8.83971252, -8.80380792, -8.81816976, -8.82535068, -8.82535068, + -8.80380792, -8.8325316, -8.7607224, -8.77508424, -8.796627, + -8.76790332, -8.77508424, -8.73199872, -8.73917964, -8.7248178, + -8.73199872, -8.73917964, -8.71763688, -8.69609412, -8.73917964, + -8.68173228, -8.70327504, -8.63146584, -8.69609412, -8.6889132, + -8.68173228, -8.6530086, -8.6530086, -8.6889132, -8.63146584, + -8.63146584, -8.617104, -8.63146584, -8.617104, -8.59556124, + -8.5811994, -8.5811994, -8.59556124, -8.56683756, -8.55965664, + -8.5452948, -8.55965664, -8.51657112, -8.5452948, -8.55247572, + -8.53093296, -8.5452948, -8.50220928, -8.48066652, -8.44476192, + -8.48784744, -8.48066652, -8.4734856, -8.4734856, -8.44476192, + -8.43040008, -8.48066652, -8.45194284, -8.45194284, -8.41603824, + -8.45194284, -8.40885732, -8.40885732, -8.40885732, -8.37295272, + -8.34422904, -8.38013364, -8.38013364, -8.30832444, -8.33704812, + -8.33704812, -8.31550536, -8.30832444, -8.32268628, -8.33704812, + -8.31550536, -8.30832444, -8.30832444, -8.258058, -8.258058, + -8.30114352, -8.23651524, -8.24369616, -8.23651524, -8.25087708, + -8.23651524, -8.258058, -8.25087708, -8.2221534, -8.19342972, + -8.20061064, -8.19342972, -8.19342972, -8.20061064, -8.1503442, + -8.14316328, -8.15752512, -8.1503442, -8.12880144, -8.12880144, + -8.09289684, -8.10007776, -8.10725868, -8.10007776, -8.05699224, + -8.05699224, -8.10007776, -8.05699224, -8.07135408, -8.02826856, + -8.03544948, -8.0426304, -8.04981132, -8.0067258, -7.99236396, + -7.99954488, -7.99954488, -7.9708212, -7.94927844, -7.92773568, + -7.94927844, -7.92773568, -7.94209752, -7.92773568, -7.9349166, + -7.9349166, -7.89183108, -7.89183108, -7.90619292, -7.84874556, + -7.89183108, -7.88465016, -7.84156464, -7.82002188, -7.82002188, + -7.83438372, -7.83438372, -7.76975544, -7.82002188, -7.84156464, + -7.81284096, -7.80566004, -7.76975544, -7.76257452, -7.7553936, + -7.7553936, -7.74821268, -7.74103176, -7.76257452, -7.7553936, + -7.719489, -7.73385084, -7.71230808, -7.66922256, -7.719489, + -7.67640348, -7.66922256, -7.70512716, -7.66204164, -7.64049888, + -7.64049888, -7.64049888, -7.5758706, -7.63331796, -7.61895612, + -7.58305152, -7.6117752, -7.59023244, -7.6117752, -7.58305152, + -7.56150876, -7.5758706, -7.55432784, -7.53278508, -7.51842324, + -7.53278508, -7.48251864, -7.49688048, -7.48251864, -7.48969956, + -7.4681568, -7.48251864, -7.44661404, -7.43943312, -7.43943312, + -7.41070944, -7.41789036, -7.41789036, -7.38916668, -7.41070944, + -7.36762392, -7.37480484, -7.37480484, -7.33890024, -7.34608116, + -7.34608116, -7.34608116, -7.31735748, -7.33171932, -7.29581472, + -7.2527292, -7.27427196, -7.23836736, -7.28145288, -7.24554828, + -7.22400552, -7.2527292, -7.18810092, -7.2527292, -7.20246276, + -7.19528184, -7.1450154, -7.15937724, -7.11629172, -7.1450154, + -7.15219632, -7.15219632, -7.08038712, -7.10192988, -7.1091108, + -7.1091108, -7.05166344, -7.0732062, -7.06602528, -7.05884436, + -7.02293976, -7.01575884, -6.99421608, -6.98703516, -6.99421608, + -6.95831148, -6.99421608, -6.95831148, -6.97985424, -6.9654924, + -6.92240688, -6.9295878, -6.90086412, -6.90086412, -6.87214044, + -6.85059768, -6.8577786, -6.85059768, -6.83623584, -6.82905492, + -6.79315032, -6.821874, -6.79315032, -6.80033124, -6.7859694, + -6.75724572, -6.74288388, -6.73570296, -6.7141602, -6.7141602, + -6.76442664, -6.70697928, -6.7141602, -6.67107468, -6.61362732, + -6.66389376, -6.64953192, -6.62080824, -6.62798916, -6.61362732, + -6.62080824, -6.5705418, -6.59208456, -6.5705418, -6.54899904, + -6.57772272, -6.4987326, -6.51309444, -6.51309444, -6.47718984, + -6.48437076, -6.47000892, -6.4269234, -6.462828, -6.45564708, + -6.41256156, -6.41974248, -6.41256156, -6.4269234, -6.36229512, + -6.38383788, -6.3551142, -6.36947604, -6.3192096, -6.34075236, + -6.283305, -6.29766684, -6.27612408, -6.27612408, -6.26176224, + -6.24021948, -6.23303856, -6.23303856, -6.18995304, -6.16841028, + -6.22585764, -6.16841028, -6.19713396, -6.14686752, -6.12532476, + -6.15404844, -6.103782, -6.12532476, -6.05351556, -6.08942016, + -6.06069648, -6.08223924, -6.0319728, -6.04633464, -6.01043004, + -6.03915372, -5.96734452, -5.95298268, -5.93862084, -5.94580176, + -5.90271624, -5.924259, -5.8883544, -5.87399256, -5.86681164, + -5.88117348, -5.84526888, -5.83808796, -5.83808796, -5.78782152, + -5.79500244, -5.7806406, -5.75909784, -5.73755508, -5.73037416, + -5.71601232, -5.744736, -5.69446956, -5.65856496, -5.68010772, + -5.68010772, -5.62984128, -5.69446956, -5.57957484, -5.62266036, + -5.6011176, -5.565213, -5.6011176, -5.55803208, -5.55803208, + -5.53648932, -5.4934038, -5.48622288, -5.4574992, -5.48622288, + -5.44313736, -5.46468012, -5.40005184, -5.4574992, -5.4215946, + -5.37850908, -5.35696632, -5.3497854, -5.3138808, -5.34260448, + -5.30669988, -5.28515712, -5.29951896, -5.26361436, -5.22052884, + -5.23489068, -5.24925252, -5.206167, -5.17744332, -5.1702624, + -5.16308148, -5.16308148, -5.14871964, -5.11999596, -5.11999596, + -5.1343578, -5.08409136, -5.09127228, -5.08409136, -5.04100584, + -5.05536768, -5.03382492, -4.99792032, -4.9907394, -4.96919664, + -4.96919664, -4.96201572, -4.9189302, -4.93329204, -4.9189302, + -4.89738744, -4.87584468, -4.86148284, -4.83275916, -4.80403548, + -4.76094996, -4.7394072, -4.76813088, -4.76813088, -4.7394072, + -4.68914076, -4.69632168, -4.667598, -4.68914076, -4.67477892, + -4.64605524, -4.64605524, -4.58860788, -4.61733156, -4.5598842, + -4.56706512, -4.56706512, -4.54552236, -4.50243684, -4.47371316, + -4.49525592, -4.4521704, -4.43062764, -4.4521704, -4.42344672, + -4.40190396, -4.42344672, -4.43062764, -4.3803612, -4.3444566, + -4.35163752, -4.32291384, -4.2726474, -4.27982832, -4.2726474, + -4.25828556, -4.22238096, -4.25828556, -4.19365728, -4.2008382, + -4.14339084, -4.1649336, -4.129029, -4.13620992, -4.06440072, + -4.08594348, -4.06440072, -4.04285796, -4.04285796, -4.00695336, + -3.9854106, -4.00695336, -3.96386784, -3.949506, -3.9136014, + -3.94232508, -3.90642048, -3.89205864, -3.87051588, -3.8776968, + -3.8776968, -3.83461128, -3.79152576, -3.79870668, -3.769983, + -3.79152576, -3.74125932, -3.75562116, -3.72689748, -3.70535472, + -3.65508828, -3.6263646, -3.61918368, -3.64790736, -3.57609816, + -3.5545554, -3.56173632, -3.53301264, -3.54737448, -3.50428896, + -3.54019356, -3.47556528, -3.410937, -3.43247976, -3.41811792, + -3.4468416, -3.410937, -3.38939424, -3.35348964, -3.36067056, + -3.2673186, -3.28168044, -3.26013768, -3.24577584, -3.2673186, + -3.23859492, -3.20987124, -3.1955094, -3.16678572, -3.17396664, + -3.1237002, -3.10215744, -3.08061468, -3.09497652, -3.06625284, + -3.00880548, -3.03034824, -2.98726272, -3.0159864, -2.92981536, + -2.95135812, -2.95135812, -2.86518708, -2.87954892, -2.82928248, + -2.85082524, -2.872368, -2.79337788, -2.80773972, -2.7646542, + -2.74311144, -2.74311144, -2.73593052, -2.68566408, -2.692845, + -2.70002592, -2.62821672, -2.63539764, -2.60667396, -2.59949304, + -2.55640752, -2.5492266, -2.52050292, -2.49896016, -2.47023648, + -2.4774174, -2.46305556, -2.43433188, -2.39842728, -2.3697036, + -2.37688452, -2.31943716, -2.31225624, -2.29071348, -2.24044704, + -2.29071348, -2.27635164, -2.25480888, -2.19736152, -2.18299968, + -2.16145692, -2.154276, -2.12555232, -2.12555232, -2.08964772, + -2.1183714, -2.09682864, -2.06092404, -2.01783852, -1.98911484, + -1.96039116, -1.974753, -1.98193392, -1.91012472, -1.87422012, + -1.91730564, -1.85985828, -1.83831552, -1.8311346, -1.80241092, + -1.78086816, -1.7234208, -1.71623988, -1.7593254, -1.69469712, + -1.7234208, -1.69469712, -1.66597344, -1.6516116, -1.60852608, + -1.60852608, -1.57262148, -1.52235504, -1.55107872, -1.50081228, + -1.5079932, -1.46490768, -1.45772676, -1.45054584, -1.44336492, + -1.4002794, -1.39309848, -1.35719388, -1.32128928, -1.30692744, + -1.2925656, -1.27820376, -1.26384192, -1.256661, -1.20639456, + -1.16330904, -1.17767088, -1.1130426, -1.12022352, -1.077138, + -1.12022352, -1.09149984, -1.09868076, -0.97660512, -0.99814788, + -0.97660512, -0.97660512, -0.94070052, -0.91915776, -0.89043408, + -0.8617104, -0.85452948, -0.85452948, -0.86889132, -0.7899012, + -0.76835844, -0.7539966, -0.73963476, -0.73245384, -0.67500648, + -0.67500648, -0.67500648, -0.6103782, -0.6103782, -0.59601636, + -0.6103782, -0.58165452, -0.51702624, -0.49548348, -0.48112164, + -0.50984532, -0.5026644, -0.45957888, -0.41649336, -0.40213152, + -0.37340784, -0.35186508, -0.31596048, -0.30877956, -0.2872368}, + {-0.01436184, 0.01436184, -0.02872368, -0.0359046, -0.00718092, + -0.00718092, 0, 0.02872368, 0.02872368, 0.02154276, + 0.00718092, 0.02154276, 0.02154276, 0.01436184, -0.00718092, + 0.01436184, -0.00718092, 0, -0.0359046, -0.01436184, + -0.01436184, 0.00718092, 0.01436184, 0.02154276, 0.02872368, + 0.02154276, -0.01436184, 0.01436184, 0.01436184, 0.02154276, + 0.00718092, -0.01436184, 0, 0.02872368, 0.0359046, + 0.02872368, 0, -0.00718092, 0.01436184, -0.02154276, + 0.00718092, -0.00718092, -0.0359046, 0, -0.05026644, + -0.05026644, -0.05026644, -0.0359046, -0.02154276, -0.0359046, + -0.08617104, -0.05744736, -0.09335196, -0.08617104, -0.09335196, + -0.12207564, -0.15079932, -0.20106576, -0.18670392, -0.24415128, + -0.2513322, -0.25851312, -0.30159864, -0.33750324, -0.359046, + -0.4308552, -0.45239796, -0.50984532, -0.538569, -0.58883544, + -0.61755912, -0.66064464, -0.6821874, -0.73245384, -0.81862488, + -0.88325316, -0.9335196, -0.95506236, -1.0053288, -1.02687156, + -1.0412334, -1.077138, -1.1130426, -1.12740444, -1.15612812, + -1.2207564, -1.22793732, -1.26384192, -1.26384192, -1.3284702, + -1.26384192, -1.27820376, -1.31410836, -1.32128928, -1.29974652, + -1.2925656, -1.27820376, -1.26384192, -1.21357548, -1.14176628, + -1.16330904, -1.09149984, -1.02687156, -0.99814788, -0.897615, + -0.87607224, -0.74681568, -0.6462828, -0.55293084, -0.42367428, + -0.3949506, -0.31596048, -0.18670392, -0.06462828, 0.08617104, + 0.20824668, 0.33032232, 0.42367428, 0.53138808, 0.6821874, + 0.69654924, 0.81144396, 0.94070052, 1.06995708, 1.10586168, + 1.19203272, 1.26384192, 1.29974652, 1.34283204, 1.39309848, + 1.37155572, 1.42182216, 1.4002794, 1.42900308, 1.436184, + 1.4002794, 1.38591756, 1.35719388, 1.33565112, 1.28538468, + 1.256661, 1.24229916, 1.14176628, 1.09149984, 1.0053288, + 0.91915776, 0.83298672, 0.74681568, 0.65346372, 0.53138808, + 0.5744736, 0.45957888, 0.33032232, 0.22978944, 0.11489472, + 0.04308552, -0.0718092, -0.18670392, -0.29441772, -0.41649336, + -0.43803612, -0.47394072, -0.56011176, -0.69654924, -0.78272028, + -0.83298672, -0.84016764, -0.90479592, -0.91197684, -0.96224328, + -0.9694242, -0.95506236, -0.9694242, -0.96224328, -0.9335196, + -0.8617104, -0.81862488, -0.76835844, -0.70373016, -0.63910188, + -0.5744736, -0.50984532, -0.3949506, -0.29441772, -0.22978944, + -0.1077138, 0.00718092, 0.13643748, 0.23697036, 0.31596048, + 0.3231414, 0.38058876, 0.50984532, 0.58883544, 0.6821874, + 0.74681568, 0.81144396, 0.84016764, 0.84734856, 0.88325316, + 0.89043408, 0.84734856, 0.81862488, 0.81144396, 0.74681568, + 0.70373016, 0.61755912, 0.54574992, 0.45957888, 0.34468416, + 0.359046, 0.2513322, 0.15798024, 0.05026644, -0.0359046, + -0.15798024, -0.20106576, -0.30877956, -0.40931244, -0.48830256, + -0.48830256, -0.52420716, -0.55293084, -0.60319728, -0.63192096, + -0.66064464, -0.6462828, -0.58165452, -0.58165452, -0.45957888, + -0.4667598, -0.38058876, -0.35186508, -0.23697036, -0.16516116, + -0.1077138, -0.02154276, 0.09335196, 0.22260852, 0.25851312, + 0.26569404, 0.33032232, 0.41649336, 0.47394072, 0.52420716, + 0.48830256, 0.51702624, 0.52420716, 0.47394072, 0.4667598, + 0.47394072, 0.38776968, 0.33032232, 0.33750324, 0.22260852, + 0.12925656, 0.06462828, -0.01436184, -0.05744736, -0.16516116, + -0.17234208, -0.22978944, -0.28005588, -0.31596048, -0.35186508, + -0.38776968, -0.36622692, -0.3231414, -0.34468416, -0.3231414, + -0.33032232, -0.30159864, -0.23697036, -0.179523, -0.09335196, + -0.02872368, 0, 0.08617104, 0.12207564, 0.18670392, + 0.19388484, 0.23697036, 0.27287496, 0.29441772, 0.3231414, + 0.27287496, 0.24415128, 0.24415128, 0.27287496, 0.15798024, + 0.20824668, 0.16516116, 0.10053288, 0.05744736, 0.01436184, + -0.0359046, -0.06462828, -0.16516116, -0.179523, -0.18670392, + -0.2154276, -0.2154276, -0.20106576, -0.23697036, -0.2154276, + -0.1436184, -0.17234208, -0.10053288, -0.02154276, -0.05026644, + 0.02872368, 0.02154276, 0.05744736, 0.11489472, 0.12925656, + 0.13643748, 0.15798024, 0.179523, 0.15798024, 0.16516116, + 0.16516116, 0.13643748, 0.1436184, 0.1077138, 0.05744736, + 0.02154276, 0.01436184, -0.02154276, -0.00718092, -0.0718092, + -0.0718092, -0.09335196, -0.09335196, -0.10053288, -0.11489472, + -0.0718092, -0.08617104, -0.0718092, -0.05744736, -0.0359046, + -0.02872368, -0.00718092, 0.0359046, 0.07899012, 0.10053288, + 0.12207564, 0.10053288, 0.12207564, 0.09335196, 0.1077138, + 0.12207564, 0.10053288, 0.08617104, 0.09335196, 0.05026644, + 0.09335196, 0.00718092, 0, -0.05026644, 0, + 0.00718092, -0.02872368, -0.04308552, -0.00718092, -0.02872368, + -0.05026644, -0.07899012, -0.00718092, 0.00718092, 0.04308552, + 0.00718092, 0, 0.02872368, 0.05026644, 0.0359046, + 0.05026644, 0.08617104, 0.06462828, 0.0718092, 0.10053288, + 0.10053288, 0.05744736, 0.0359046, 0.05744736, 0.08617104, + 0.05026644, 0.04308552, 0.01436184, 0.02872368, 0.02154276, + 0.01436184, -0.02872368, 0.00718092, 0.01436184, 0.00718092, + 0.02154276, -0.02872368, 0.02154276, 0.02872368, 0.04308552, + 0.02154276, 0.04308552, 0.07899012, 0.04308552, 0.05026644, + 0.05744736, 0.07899012, 0.02154276, 0.02872368, 0.09335196, + 0.04308552, 0.05026644, 0.08617104, 0.05744736, 0.05744736, + 0.0359046, 0.05026644, 0.04308552, 0.04308552, 0.02872368, + 0.00718092, 0.05026644, 0.02872368, 0.04308552, 0.02154276, + 0.02872368, 0.05026644, 0.05026644, 0.05026644, 0.05026644, + 0.06462828, 0.0359046, 0.02872368, 0.02154276, 0.02154276, + 0.05744736, 0.02872368, 0.01436184, 0.00718092, 0.00718092, + 0.02872368, 0, 0.02872368, 0.0359046, 0.02872368, + 0.00718092, 0.04308552, 0.01436184, 0.02154276, 0.05026644, + 0.04308552, 0.00718092, 0.06462828, 0.02872368, -0.01436184, + 0.07899012, 0.02154276, 0.05744736, 0.02154276, 0.05026644, + 0.02154276, 0.01436184, 0.05744736, 0.02154276, 0.05026644, + 0.05026644, 0.06462828, 0.01436184, 0.05026644, 0.0359046, + 0.05744736, 0.04308552, 0.05026644, 0.04308552, 0, + 0.0359046, 0.05744736, -0.01436184, 0.02872368, 0.0718092, + 0.02154276, 0.05026644, 0.02154276, 0.06462828, 0.00718092, + 0.06462828, 0.02154276, 0.0359046, 0.06462828, 0.07899012, + 0.08617104, 0.02872368, 0.0718092, 0.11489472, 0.10053288, + 0.0718092, 0.07899012, 0.06462828, 0.02154276, 0.07899012, + 0.05744736, 0.06462828, 0.0359046, 0.01436184, 0.06462828, + 0.0718092, 0.0359046, 0.02154276, 0.04308552, 0.01436184, + 0.06462828, 0.0359046, 0.0718092, 0.06462828, 0.06462828, + 0.05744736, 0.05744736, 0.05026644, 0.0718092, 0.06462828, + 0.02872368, 0.05026644, 0.06462828, 0.08617104, 0.10053288, + 0.02872368, 0.05026644, 0.0718092, 0.11489472, 0.07899012, + 0.05026644, 0.06462828, 0.06462828, 0.05026644, 0.0359046, + 0.05026644, 0.07899012, 0.0359046, 0.02872368, 0.06462828, + 0.05026644, 0.05026644, 0.07899012, 0.05026644, 0.0359046, + 0.01436184, 0.04308552, 0.0718092, 0.05744736, 0.09335196, + 0.05026644, 0.09335196, 0.05744736, 0.07899012, 0.04308552, + 0.06462828, 0.04308552, 0.05026644, 0.05026644, 0.05026644, + 0.02872368, 0.06462828, 0.06462828, 0.02154276, 0.06462828, + 0.07899012, 0.05744736, 0.04308552, 0.05744736, 0.05026644, + 0.10053288, 0.04308552, 0.02872368, 0.02872368, 0.08617104, + 0.06462828, 0.0718092, 0.05026644, 0.04308552, 0.04308552, + 0.0718092, 0.05744736, 0.0718092, 0.09335196, 0.05026644, + 0.02872368, 0.05026644, 0.06462828, -0.02154276, 0.04308552, + 0.06462828, 0.04308552, 0.0718092, 0.0359046, 0.05026644, + 0.0359046, 0.02872368, 0.06462828, 0.02872368, 0.02872368, + 0.04308552, 0.0359046, 0.0718092, 0.05744736, 0.0359046, + 0.04308552, 0.05026644, 0.05744736, 0.05744736, 0.02154276, + 0.05026644, 0.04308552, 0.04308552, 0.05026644, 0.01436184, + 0.10053288, 0.05026644, 0.05026644, 0.05744736, 0.04308552, + 0.08617104, 0.0359046, 0.0359046, 0.04308552, 0.0359046, + 0.02872368, 0.01436184, 0.0359046, 0.0359046, 0.04308552, + 0.0359046, 0.05744736, 0.04308552, 0.04308552, 0.05026644, + 0.04308552, 0.05026644, 0.0359046, 0.02154276, 0.05026644, + 0.05744736, 0.00718092, 0.05026644, 0.04308552, 0.06462828, + 0.0359046, 0.02872368, 0.04308552, 0.05744736, 0.07899012, + -0.00718092, 0.0359046, 0.02872368, 0.01436184, 0.00718092, + 0.02872368, 0.01436184, 0.02872368, 0.02154276, 0.02872368, + 0.02872368, -0.00718092, 0.00718092, 0.01436184, 0.0718092, + 0.02154276, -0.00718092, 0, 0.01436184, 0.05026644, + 0.00718092, 0.0359046, 0.01436184, 0.01436184, 0.0359046, + 0.05744736, 0.01436184, 0.04308552, 0.02872368, 0.02154276, + 0.02872368, 0.02872368, 0.02872368, 0.05744736, 0.02154276, + 0.01436184, 0.02872368, 0.01436184, 0.02872368, 0.02154276, + 0.0359046, 0.04308552, 0.05026644, 0.00718092, 0.0359046, + 0.0359046, 0.05026644, 0.01436184, 0.02872368, -0.00718092, + -0.01436184, 0.02154276, 0.02154276, 0.0359046, -0.00718092, + 0.01436184, 0.05026644, 0.02154276, 0, 0.00718092, + -0.00718092, 0.04308552, 0.0359046, 0.05026644, 0.05026644, + 0.00718092, 0.00718092, 0.02872368, 0.0359046, 0.00718092, + -0.01436184, 0.04308552, -0.01436184, 0.00718092, 0.02872368, + 0.04308552, 0.04308552, 0.0359046, 0.0359046, 0.02872368, + 0.06462828, 0.04308552, 0.06462828, 0.0359046, 0.06462828, + 0.04308552, 0.0359046, 0.06462828, 0.01436184, 0.02872368, + 0.00718092, -0.02872368, 0.05026644, 0.00718092, 0.0359046, + 0.01436184, 0.02154276, 0, 0.02154276, 0.02872368, + 0.07899012, 0.05744736, 0.01436184, 0.02872368, 0.05744736, + 0.05744736, 0, 0.0718092, 0.01436184, 0.02872368, + 0.02154276, -0.01436184, 0, 0.00718092, 0.06462828, + 0.01436184, 0.00718092, 0.02154276, 0.02154276, 0.02154276, + 0.06462828, 0.01436184, 0.05744736, 0.0718092, 0.02154276, + 0.02154276, -0.00718092, 0.01436184, 0.02872368, 0.02872368, + 0.04308552, 0.01436184, 0.02154276, 0.05026644, 0.05026644, + 0.06462828, 0.02872368, 0.00718092, 0.00718092, 0.01436184, + 0.02872368, 0.05026644, 0.05744736, 0.0359046, 0.05744736, + 0.0359046, 0.0359046, 0.05744736, 0.02872368, 0.02154276, + 0.06462828, 0.06462828, 0.04308552, 0.0718092, 0.06462828, + 0.0359046, 0.02154276, 0.04308552, 0.05026644, 0.06462828, + 0.01436184, 0.0359046, 0.0359046, 0.05744736, 0.04308552, + 0.04308552, 0.02154276, 0.0359046, 0.02154276, 0.06462828, + 0.04308552, 0.01436184, 0.04308552, 0.05026644, 0.04308552, + 0.0718092, 0.02872368, 0.05026644, 0.0359046, 0.05744736, + 0.05744736, 0.02872368, 0.05744736, 0.05744736, 0.09335196, + 0.07899012, 0, 0.06462828, 0.02154276, 0.02154276, + 0.09335196, 0.02154276, 0.05026644, 0.02154276, 0.06462828, + 0.02154276, 0.00718092, 0.02872368, 0.0359046, 0.05744736, + 0.04308552, 0.06462828, 0.05026644, 0.06462828, 0.01436184, + 0.05744736, 0.04308552, 0.09335196, 0.04308552, 0.02872368, + 0.02872368, 0.05744736, 0.02872368, 0.01436184, 0.02872368, + 0.05744736, 0.02872368, 0.04308552, 0.02872368, 0.0359046, + 0.05026644, 0.02872368, 0, 0.05744736, 0.05744736, + 0.0359046, 0.02872368, 0.02872368, 0, 0.0718092, + 0.07899012, 0.04308552, 0.0359046, 0.05026644, 0.02154276, + 0.02872368, 0.05744736, 0.02872368, -0.00718092, 0.07899012, + 0.0359046, 0.02872368, 0.06462828, 0.02872368, 0.05026644, + 0.02154276, 0.05744736, 0.05744736, 0.0359046, 0.05026644, + 0, 0.04308552, 0.05026644, 0.06462828, 0.06462828, + 0.04308552, 0.02154276, 0.00718092, 0.02872368, 0.02872368, + 0.04308552, 0.05744736, 0.05026644, 0.01436184, 0.06462828, + 0.05026644, 0.02872368, 0.02872368, 0.05026644, 0.02872368, + 0.04308552, 0.07899012, 0.02872368, 0.05744736, 0.05026644, + 0.04308552, 0.05744736, 0.05744736, 0.04308552, 0.07899012, + 0.0359046, 0.05026644, 0.00718092, 0.06462828, 0.02154276, + 0.04308552, 0.0718092, 0.0359046, 0.0359046, 0.04308552, + 0.01436184, 0.05026644, 0.05026644, 0.04308552, 0.05744736, + 0.04308552, 0.05026644, 0.0359046, 0.01436184, 0.0718092, + 0.05744736, 0.05744736, 0.0718092, 0.1077138, 0.0718092, + 0.0359046, 0.06462828, 0.05744736, 0.06462828, 0.06462828, + 0.04308552, 0.02154276, 0.09335196, 0.05026644, 0.04308552, + 0.05744736, 0.0718092, 0.04308552, 0.05026644, 0.05744736, + 0.02872368, 0.02154276, 0.04308552, 0.00718092, 0.0359046, + 0.05026644, 0.04308552, 0.02872368, 0.06462828, 0.09335196, + 0.02154276, 0.00718092, 0.04308552, 0.05026644, 0.05026644, + 0.05744736, 0.04308552, 0.00718092, 0.04308552, 0.04308552, + 0.01436184, 0.06462828, 0.05744736, 0.04308552, 0.05026644, + 0.0359046, 0.05744736, 0.04308552, 0.02872368, 0.05026644, + 0.01436184, 0.00718092, 0.05026644, 0.0359046, 0.0718092, + 0.02872368, 0.06462828, 0.06462828, 0.0718092, 0.06462828, + 0.05744736, 0.00718092, 0.0718092, 0.04308552, 0.05026644, + 0.01436184, 0.05026644, 0.0718092, 0.01436184, 0.04308552, + 0.00718092, 0.08617104, 0.0718092, 0.02154276, 0.05026644, + 0.06462828, 0.02872368, 0.0359046, 0.05026644, 0.04308552, + 0.0718092, 0.04308552, 0.02872368, 0.05744736, 0.02154276, + 0.04308552, 0.08617104, 0.04308552, 0.00718092, 0.04308552, + 0.0359046, 0.0359046, 0.05026644, 0.0718092, 0.05744736, + 0.05744736, -0.00718092, 0.06462828, 0.06462828, 0.06462828, + 0.0359046, 0.04308552, 0.02872368, 0.0359046, 0.0359046, + 0.0359046, 0.05026644, 0.02872368, 0.05026644, 0.02154276, + 0.0718092, 0.01436184, 0.05026644, 0.05744736, 0.05026644, + 0.04308552, 0.05026644, 0.0359046, 0.06462828, 0.05744736, + 0.05744736, 0.05026644, 0.05744736, 0.0359046, 0.05026644, + 0.04308552, 0.02872368, 0.05026644, 0.05026644, 0.05744736, + 0.01436184, 0.04308552, 0.05744736, 0.05744736, 0.0359046, + 0.02154276, 0.02872368, 0.04308552, 0.05026644, 0.05744736, + 0.05744736, 0.0359046, 0.06462828, 0.05744736, 0.0718092, + 0.07899012, 0.04308552, 0.08617104, 0.05026644, 0.05744736, + 0.06462828, 0.07899012, 0.06462828, 0.02872368, 0.0718092, + 0.07899012, 0.05026644, 0.08617104, 0.01436184, 0.05026644, + 0.09335196, 0.04308552, 0.05744736, 0.07899012, 0.09335196, + 0.02154276, 0.05744736, 0.04308552, 0.08617104, 0.05744736, + 0.08617104, 0.02872368, 0.0718092, 0.0359046, 0.05026644, + 0.07899012, 0.06462828, 0.08617104, 0.05744736, 0.06462828, + 0.06462828, 0.06462828, 0.05026644, 0.05744736, 0.01436184, + 0.05744736, 0.08617104, 0.05026644, 0.05744736, 0.06462828, + 0.02154276, 0.05744736, 0.05026644, 0.06462828, 0.05744736, + 0.05026644, 0.06462828, 0.08617104, 0.04308552, 0.0718092, + 0.05026644, 0.02154276, 0.05744736, 0.10053288, 0.08617104, + 0.0359046, 0.04308552, 0.07899012, 0.05744736, 0.0718092, + 0.05744736, 0.05744736, 0.04308552, 0.05744736, 0.04308552, + 0.07899012, 0.05744736, 0.02154276, 0.06462828, 0.05744736, + 0.0359046, 0.00718092, 0.0359046, 0.09335196, 0.0359046, + 0.05744736, 0.07899012, 0.05026644, 0.04308552, 0.06462828, + 0.01436184, 0.01436184, 0.0718092, 0.06462828, 0.07899012, + 0.0359046, 0.07899012, 0.05744736, 0.02872368, 0.0718092, + 0.05026644, 0.0359046, 0.0359046, 0.08617104, 0.07899012, + 0.06462828, 0.04308552, 0.02154276, 0.05744736, 0.04308552, + 0.09335196, 0.04308552, 0.09335196, 0.08617104, 0.06462828, + 0.02154276, 0.04308552, 0.06462828, 0.07899012, 0.05026644, + 0.08617104, 0.02872368, 0.02872368, 0.0359046, 0.02154276, + 0.06462828, 0.07899012, 0.04308552, 0.0718092, 0.05744736, + 0.08617104, 0.05026644, 0.0718092, 0.10053288, 0.06462828, + 0.05744736, 0.08617104, 0.05744736, 0.0718092, 0.02872368, + 0.07899012, 0.05744736, 0.06462828, 0.0359046, 0.05026644, + 0.06462828, 0.07899012, 0.05026644, 0.07899012, 0.09335196, + 0.0718092, 0.05744736, 0.08617104, 0.07899012, 0.11489472, + 0.08617104, 0.06462828, 0.08617104, 0.09335196, 0.05026644, + 0.09335196, 0.0359046, 0.07899012, 0.0718092, 0.07899012, + 0.07899012, 0.05744736, 0.09335196, 0.05744736, 0.07899012, + 0.05026644, 0.0718092, 0.09335196, 0.05026644, 0.06462828, + 0.0718092, 0.05026644, 0.0718092, 0.0718092, 0.0359046, + 0.06462828, 0.08617104, 0.05744736, 0.05744736, 0.07899012, + 0.0718092, 0.05744736, 0.05744736, 0.07899012, 0.06462828, + 0.06462828, 0.07899012, 0.08617104, 0.05026644, 0.0718092, + 0.06462828, 0.08617104, 0.09335196, 0.0718092, 0.09335196, + 0.07899012, 0.0718092, 0.05744736, 0.05744736, 0.08617104, + 0.09335196, 0.05026644, 0.05744736, 0.07899012, 0.09335196, + 0.05026644, 0.09335196, 0.07899012, 0.04308552, 0.10053288, + 0.07899012, 0.11489472, 0.07899012, 0.07899012, 0.0718092, + 0.07899012, 0.07899012, 0.07899012, 0.08617104, 0.09335196, + 0.07899012, 0.06462828, 0.08617104, 0.05026644, 0.0718092, + 0.07899012, 0.09335196, 0.09335196, 0.11489472, 0.11489472, + 0.1077138, 0.07899012, 0.12207564, 0.0718092, 0.10053288, + 0.07899012, 0.05744736, 0.08617104, 0.09335196, 0.13643748, + 0.05744736, 0.10053288, 0.10053288, 0.08617104, 0.12207564, + 0.10053288, 0.08617104, 0.10053288, 0.11489472, 0.10053288, + 0.09335196, 0.10053288, 0.07899012, 0.08617104, 0.08617104, + 0.12207564, 0.0718092, 0.06462828, 0.09335196, 0.10053288, + 0.10053288, 0.05026644, 0.12207564, 0.10053288, 0.09335196, + 0.07899012, 0.0718092, 0.10053288, 0.1436184, 0.11489472, + 0.07899012, 0.11489472, 0.11489472, 0.10053288, 0.11489472, + 0.07899012, 0.11489472, 0.08617104, 0.10053288, 0.12207564, + 0.1077138, 0.08617104, 0.09335196, 0.11489472, 0.10053288, + 0.08617104, 0.08617104, 0.08617104, 0.15079932, 0.11489472, + 0.09335196, 0.12925656, 0.10053288, 0.08617104, 0.1077138, + 0.07899012, 0.10053288, 0.12925656, 0.10053288, 0.1077138, + 0.07899012, 0.08617104, 0.12207564, 0.10053288, 0.09335196, + 0.09335196, 0.0718092, 0.11489472, 0.12207564, 0.12925656, + 0.12925656, 0.10053288, 0.08617104, 0.12925656, 0.11489472, + 0.13643748, 0.06462828, 0.12207564, 0.10053288, 0.12925656, + 0.11489472, 0.12207564, 0.15079932, 0.12207564, 0.1077138, + 0.1436184, 0.09335196, 0.12207564, 0.1077138, 0.08617104, + 0.11489472, 0.07899012, 0.07899012, 0.08617104, 0.12207564, + 0.10053288, 0.08617104, 0.1077138, 0.11489472, 0.1077138, + 0.07899012, 0.12207564, 0.08617104, 0.12925656, 0.15798024, + 0.09335196, 0.12207564, 0.0718092, 0.10053288, 0.09335196, + 0.09335196, 0.12207564, 0.08617104, 0.0718092, 0.10053288, + 0.09335196, 0.0718092, 0.07899012, 0.08617104, 0.09335196, + 0.07899012, 0.07899012, 0.10053288, 0.10053288, 0.1077138, + 0.08617104, 0.11489472, 0.07899012, 0.08617104, 0.09335196, + 0.1077138, 0.1077138, 0.10053288, 0.10053288, 0.08617104, + 0.0718092, 0.10053288, 0.1436184, 0.07899012, 0.10053288, + 0.09335196, 0.1077138, 0.0718092, 0.10053288, 0.1077138, + 0.11489472, 0.11489472, 0.12207564, 0.13643748, 0.0718092, + 0.0718092, 0.09335196, 0.1077138, 0.12925656, 0.08617104, + 0.09335196, 0.11489472, 0.09335196, 0.08617104, 0.08617104, + 0.06462828, 0.10053288, 0.1077138, 0.07899012, 0.09335196, + 0.08617104, 0.05744736, 0.11489472, 0.11489472, 0.09335196, + 0.09335196, 0.07899012, 0.06462828, 0.07899012, 0.0718092, + 0.0718092, 0.06462828, 0.08617104, 0.08617104, 0.06462828, + 0.06462828, 0.05026644, 0.07899012, 0.1077138, 0.0718092, + 0.1077138, 0.10053288, 0.06462828, 0.0718092, 0.09335196, + 0.10053288, 0.05744736, 0.10053288, 0.05744736, 0.10053288, + 0.08617104, 0.05744736, 0.05744736, 0.08617104, 0.06462828, + 0.05744736, 0.0359046, 0.05744736, 0.09335196, 0.0718092, + 0.06462828, 0.07899012, 0.09335196, 0.08617104, 0.09335196, + 0.05744736, 0.08617104, 0.13643748, 0.0718092, 0.05026644, + 0.06462828, 0.06462828, 0.11489472, 0.09335196, 0.0718092, + 0.06462828, 0.0718092, 0.06462828, 0.10053288, 0.06462828, + 0.0718092, 0.09335196, 0.12207564, 0.05026644, 0.09335196, + 0.0718092, 0.07899012, 0.05744736, 0.07899012, 0.08617104, + 0.0718092, 0.10053288, 0.09335196, 0.05026644, 0.0718092, + 0.07899012, 0.10053288, 0.06462828, 0.10053288, 0.07899012, + 0.05744736, 0.10053288, 0.05026644, 0.09335196, 0.08617104, + 0.1077138, 0.05744736, 0.1077138, 0.05744736, 0.05744736, + 0.0718092, 0.08617104, 0.08617104, 0.05744736, 0.08617104, + 0.0718092, 0.1077138, 0.07899012, 0.10053288, 0.06462828, + 0.07899012, 0.06462828, 0.06462828, 0.05744736, 0.09335196, + 0.07899012, 0.1077138, 0.08617104, 0.07899012, 0.0718092, + 0.11489472, 0.04308552, 0.0359046, 0.1077138, 0.06462828, + 0.11489472, 0.07899012, 0.07899012, 0.1077138, 0.06462828, + 0.05026644, 0.10053288, 0.08617104, 0.0718092, 0.08617104, + 0.11489472, 0.13643748, 0.1077138, 0.11489472, 0.1077138, + 0.10053288, 0.09335196, 0.07899012, 0.11489472, 0.10053288, + 0.1077138, 0.11489472, 0.1077138, 0.15079932, 0.07899012, + 0.09335196, 0.1077138, 0.08617104, 0.12207564, 0.06462828, + 0.1077138, 0.09335196, 0.10053288, 0.05026644, 0.12925656, + 0.10053288, 0.07899012, 0.1077138, 0.10053288, 0.06462828, + 0.13643748, 0.05026644, 0.07899012, 0.05744736, 0.07899012, + 0.11489472, 0.0718092, 0.09335196, 0.08617104, 0.08617104, + 0.11489472, 0.07899012, 0.13643748, 0.11489472, 0.10053288, + 0.10053288, 0.08617104, 0.10053288, 0.08617104, 0.08617104, + 0.06462828, 0.12207564, 0.09335196, 0.12207564, 0.10053288, + 0.08617104, 0.1077138, 0.11489472, 0.11489472, 0.1077138, + 0.08617104, 0.1077138, 0.10053288, 0.08617104, 0.0718092, + 0.12207564, 0.12207564, 0.12207564, 0.12925656, 0.08617104, + 0.07899012, 0.10053288, 0.12207564, 0.15079932, 0.0718092, + 0.12925656, 0.11489472, 0.10053288, 0.09335196, 0.12207564, + 0.12207564, 0.11489472, 0.10053288, 0.08617104, 0.12925656, + 0.11489472, 0.13643748, 0.09335196, 0.13643748, 0.13643748, + 0.1436184, 0.1436184, 0.12207564, 0.13643748, 0.16516116, + 0.1077138, 0.12207564, 0.15079932, 0.12207564, 0.15079932, + 0.11489472, 0.1077138, 0.11489472, 0.12207564, 0.1077138, + 0.1436184, 0.06462828, 0.15079932, 0.1077138, 0.10053288, + 0.17234208, 0.1436184, 0.12207564, 0.13643748, 0.12925656, + 0.15798024, 0.11489472, 0.11489472, 0.16516116, 0.1436184, + 0.15079932, 0.1077138, 0.1077138, 0.12925656, 0.11489472, + 0.11489472, 0.11489472, 0.15798024, 0.08617104, 0.13643748, + 0.17234208, 0.13643748, 0.13643748, 0.10053288, 0.12207564, + 0.11489472, 0.15798024, 0.15079932, 0.15079932, 0.16516116, + 0.12925656, 0.1436184, 0.09335196, 0.08617104, 0.12207564, + 0.1077138, 0.15798024, 0.13643748, 0.12207564, 0.13643748, + 0.12207564, 0.09335196, 0.15798024, 0.12925656, 0.1077138, + 0.1077138, 0.12207564, 0.13643748, 0.15079932, 0.11489472, + 0.1436184, 0.13643748, 0.11489472, 0.13643748, 0.16516116, + 0.13643748, 0.16516116, 0.17234208, 0.15798024, 0.1077138, + 0.12207564, 0.13643748, 0.1436184, 0.12925656, 0.15798024, + 0.1436184, 0.15798024, 0.13643748, 0.15798024, 0.15798024, + 0.15079932, 0.12925656, 0.12925656, 0.12925656, 0.12207564, + 0.13643748, 0.1436184, 0.16516116, 0.16516116, 0.15798024, + 0.12207564, 0.16516116, 0.16516116, 0.1436184, 0.13643748, + 0.12207564, 0.179523, 0.18670392, 0.1436184, 0.12925656, + 0.1077138, 0.1436184, 0.1436184, 0.17234208, 0.11489472, + 0.12925656, 0.13643748, 0.08617104, 0.16516116, 0.15079932, + 0.13643748, 0.15798024, 0.1436184, 0.11489472, 0.16516116, + 0.12925656, 0.13643748, 0.09335196, 0.1077138, 0.16516116, + 0.16516116, 0.16516116, 0.15798024, 0.15079932, 0.1436184, + 0.12925656, 0.1436184, 0.1436184, 0.12925656, 0.13643748, + 0.1436184, 0.17234208, 0.13643748, 0.13643748, 0.13643748, + 0.15079932, 0.16516116, 0.1436184, 0.1436184, 0.1436184, + 0.1436184, 0.12207564, 0.15798024, 0.16516116, 0.15079932, + 0.12925656, 0.11489472, 0.15079932, 0.16516116, 0.12207564, + 0.13643748, 0.15798024, 0.1436184, 0.18670392, 0.12207564, + 0.12925656, 0.15079932, 0.15798024, 0.17234208, 0.13643748, + 0.13643748, 0.12925656, 0.179523, 0.15798024, 0.20824668, + 0.15079932, 0.18670392, 0.07899012, 0.15798024, 0.15798024, + 0.12925656, 0.17234208, 0.1436184, 0.179523, 0.15079932, + 0.16516116, 0.15079932, 0.17234208, 0.18670392, 0.15798024, + 0.17234208, 0.12207564, 0.17234208, 0.15798024, 0.15079932, + 0.1436184, 0.15079932, 0.18670392, 0.16516116, 0.18670392, + 0.19388484, 0.12925656, 0.1436184, 0.2154276, 0.15079932, + 0.179523, 0.18670392, 0.179523, 0.18670392, 0.16516116, + 0.17234208, 0.16516116, 0.12925656, 0.13643748, 0.16516116, + 0.17234208, 0.15798024, 0.1436184, 0.15798024, 0.18670392, + 0.20106576, 0.18670392, 0.15079932, 0.15798024, 0.15798024, + 0.19388484, 0.17234208, 0.1436184, 0.17234208, 0.179523, + 0.179523, 0.16516116, 0.18670392, 0.15079932, 0.1436184, + 0.20106576, 0.19388484, 0.15079932, 0.20106576, 0.17234208, + 0.17234208, 0.19388484, 0.18670392, 0.15798024, 0.12207564, + 0.179523, 0.15079932, 0.179523, 0.1436184, 0.15798024, + 0.20106576, 0.15079932, 0.17234208, 0.16516116, 0.16516116, + 0.16516116, 0.12925656, 0.15798024, 0.17234208, 0.18670392, + 0.19388484, 0.15079932, 0.17234208, 0.16516116, 0.179523, + 0.19388484, 0.15798024, 0.15798024, 0.1077138, 0.15798024, + 0.15798024, 0.179523, 0.16516116, 0.17234208, 0.1436184, + 0.179523, 0.179523, 0.15079932, 0.1436184, 0.16516116, + 0.15079932, 0.1436184, 0.1077138, 0.15798024, 0.17234208, + 0.15798024, 0.17234208, 0.19388484, 0.18670392, 0.179523, + 0.18670392, 0.179523, 0.20106576, 0.18670392, 0.20106576, + 0.12207564, 0.20106576, 0.16516116, 0.13643748, 0.17234208, + 0.17234208, 0.179523, 0.20106576, 0.19388484, 0.18670392, + 0.2154276, 0.16516116, 0.16516116, 0.179523, 0.20106576, + 0.15798024, 0.15079932, 0.19388484, 0.19388484, 0.16516116, + 0.19388484, 0.18670392, 0.17234208, 0.15798024, 0.18670392, + 0.20824668, 0.20106576, 0.12925656, 0.17234208, 0.17234208, + 0.16516116, 0.179523, 0.15798024, 0.20106576, 0.18670392, + 0.2154276, 0.13643748, 0.179523, 0.18670392, 0.17234208, + 0.15798024, 0.17234208, 0.17234208, 0.15798024, 0.18670392, + 0.20824668, 0.18670392, 0.179523, 0.19388484, 0.23697036, + 0.179523, 0.18670392, 0.18670392, 0.18670392, 0.179523, + 0.19388484, 0.22260852, 0.19388484, 0.18670392, 0.20106576, + 0.18670392, 0.15079932, 0.19388484, 0.19388484, 0.20824668, + 0.179523, 0.15798024, 0.19388484, 0.15798024, 0.179523, + 0.179523, 0.18670392, 0.20824668, 0.17234208, 0.16516116, + 0.19388484, 0.179523, 0.17234208, 0.19388484, 0.22260852, + 0.17234208, 0.15079932, 0.20106576, 0.15079932, 0.15798024, + 0.18670392, 0.17234208, 0.16516116, 0.15798024, 0.17234208, + 0.20824668, 0.15079932, 0.20106576, 0.19388484, 0.17234208, + 0.19388484, 0.16516116, 0.15798024, 0.179523, 0.17234208, + 0.16516116, 0.16516116, 0.17234208, 0.18670392, 0.18670392, + 0.15798024, 0.15798024, 0.20106576, 0.17234208, 0.17234208, + 0.179523, 0.18670392, 0.15079932, 0.1436184, 0.16516116, + 0.17234208, 0.18670392, 0.15079932, 0.15079932, 0.15798024, + 0.15079932, 0.17234208, 0.1436184, 0.179523, 0.15798024, + 0.12207564, 0.15079932, 0.17234208, 0.17234208, 0.15079932, + 0.16516116, 0.179523, 0.16516116, 0.15798024, 0.2154276, + 0.12925656, 0.15079932, 0.15798024, 0.12925656, 0.16516116, + 0.13643748, 0.13643748, 0.16516116, 0.1436184, 0.17234208, + 0.17234208, 0.15079932, 0.16516116, 0.12207564, 0.18670392, + 0.17234208, 0.15079932, 0.15798024, 0.13643748, 0.16516116, + 0.179523, 0.17234208, 0.16516116, 0.18670392, 0.179523, + 0.13643748, 0.15079932, 0.1436184, 0.16516116, 0.1436184, + 0.15798024, 0.1436184, 0.12925656, 0.15798024, 0.15079932, + 0.15798024, 0.16516116, 0.1436184, 0.17234208, 0.1436184, + 0.19388484, 0.19388484, 0.17234208, 0.17234208, 0.18670392, + 0.13643748, 0.15798024, 0.15798024, 0.18670392, 0.20106576, + 0.12925656, 0.15798024, 0.17234208, 0.179523, 0.16516116, + 0.15798024, 0.18670392, 0.15798024, 0.1436184, 0.16516116, + 0.179523, 0.15798024, 0.179523, 0.15079932, 0.20824668, + 0.16516116, 0.15798024, 0.11489472, 0.1436184, 0.18670392, + 0.16516116, 0.15079932, 0.13643748, 0.13643748, 0.15079932, + 0.15079932, 0.15079932, 0.15079932, 0.1436184, 0.16516116, + 0.16516116, 0.12925656, 0.15079932, 0.15798024, 0.17234208, + 0.16516116, 0.1436184, 0.15079932, 0.12207564, 0.15798024, + 0.13643748, 0.16516116, 0.22260852, 0.19388484, 0.22978944, + 0.15079932, 0.179523, 0.15798024, 0.18670392, 0.17234208, + 0.1436184, 0.15079932, 0.18670392, 0.1436184, 0.13643748, + 0.179523, 0.16516116, 0.17234208, 0.15798024, 0.179523, + 0.17234208, 0.12925656, 0.17234208, 0.18670392, 0.15079932, + 0.18670392, 0.15798024, 0.18670392, 0.16516116, 0.17234208, + 0.1436184, 0.1436184, 0.15798024, 0.17234208, 0.1436184, + 0.17234208, 0.179523, 0.13643748, 0.20106576, 0.15798024, + 0.18670392, 0.15079932, 0.15079932, 0.179523, 0.19388484, + 0.17234208, 0.15079932, 0.20824668, 0.179523, 0.2154276, + 0.15798024, 0.17234208, 0.15079932, 0.17234208, 0.15079932, + 0.179523, 0.18670392, 0.18670392, 0.17234208, 0.15798024, + 0.1436184, 0.20106576, 0.19388484, 0.17234208, 0.179523, + 0.19388484, 0.13643748, 0.17234208, 0.1436184, 0.19388484, + 0.19388484, 0.17234208, 0.18670392, 0.17234208, 0.179523, + 0.1436184, 0.15079932, 0.19388484, 0.17234208, 0.179523, + 0.22260852, 0.20106576, 0.179523, 0.16516116, 0.19388484, + 0.18670392, 0.179523, 0.20824668, 0.18670392, 0.18670392, + 0.16516116, 0.179523, 0.17234208, 0.20106576, 0.15079932, + 0.19388484, 0.19388484, 0.16516116, 0.19388484, 0.19388484, + 0.15079932, 0.15079932, 0.13643748, 0.17234208, 0.18670392, + 0.16516116, 0.16516116, 0.15798024, 0.15079932, 0.15079932, + 0.18670392, 0.18670392, 0.17234208, 0.17234208, 0.19388484, + 0.179523, 0.18670392, 0.16516116, 0.179523, 0.20106576, + 0.18670392, 0.179523, 0.19388484, 0.179523, 0.19388484, + 0.20824668, 0.16516116, 0.19388484, 0.20106576, 0.19388484, + 0.20824668, 0.179523, 0.179523, 0.19388484, 0.19388484, + 0.15798024, 0.179523, 0.18670392, 0.18670392, 0.19388484, + 0.18670392, 0.20106576, 0.20106576, 0.2154276, 0.16516116, + 0.20824668, 0.20106576, 0.19388484, 0.15079932, 0.22260852, + 0.18670392, 0.17234208, 0.20106576, 0.179523, 0.19388484, + 0.19388484, 0.179523, 0.19388484, 0.20824668, 0.19388484, + 0.16516116, 0.20824668, 0.18670392, 0.2154276, 0.19388484, + 0.2154276, 0.19388484, 0.20106576, 0.2154276, 0.22978944, + 0.19388484, 0.16516116, 0.20106576, 0.2154276, 0.15798024, + 0.22260852, 0.18670392, 0.20106576, 0.19388484, 0.18670392, + 0.19388484, 0.17234208, 0.16516116, 0.179523, 0.20106576, + 0.22260852, 0.18670392, 0.20824668, 0.19388484, 0.20824668, + 0.179523, 0.16516116, 0.15798024, 0.2154276, 0.15079932, + 0.17234208, 0.179523, 0.15079932, 0.15079932, 0.15079932, + 0.15798024, 0.16516116, 0.17234208, 0.15079932, 0.1436184, + 0.18670392, 0.19388484, 0.16516116, 0.16516116, 0.15079932, + 0.16516116, 0.179523, 0.18670392, 0.18670392, 0.18670392, + 0.15798024, 0.17234208, 0.17234208, 0.16516116, 0.179523, + 0.15798024, 0.1436184, 0.16516116, 0.16516116, 0.15798024, + 0.1436184, 0.15079932, 0.20106576, 0.15798024, 0.17234208, + 0.18670392, 0.17234208, 0.16516116, 0.16516116, 0.18670392, + 0.20824668, 0.19388484, 0.18670392, 0.2154276, 0.16516116, + 0.1436184, 0.16516116, 0.19388484, 0.20106576, 0.16516116, + 0.179523, 0.20106576, 0.20824668, 0.16516116, 0.15798024, + 0.20106576, 0.18670392, 0.15798024, 0.179523, 0.179523, + 0.15079932, 0.15798024, 0.19388484, 0.20106576, 0.15798024, + 0.18670392, 0.1436184, 0.13643748, 0.17234208, 0.16516116, + 0.179523, 0.17234208, 0.16516116, 0.16516116, 0.18670392, + 0.16516116, 0.15079932, 0.12925656, 0.179523, 0.15079932, + 0.20106576, 0.20824668, 0.17234208, 0.179523, 0.19388484, + 0.19388484, 0.19388484, 0.19388484, 0.15798024, 0.17234208, + 0.18670392, 0.17234208, 0.15079932, 0.179523, 0.15079932, + 0.20106576, 0.16516116, 0.18670392, 0.15079932, 0.15798024, + 0.12925656, 0.179523, 0.15798024, 0.17234208, 0.179523, + 0.15798024, 0.18670392, 0.17234208, 0.18670392, 0.15798024, + 0.18670392, 0.179523, 0.15798024, 0.17234208, 0.13643748, + 0.18670392, 0.13643748, 0.17234208, 0.1436184, 0.15079932, + 0.12207564, 0.15798024, 0.17234208, 0.17234208, 0.16516116, + 0.179523, 0.179523, 0.15798024, 0.15079932, 0.17234208, + 0.12207564, 0.2154276, 0.1436184, 0.19388484, 0.13643748, + 0.17234208, 0.17234208, 0.20106576, 0.16516116, 0.18670392}, + {1.0254F, + 0.00718092, + 0.01436184, + 0.00718092, + -0.01436184, + 0.02872368, + -0.02154276, + -0.00718092, + 0.01436184, + 0.02872368, + 0.02872368, + -0.01436184, + 0.04308552, + -0.01436184, + 0, + 0.0359046, + -0.01436184, + -0.02872368, + -0.02154276, + -0.0359046, + -0.01436184, + 0.00718092, + -0.01436184, + 0.01436184, + -0.01436184, + -0.01436184, + 0.02154276, + 0.01436184, + 0.01436184, + -0.04308552, + -0.02154276, + 0.01436184, + 0.00718092, + -0.00718092, + -0.00718092, + -0.84734856, + -1.27820376, + -1.76650632, + -2.27635164, + -2.8005588, + -2.80773972, + -3.32476596, + -3.92078232, + -4.58142696, + -5.22770976, + -5.87399256, + -6.57772272, + -7.30299564, + -8.02108764, + -8.77508424, + -8.78944608, + -9.5865282, + -10.36206756, + -11.12324508, + -11.97059364, + -12.79639944, + -13.62220524, + -14.46237288, + -15.2953596, + -16.13552724, + -16.14270816, + -16.97569488, + -17.76559608, + -18.59140188, + -19.3525794, + -20.12093784, + -20.83902984, + -21.62893104, + -22.30393752, + -23.00048676, + -22.96458216, + -23.58214128, + -24.15661488, + -24.68800296, + -25.13322, + -25.5999798, + -26.00929224, + -26.36833824, + -26.61967044, + -26.8566408, + -26.8566408, + -27.00744012, + -27.08643024, + -27.08643024, + -27.07924932, + -26.92845, + -26.69866056, + -26.40424284, + -25.98056856, + -25.44199956, + -25.46354232, + -24.83880228, + -24.07762476, + -23.28054264, + -22.31829936, + -21.42068436, + -20.56615488, + -19.6039116, + -18.56985912, + -17.32037904, + -17.36346456, + -16.02063252, + -14.6131722, + -13.01182704, + -11.31712992, + -9.52908084, + -7.61895612, + -5.57957484, + -3.51146988, + -1.26384192, + -1.27102284, + 1.03405248, + 3.38221332, + 5.83090704, + 8.32268628, + 10.80010368, + 13.34933028, + 15.81956676, + 18.3472506, + 20.85339168, + 20.81030616, + 23.22309528, + 25.60716072, + 27.88351236, + 30.03060744, + 32.0269032, + 33.85085688, + 35.50246848, + 36.92429064, + 37.49158332, + 37.50594516, + 37.86499116, + 38.00142864, + 37.83626748, + 37.4844024, + 36.81657684, + 35.84715264, + 34.66230084, + 33.11840304, + 31.3447158, + 31.39498224, + 29.31251544, + 26.9643546, + 24.40794708, + 21.62175012, + 18.5985828, + 15.46770168, + 12.0998502, + 8.69609412, + 5.14871964, + 5.15590056, + 1.55107872, + -2.10400956, + -5.6729268, + -9.21312036, + -12.6743238, + -15.98472792, + -19.15869456, + -22.06696716, + -24.75981216, + -24.73108848, + -27.1438776, + -29.19762072, + -30.9138606, + -32.32850184, + -33.37691616, + -34.0375608, + -34.35352128, + -34.20272196, + -33.7144194, + -33.74314308, + -32.85988992, + -31.56732432, + -29.86544628, + -27.84042684, + -25.53535152, + -22.80660192, + -19.927053, + -16.76026728, + -13.38523488, + -13.37805396, + -9.81631764, + -6.18995304, + -2.44869372, + 1.29974652, + 4.94765388, + 8.53811388, + 11.9562318, + 15.13737936, + 18.05283288, + 18.0241092, + 20.66668776, + 23.03639136, + 24.953697, + 26.45450928, + 27.56037096, + 28.19947284, + 28.31436756, + 27.97686432, + 27.20850588, + 27.1797822, + 25.9231212, + 24.28587144, + 22.21058556, + 19.69726356, + 16.93260936, + 13.90944204, + 10.64212344, + 7.19528184, + 3.70535472, + 3.71253564, + 0.17234208, + -3.32476596, + -6.6782556, + -9.86658408, + -12.76767576, + -15.39589248, + -17.60043492, + -19.51055964, + -20.91801996, + -20.90365812, + -21.88744416, + -22.37574672, + -22.33984212, + -21.94489152, + -20.95392456, + -19.55364516, + -17.70814872, + -15.46770168, + -12.91847508, + -12.95437968, + -10.11791628, + -7.16655816, + -4.07876256, + -0.98378604, + 2.11119048, + 5.00510124, + 7.76975544, + 10.23999192, + 12.3511824, + 12.4229916, + 14.17513608, + 15.46770168, + 16.4084022, + 16.81771464, + 16.78899096, + 16.336593, + 15.37434972, + 13.98843216, + 12.25783044, + 12.2793732, + 10.1969064, + 7.9349166, + 5.4215946, + 2.89391076, + 0.30159864, + -2.21172336, + -4.64605524, + -6.77878848, + -8.70327504, + -8.71045596, + -10.31180112, + -11.50383384, + -12.33682056, + -12.80358036, + -12.76767576, + -12.3152778, + -11.5612812, + -10.3764294, + -8.88279804, + -8.91152172, + -7.20246276, + -5.29951896, + -3.20987124, + -1.15612812, + 0.84016764, + 2.85800616, + 4.57424604, + 6.11814384, + 7.40352852, + 7.3963476, + 8.35859088, + 8.98333092, + 9.17721576, + 9.0838638, + 8.60274216, + 7.82002188, + 6.7859694, + 5.47904196, + 4.00695336, + 4.02849612, + 2.41997004, + 0.79708212, + -0.84734856, + -2.35534176, + -3.79152576, + -4.96201572, + -5.82372612, + -6.54899904, + -6.8936832, + -6.91522596, + -6.97267332, + -6.79315032, + -6.31202868, + -5.55085116, + -4.58860788, + -3.52583172, + -2.31225624, + -1.077138, + 0.15079932, + 0.179523, + 1.3643748, + 2.48459832, + 3.31758504, + 4.06440072, + 4.49525592, + 4.75376904, + 4.7753118, + 4.54552236, + 4.04285796, + 4.07158164, + 3.42529884, + 2.67130224, + 1.77368724, + 0.85452948, + -0.0718092, + -0.94070052, + -1.78086816, + -2.513322, + -3.09497652, + -3.051891, + -3.46120344, + -3.63354552, + -3.67663104, + -3.51146988, + -3.21705216, + -2.70720684, + -2.17581876, + -1.5079932, + -0.86889132, + -0.8617104, + -0.18670392, + 0.50984532, + 1.04841432, + 1.52953596, + 1.88858196, + 2.13991416, + 2.29071348, + 2.24044704, + 2.10400956, + 2.1183714, + 1.84549644, + 1.5079932, + 1.04841432, + 0.6103782, + 0.1077138, + -0.37340784, + -0.79708212, + -1.17767088, + -1.5438978, + -1.52953596, + -1.75214448, + -1.9029438, + -1.87422012, + -1.79523, + -1.66597344, + -1.44336492, + -1.17767088, + -0.81862488, + -0.49548348, + -0.48830256, + -0.15798024, + 0.19388484, + 0.45957888, + 0.73963476, + 0.91915776, + 1.077138, + 1.1848518, + 1.19203272, + 1.13458536, + 1.1130426, + 1.01250972, + 0.79708212, + 0.63910188, + 0.40213152, + 0.19388484, + -0.05026644, + -0.27287496, + -0.44521704, + -0.63192096, + -0.6103782, + -0.76117752, + -0.77553936, + -0.77553936, + -0.74681568, + -0.68936832, + -0.538569, + -0.45239796, + -0.27287496, + -0.07899012, + -0.1077138, + 0.04308552, + 0.2154276, + 0.33750324, + 0.41649336, + 0.52420716, + 0.59601636, + 0.55293084, + 0.56729268, + 0.50984532, + 0.49548348, + 0.41649336, + 0.34468416, + 0.24415128, + 0.15079932, + 0.02154276, + -0.12207564, + -0.179523, + -0.2513322, + -0.30877956, + -0.2872368, + -0.30877956, + -0.359046, + -0.3231414, + -0.31596048, + -0.30877956, + -0.30877956, + -0.3231414, + -0.33032232, + -0.37340784, + -0.35186508, + -0.44521704, + -0.50984532, + -0.538569, + -0.58165452, + -0.62474004, + -0.72527292, + -0.76835844, + -0.7539966, + -0.78272028, + -0.81144396, + -0.81862488, + -0.78272028, + -0.73963476, + -0.718092, + -0.71091108, + -0.63910188, + -0.65346372, + -0.61755912, + -0.56729268, + -0.56729268, + -0.48112164, + -0.45239796, + -0.43803612, + -0.47394072, + -0.45239796, + -0.43803612, + -0.4667598, + -0.49548348, + -0.48830256, + -0.52420716, + -0.55293084, + -0.51702624, + -0.56011176, + -0.56729268, + -0.58165452, + -0.6103782, + -0.67500648, + -0.65346372, + -0.63910188, + -0.6462828, + -0.6821874, + -0.66064464, + -0.66064464, + -0.65346372, + -0.59601636, + -0.58883544, + -0.56729268, + -0.5744736, + -0.55293084, + -0.53138808, + -0.48830256, + -0.38058876, + -0.29441772, + -0.18670392, + -0.05744736, + 0.05026644, + 0.15798024, + 0.25851312, + 0.30159864, + 0.29441772, + 0.37340784, + 0.41649336, + 0.41649336, + 0.4308552, + 0.3949506, + 0.37340784, + 0.33032232, + 0.27287496, + 0.16516116, + 0.20824668, + 0.11489472, + 0.02872368, + -0.07899012, + -0.1436184, + -0.15079932, + -0.16516116, + -0.22260852, + -0.24415128, + -0.28005588, + -0.25851312, + -0.25851312, + -0.2154276, + -0.13643748, + -0.1077138, + -0.10053288, + -0.04308552, + 0.04308552, + 0.0718092, + 0.12207564, + 0.09335196, + 0.17234208, + 0.20824668, + 0.20106576, + 0.24415128, + 0.22978944, + 0.22978944, + 0.19388484, + 0.13643748, + 0.17234208, + 0.17234208, + 0.10053288, + 0.06462828, + 0.06462828, + 0.02872368, + -0.00718092, + -0.0359046, + -0.06462828, + -0.06462828, + -0.09335196, + -0.0718092, + -0.13643748, + -0.12207564, + -0.08617104, + -0.0359046, + -0.05026644, + -0.0359046, + 0, + -0.00718092, + 0.05026644, + 0.04308552, + 0.07899012, + 0.0718092, + 0.0718092, + 0.1436184, + 0.11489472, + 0.09335196, + 0.1436184, + 0.10053288, + 0.13643748, + 0.1077138, + 0.06462828, + 0.10053288, + 0.1077138, + 0.0718092, + 0.09335196, + 0.05744736, + 0, + 0.02154276, + 0.00718092, + 0.02154276, + -0.02154276, + -0.01436184, + -0.00718092, + -0.05026644, + 0.00718092, + 0, + -0.01436184, + -0.02154276, + 0.02154276, + -0.00718092, + 0.02872368, + 0.0359046, + 0.06462828, + 0.05744736, + 0.08617104, + 0.05744736, + 0.05744736, + 0.1077138, + 0.0718092, + 0.1077138, + 0.12207564, + 0.08617104, + 0.08617104, + 0.09335196, + 0.0718092, + 0.0718092, + 0.0718092, + 0.05026644, + 0.0718092, + 0.07899012, + 0.01436184, + 0.02872368, + 0.04308552, + 0.04308552, + 0.02154276, + 0.01436184, + 0.00718092, + 0.05744736, + 0.0359046, + 0.00718092, + 0.05026644, + 0.02872368, + 0.0359046, + 0.04308552, + 0.05744736, + 0.04308552, + 0.0359046, + 0.0718092, + 0.06462828, + 0.05026644, + 0.06462828, + 0.05744736, + 0.06462828, + 0.05026644, + 0.06462828, + 0.0359046, + 0.0359046, + 0.05744736, + 0.05026644, + 0.05744736, + 0.05026644, + 0.04308552, + 0.0718092, + 0.04308552, + 0.05026644, + 0.02872368, + 0.02154276, + 0.04308552, + 0.01436184, + 0.05744736, + 0.0359046, + 0.04308552, + 0.02872368, + 0.05026644, + 0.05744736, + 0.0718092, + 0.04308552, + 0.0359046, + 0.05026644, + 0.05744736, + 0.1077138, + 0.05744736, + 0.07899012, + 0.0718092, + 0.01436184, + 0.05026644, + 0.07899012, + 0.08617104, + 0.0718092, + 0.08617104, + 0.05744736, + 0.06462828, + 0.08617104, + 0.0718092, + 0.01436184, + 0.0718092, + 0.0718092, + 0.04308552, + 0.02154276, + 0.06462828, + 0.04308552, + 0.04308552, + 0.06462828, + 0.05026644, + 0.09335196, + 0.1077138, + 0.00718092, + 0.02872368, + 0.06462828, + 0.05026644, + 0.04308552, + 0.04308552, + 0.07899012, + 0.04308552, + 0, + 0.06462828, + 0.0359046, + 0.04308552, + 0.04308552, + 0.06462828, + 0.0359046, + 0.01436184, + 0.04308552, + 0.02872368, + 0.01436184, + 0.05744736, + 0.09335196, + 0.06462828, + 0.06462828, + 0.05744736, + 0.02154276, + 0.0718092, + 0.04308552, + 0.0718092, + 0.02872368, + 0.05744736, + 0.02154276, + 0.05026644, + 0.06462828, + 0.06462828, + 0.04308552, + 0.0359046, + 0.05026644, + 0.06462828, + 0.04308552, + 0.0718092, + 0.02154276, + 0.05026644, + 0.06462828, + 0.05026644, + 0.01436184, + 0.02154276, + 0.0359046, + 0.02154276, + 0.08617104, + 0.05744736, + 0.02872368, + 0.04308552, + 0.04308552, + 0.00718092, + 0.02872368, + 0.01436184, + 0.0359046, + -0.00718092, + 0.02872368, + -0.02154276, + -0.04308552, + -0.00718092, + -0.0359046, + -0.02872368, + -0.06462828, + -0.04308552, + -0.05026644, + -0.01436184, + -0.09335196, + -0.05026644, + -0.05026644, + -0.02154276, + -0.05026644, + -0.05744736, + -0.02872368, + -0.05744736, + -0.02872368, + -0.02872368, + -0.01436184, + 0.02872368, + -0.02872368, + 0.01436184, + -0.00718092, + -0.00718092, + 0.0359046, + 0.02154276, + 0, + 0.0359046, + 0.0359046, + 0, + 0.0359046, + 0.0359046, + -0.00718092, + 0, + 0.02154276, + 0.02872368, + 0.01436184, + -0.01436184, + -0.00718092, + -0.01436184, + -0.00718092, + -0.02154276, + -0.02154276, + -0.0359046, + -0.02872368, + -0.00718092, + -0.01436184, + -0.08617104, + -0.01436184, + -0.01436184, + -0.02872368, + -0.00718092, + -0.05026644, + -0.07899012, + -0.0359046, + -0.05744736, + -0.01436184, + -0.04308552, + 0.02154276, + -0.06462828, + -0.01436184, + 0.02872368, + -0.02872368, + 0.0359046, + 0, + 0, + 0.00718092, + -0.01436184, + 0.02872368, + -0.00718092, + 0.01436184, + 0.01436184, + 0.06462828, + 0.00718092, + -0.02872368, + 0.01436184, + 0.01436184, + -0.01436184, + -0.04308552, + 0.00718092, + -0.02154276, + 0.01436184, + 0, + -0.0359046, + -0.02154276, + -0.01436184, + -0.02154276, + -0.02154276, + -0.04308552, + 0, + -0.04308552, + -0.01436184, + 0, + 0.02872368, + -0.05026644, + 0, + -0.01436184, + -0.02872368, + -0.01436184, + -0.0359046, + 0, + -0.00718092, + -0.00718092, + -0.01436184, + 0, + 0.0359046, + 0, + 0.00718092, + -0.01436184, + -0.00718092, + 0, + -0.02872368, + -0.01436184, + 0.02154276, + -0.00718092, + 0.00718092, + 0, + -0.02154276, + 0.02154276, + 0.00718092, + 0.00718092, + 0.00718092, + 0.02154276, + 0.00718092, + -0.00718092, + -0.02872368, + -0.02154276, + -0.0359046, + -0.00718092, + -0.00718092, + -0.05026644, + -0.02872368, + -0.04308552, + -0.02872368, + -0.00718092, + -0.02872368, + -0.01436184, + -0.01436184, + -0.00718092, + 0, + -0.02154276, + 0, + -0.04308552, + -0.02872368, + -0.01436184, + -0.00718092, + -0.01436184, + -0.01436184, + 0.0359046, + -0.04308552, + -0.02872368, + -0.01436184, + 0.00718092, + -0.02154276, + -0.00718092, + -0.01436184, + -0.00718092, + -0.02154276, + -0.02154276, + 0.02154276, + -0.00718092, + -0.02872368, + -0.0359046, + -0.02154276, + -0.02872368, + -0.02872368, + -0.02872368, + 0.01436184, + -0.02154276, + -0.02872368, + -0.0359046, + -0.01436184, + 0.02154276, + 0, + 0, + -0.0359046, + 0.00718092, + -0.00718092, + -0.0359046, + -0.01436184, + 0, + -0.01436184, + -0.01436184, + -0.04308552, + -0.01436184, + -0.05744736, + -0.00718092, + -0.00718092, + -0.02872368, + 0, + -0.02154276, + -0.05026644, + -0.05026644, + -0.05026644, + -0.00718092, + -0.02872368, + -0.0359046, + -0.02154276, + 0, + -0.05026644, + -0.04308552, + -0.01436184, + 0.01436184, + -0.00718092, + 0.02872368, + -0.05026644, + -0.01436184, + 0.00718092, + -0.00718092, + -0.02872368, + -0.04308552, + -0.0359046, + -0.02154276, + -0.0359046, + -0.01436184, + -0.05026644, + -0.02872368, + -0.02872368, + -0.05744736, + -0.01436184, + -0.01436184, + 0.00718092, + -0.02872368, + -0.0359046, + -0.01436184, + -0.02154276, + -0.00718092, + -0.02154276, + -0.0359046, + -0.02872368, + -0.01436184, + 0, + -0.0359046, + -0.02154276, + -0.02872368, + 0.02872368, + 0, + -0.02872368, + 0.00718092, + -0.05026644, + 0.01436184, + -0.01436184, + -0.02872368, + -0.04308552, + 0, + -0.00718092, + -0.0718092, + -0.0359046, + -0.02872368, + -0.05026644, + -0.02154276, + -0.04308552, + 0.01436184, + -0.02154276, + -0.02154276, + -0.0359046, + -0.0359046, + -0.02154276, + -0.0359046, + -0.00718092, + -0.0359046, + -0.00718092, + -0.01436184, + -0.01436184, + -0.05026644, + -0.05744736, + -0.01436184, + -0.02154276, + -0.04308552, + -0.02154276, + -0.01436184, + -0.01436184, + 0.00718092, + -0.05744736, + -0.04308552, + -0.00718092, + -0.08617104, + -0.04308552, + -0.0359046, + -0.02872368, + -0.04308552, + -0.04308552, + -0.01436184, + 0, + -0.05026644, + -0.0718092, + -0.05026644, + -0.04308552, + -0.01436184, + -0.01436184, + -0.0359046, + 0.00718092, + -0.02872368, + -0.05744736, + 0, + -0.06462828, + 0.01436184, + -0.04308552, + -0.04308552, + -0.04308552, + -0.04308552, + 0.01436184, + -0.01436184, + -0.00718092, + -0.00718092, + -0.02154276, + -0.05744736, + -0.04308552, + -0.01436184, + -0.02154276, + -0.0359046, + -0.0359046, + -0.05744736, + -0.01436184, + -0.05744736, + -0.01436184, + -0.06462828, + -0.02872368, + -0.0359046, + -0.00718092, + -0.05744736, + -0.00718092, + -0.05744736, + -0.00718092, + -0.00718092, + -0.05744736, + -0.05744736, + -0.02154276, + -0.02154276, + -0.02872368, + -0.02154276, + -0.02872368, + -0.01436184, + -0.0359046, + -0.06462828, + -0.01436184, + -0.05744736, + -0.00718092, + -0.02872368, + -0.00718092, + -0.04308552, + -0.02872368, + -0.01436184, + -0.02154276, + -0.02154276, + -0.02154276, + -0.02872368, + 0.01436184, + -0.07899012, + 0.01436184, + -0.02872368, + 0, + -0.00718092, + 0, + 0, + -0.00718092, + -0.01436184, + -0.02154276, + -0.00718092, + -0.01436184, + -0.0359046, + -0.02872368, + -0.00718092, + 0.02154276, + 0.00718092, + 0, + 0.00718092, + 0, + 0.0359046, + -0.01436184, + -0.0359046, + -0.01436184, + 0.04308552, + -0.00718092, + 0.00718092, + 0.00718092, + -0.00718092, + 0.01436184, + 0.00718092, + 0.00718092, + 0.00718092, + -0.01436184, + -0.00718092, + -0.02154276, + 0, + 0.02154276, + 0, + -0.00718092, + -0.02154276, + -0.02872368, + -0.02872368, + 0.00718092, + 0.02872368, + 0, + -0.00718092, + -0.01436184, + 0.00718092, + -0.02154276, + 0.00718092, + -0.00718092, + 0.01436184, + 0, + 0.01436184, + -0.01436184, + -0.00718092, + -0.02872368, + 0.02154276, + -0.01436184, + -0.01436184, + -0.02154276, + -0.00718092, + -0.00718092, + -0.00718092, + 0, + -0.02154276, + 0.00718092, + 0.01436184, + 0.00718092, + -0.00718092, + -0.01436184, + 0.00718092, + -0.00718092, + 0.02154276, + -0.00718092, + -0.02872368, + -0.00718092, + -0.00718092, + 0.01436184, + 0.00718092, + 0.02872368, + 0.01436184, + 0.02154276, + 0.02872368, + -0.00718092, + 0.02872368, + -0.00718092, + -0.00718092, + 0.00718092, + 0, + 0, + 0, + 0, + 0.04308552, + -0.00718092, + 0.00718092, + -0.01436184, + 0.01436184, + 0.00718092, + 0, + 0.02872368, + -0.01436184, + -0.00718092, + 0.01436184, + 0, + 0, + 0.00718092, + 0.00718092, + 0.0359046, + 0.00718092, + -0.0359046, + -0.01436184, + -0.01436184, + -0.00718092, + 0.01436184, + -0.02154276, + -0.01436184, + 0.00718092, + -0.04308552, + -0.01436184, + 0, + 0.00718092, + 0.02154276, + -0.00718092, + 0.00718092, + 0.0359046, + 0.00718092, + -0.02154276, + -0.00718092, + 0.02872368, + 0, + 0.01436184, + 0.01436184, + 0.02872368, + -0.02154276, + 0.02154276, + 0.01436184, + -0.01436184, + 0.00718092, + 0.04308552, + 0.00718092, + 0.01436184, + 0, + 0, + -0.00718092, + 0.04308552, + -0.00718092, + 0.00718092, + -0.01436184, + 0, + -0.00718092, + 0.01436184, + 0.0359046, + 0.02872368, + 0.01436184, + 0.00718092, + 0.02872368, + 0.01436184, + -0.00718092, + 0.00718092, + 0.04308552, + 0.0359046, + 0.0359046, + -0.01436184, + -0.00718092, + -0.00718092, + 0.01436184, + 0.00718092, + 0.02154276, + 0, + -0.0359046, + 0.02154276, + 0.02872368, + 0.02154276, + 0.01436184, + 0.01436184, + 0.04308552, + 0.01436184, + -0.00718092, + 0.02154276, + 0.00718092, + 0.05026644, + 0.01436184, + 0.01436184, + 0.02872368, + 0.02872368, + 0.04308552, + 0.06462828, + 0.00718092, + 0.05026644, + 0.02872368, + 0.04308552, + 0.01436184, + 0.05026644, + 0.06462828, + 0.06462828, + 0.0359046, + 0.05026644, + 0.02154276, + 0.02154276, + 0.0359046, + 0.02872368, + 0.05744736, + 0.02872368, + 0.00718092, + 0.02872368, + 0.04308552, + 0.05026644, + 0.05026644, + 0.01436184, + 0.0718092, + 0.04308552, + 0.02872368, + 0.0359046, + 0.02872368, + 0.02154276, + 0.02154276, + 0.05026644, + 0.02872368, + 0.04308552, + 0.05026644, + 0.0718092, + 0.0718092, + 0.04308552, + 0.05026644, + 0.0718092, + 0.11489472, + 0.05026644, + 0.05744736, + 0.07899012, + 0.0718092, + 0.05744736, + 0.07899012, + 0.1077138, + 0.06462828, + 0.09335196, + 0.09335196, + 0.11489472, + 0.12925656, + 0.1077138, + 0.10053288, + 0.12207564, + 0.08617104, + 0.12207564, + 0.15798024, + 0.10053288, + 0.10053288, + 0.1077138, + 0.11489472, + 0.09335196, + 0.1077138, + 0.11489472, + 0.11489472, + 0.09335196, + 0.09335196, + 0.10053288, + 0.12207564, + 0.0718092, + 0.06462828, + 0.11489472, + 0.08617104, + 0.12925656, + 0.07899012, + 0.07899012, + 0.07899012, + 0.08617104, + 0.09335196, + 0.06462828, + 0.08617104, + 0.0718092, + 0.07899012, + 0.1077138, + 0.09335196, + 0.05026644, + 0.0718092, + 0.08617104, + 0.05744736, + 0.06462828, + 0.0718092, + 0.0718092, + 0.08617104, + 0.10053288, + 0.05026644, + 0.10053288, + 0.09335196, + 0.06462828, + 0.05744736, + 0.08617104, + 0.09335196, + 0.08617104, + 0.05026644, + 0.13643748, + 0.09335196, + 0.06462828, + 0.11489472, + 0.10053288, + 0.09335196, + 0.09335196, + 0.06462828, + 0.08617104, + 0.08617104, + 0.10053288, + 0.12925656, + 0.1436184, + 0.1077138, + 0.1077138, + 0.11489472, + 0.12207564, + 0.12925656, + 0.09335196, + 0.15079932, + 0.12925656, + 0.09335196, + 0.12925656, + 0.08617104, + 0.12207564, + 0.12207564, + 0.09335196, + 0.13643748, + 0.10053288, + 0.1077138, + 0.12207564, + 0.10053288, + 0.12925656, + 0.11489472, + 0.08617104, + 0.07899012, + 0.12925656, + 0.12207564, + 0.12925656, + 0.11489472, + 0.1077138, + 0.08617104, + 0.08617104, + 0.09335196, + 0.08617104, + 0.08617104, + 0.08617104, + 0.09335196, + 0.07899012, + 0.10053288, + 0.11489472, + 0.07899012, + 0.07899012, + 0.09335196, + 0.11489472, + 0.10053288, + 0.11489472, + 0.09335196, + 0.05744736, + 0.08617104, + 0.09335196, + 0.0718092, + 0.1077138, + 0.1077138, + 0.06462828, + 0.09335196, + 0.10053288, + 0.1077138, + 0.07899012, + 0.07899012, + 0.1077138, + 0.09335196, + 0.11489472, + 0.09335196, + 0.12207564, + 0.11489472, + 0.12925656, + 0.11489472, + 0.09335196, + 0.1077138, + 0.11489472, + 0.13643748, + 0.1077138, + 0.12207564, + 0.10053288, + 0.12925656, + 0.12925656, + 0.11489472, + 0.11489472, + 0.10053288, + 0.11489472, + 0.08617104, + 0.12207564, + 0.12207564, + 0.12207564, + 0.15079932, + 0.1436184, + 0.1436184, + 0.16516116, + 0.15079932, + 0.12925656, + 0.13643748, + 0.13643748, + 0.1077138, + 0.1077138, + 0.13643748, + 0.09335196, + 0.12925656, + 0.13643748, + 0.15079932, + 0.1077138, + 0.12925656, + 0.12207564, + 0.1077138, + 0.13643748, + 0.13643748, + 0.10053288, + 0.1436184, + 0.13643748, + 0.11489472, + 0.1436184, + 0.12207564, + 0.1436184, + 0.1436184, + 0.12925656, + 0.1077138, + 0.12207564, + 0.12925656, + 0.13643748, + 0.13643748, + 0.11489472, + 0.15798024, + 0.15079932, + 0.13643748, + 0.12207564, + 0.1436184, + 0.10053288, + 0.12207564, + 0.13643748, + 0.12207564, + 0.1077138, + 0.18670392, + 0.1077138, + 0.1436184, + 0.15798024, + 0.17234208, + 0.1436184, + 0.13643748, + 0.179523, + 0.15079932, + 0.15079932, + 0.16516116, + 0.15798024, + 0.18670392, + 0.179523, + 0.13643748, + 0.1436184, + 0.17234208, + 0.15798024, + 0.12925656, + 0.15798024, + 0.19388484, + 0.18670392, + 0.15079932, + 0.15079932, + 0.17234208, + 0.17234208, + 0.17234208, + 0.15798024, + 0.17234208, + 0.1436184, + 0.15079932, + 0.15798024, + 0.18670392, + 0.1436184, + 0.19388484, + 0.11489472, + 0.16516116, + 0.15079932, + 0.15079932, + 0.19388484, + 0.16516116, + 0.16516116, + 0.15798024, + 0.17234208, + 0.17234208, + 0.16516116, + 0.179523, + 0.1436184, + 0.15798024, + 0.15798024, + 0.12925656, + 0.18670392, + 0.179523, + 0.20106576, + 0.16516116, + 0.20106576, + 0.15079932, + 0.16516116, + 0.1436184, + 0.17234208, + 0.18670392, + 0.20106576, + 0.18670392, + 0.16516116, + 0.16516116, + 0.15079932, + 0.15798024, + 0.1436184, + 0.15079932, + 0.1436184, + 0.1436184, + 0.15079932, + 0.179523, + 0.17234208, + 0.16516116, + 0.2154276, + 0.179523, + 0.15798024, + 0.15079932, + 0.12925656, + 0.16516116, + 0.179523, + 0.17234208, + 0.15798024, + 0.18670392, + 0.15079932, + 0.18670392, + 0.17234208, + 0.16516116, + 0.17234208, + 0.179523, + 0.179523, + 0.18670392, + 0.1436184, + 0.179523, + 0.18670392, + 0.19388484, + 0.23697036, + 0.20824668, + 0.1436184, + 0.18670392, + 0.179523, + 0.20824668, + 0.19388484, + 0.16516116, + 0.179523, + 0.15798024, + 0.18670392, + 0.18670392, + 0.13643748, + 0.18670392, + 0.16516116, + 0.17234208, + 0.16516116, + 0.16516116, + 0.17234208, + 0.18670392, + 0.20824668, + 0.179523, + 0.179523, + 0.15079932, + 0.16516116, + 0.20106576, + 0.17234208, + 0.20106576, + 0.15798024, + 0.20106576, + 0.19388484, + 0.18670392, + 0.15798024, + 0.19388484, + 0.18670392, + 0.20106576, + 0.16516116, + 0.1436184, + 0.15079932, + 0.19388484, + 0.18670392, + 0.16516116, + 0.17234208, + 0.17234208, + 0.20824668, + 0.22260852, + 0.18670392, + 0.18670392, + 0.15079932, + 0.17234208, + 0.20106576, + 0.2154276, + 0.16516116, + 0.179523, + 0.179523, + 0.20106576, + 0.18670392, + 0.20824668, + 0.20824668, + 0.19388484, + 0.19388484, + 0.15798024, + 0.18670392, + 0.20824668, + 0.18670392, + 0.15798024, + 0.18670392, + 0.20106576, + 0.20106576, + 0.2513322, + 0.20106576, + 0.179523, + 0.19388484, + 0.20106576, + 0.20106576, + 0.15798024, + 0.19388484, + 0.2154276, + 0.20824668, + 0.20106576, + 0.20106576, + 0.18670392, + 0.20824668, + 0.19388484, + 0.20106576, + 0.20106576, + 0.18670392, + 0.179523, + 0.18670392, + 0.18670392, + 0.17234208, + 0.18670392, + 0.20824668, + 0.20824668, + 0.22978944, + 0.18670392, + 0.20106576, + 0.22978944, + 0.19388484, + 0.22260852, + 0.19388484, + 0.2154276, + 0.25851312, + 0.179523, + 0.22978944, + 0.20106576, + 0.22978944, + 0.22978944, + 0.2154276, + 0.24415128, + 0.2513322, + 0.22978944, + 0.23697036, + 0.22260852, + 0.22978944, + 0.2154276, + 0.2154276, + 0.23697036, + 0.22978944, + 0.22978944, + 0.26569404, + 0.22260852, + 0.27287496, + 0.17234208, + 0.20824668, + 0.2154276, + 0.20824668, + 0.23697036, + 0.22260852, + 0.26569404, + 0.24415128, + 0.23697036, + 0.24415128, + 0.26569404, + 0.22260852, + 0.20824668, + 0.2872368, + 0.22978944, + 0.24415128, + 0.20106576, + 0.23697036, + 0.2513322, + 0.25851312, + 0.2513322, + 0.2513322, + 0.27287496, + 0.24415128, + 0.2513322, + 0.26569404, + 0.23697036, + 0.28005588, + 0.25851312, + 0.2513322, + 0.25851312, + 0.26569404, + 0.29441772, + 0.23697036, + 0.23697036, + 0.28005588, + 0.24415128, + 0.27287496, + 0.2872368, + 0.27287496, + 0.22260852, + 0.26569404, + 0.27287496, + 0.28005588, + 0.27287496, + 0.2513322, + 0.25851312, + 0.2513322, + 0.24415128, + 0.26569404, + 0.28005588, + 0.27287496, + 0.2513322, + 0.26569404, + 0.2513322, + 0.25851312, + 0.24415128, + 0.28005588, + 0.2513322, + 0.2513322, + 0.24415128, + 0.27287496, + 0.29441772, + 0.25851312, + 0.26569404, + 0.25851312, + 0.2872368, + 0.2872368, + 0.24415128, + 0.28005588, + 0.2872368, + 0.26569404, + 0.25851312, + 0.2513322, + 0.28005588, + 0.27287496, + 0.31596048, + 0.29441772, + 0.25851312, + 0.30877956, + 0.27287496, + 0.30877956, + 0.29441772, + 0.28005588, + 0.29441772, + 0.29441772, + 0.30159864, + 0.3231414, + 0.29441772, + 0.31596048, + 0.28005588, + 0.27287496, + 0.31596048, + 0.29441772, + 0.33750324, + 0.30877956, + 0.31596048, + 0.27287496, + 0.31596048, + 0.30877956, + 0.31596048, + 0.31596048, + 0.33032232, + 0.29441772, + 0.30159864, + 0.3231414, + 0.31596048, + 0.3231414, + 0.29441772, + 0.3231414, + 0.31596048, + 0.3231414, + 0.34468416, + 0.33750324, + 0.33032232, + 0.29441772, + 0.34468416, + 0.33032232, + 0.3231414, + 0.33032232, + 0.33032232, + 0.33750324, + 0.33032232, + 0.3231414, + 0.33750324, + 0.3231414, + 0.28005588, + 0.30877956, + 0.31596048, + 0.3231414, + 0.33032232, + 0.37340784, + 0.33750324, + 0.34468416, + 0.33032232, + 0.3231414, + 0.3231414, + 0.3231414, + 0.35186508, + 0.2872368, + 0.31596048, + 0.35186508, + 0.34468416, + 0.34468416, + 0.36622692, + 0.34468416, + 0.38776968, + 0.359046, + 0.35186508, + 0.359046, + 0.33032232, + 0.36622692, + 0.36622692, + 0.35186508, + 0.37340784, + 0.37340784, + 0.34468416, + 0.31596048, + 0.38058876, + 0.38058876, + 0.37340784, + 0.3949506, + 0.3949506, + 0.359046, + 0.38776968, + 0.33032232, + 0.38058876, + 0.37340784, + 0.35186508, + 0.38058876, + 0.35186508, + 0.36622692, + 0.40213152, + 0.42367428, + 0.359046, + 0.38776968, + 0.38776968, + 0.41649336, + 0.4308552, + 0.35186508, + 0.38058876, + 0.359046, + 0.3949506, + 0.37340784, + 0.38776968, + 0.41649336, + 0.40213152, + 0.3949506, + 0.37340784, + 0.41649336, + 0.37340784, + 0.38058876, + 0.38776968, + 0.37340784, + 0.37340784, + 0.38776968, + 0.42367428, + 0.40931244, + 0.41649336, + 0.42367428, + 0.38058876, + 0.43803612, + 0.4308552, + 0.41649336, + 0.4308552, + 0.40931244, + 0.41649336, + 0.38058876, + 0.40931244, + 0.40931244, + 0.38058876, + 0.3949506, + 0.43803612, + 0.43803612, + 0.44521704, + 0.40213152, + 0.40931244, + 0.3949506, + 0.40931244, + 0.45239796, + 0.4308552, + 0.4308552, + 0.42367428, + 0.45239796, + 0.4308552, + 0.4308552, + 0.47394072, + 0.42367428, + 0.44521704, + 0.42367428, + 0.44521704, + 0.40213152, + 0.44521704, + 0.4308552, + 0.45957888, + 0.42367428, + 0.45957888, + 0.48112164, + 0.43803612, + 0.43803612, + 0.45239796, + 0.45957888, + 0.44521704, + 0.4308552, + 0.45239796, + 0.42367428, + 0.42367428, + 0.45957888, + 0.42367428, + 0.45957888, + 0.45239796, + 0.4667598, + 0.44521704, + 0.48112164, + 0.45239796, + 0.49548348, + 0.4667598, + 0.4667598, + 0.44521704, + 0.44521704, + 0.45239796, + 0.47394072, + 0.43803612, + 0.5026644, + 0.48112164, + 0.44521704, + 0.48112164, + 0.48830256, + 0.45239796, + 0.48830256, + 0.4667598, + 0.4667598, + 0.48830256, + 0.47394072, + 0.5026644, + 0.50984532, + 0.49548348, + 0.45239796, + 0.49548348, + 0.51702624, + 0.53138808, + 0.51702624, + 0.5026644, + 0.52420716, + 0.5026644, + 0.538569, + 0.51702624, + 0.538569, + 0.48830256, + 0.51702624, + 0.49548348, + 0.49548348, + 0.50984532, + 0.51702624, + 0.52420716, + 0.52420716, + 0.52420716, + 0.55293084, + 0.51702624, + 0.52420716, + 0.50984532, + 0.53138808, + 0.51702624, + 0.56011176, + 0.538569, + 0.56729268, + 0.53138808, + 0.54574992, + 0.60319728, + 0.56729268, + 0.56729268, + 0.56011176, + 0.538569, + 0.56729268, + 0.55293084, + 0.59601636, + 0.55293084, + 0.56011176, + 0.56011176, + 0.5744736, + 0.60319728, + 0.5744736, + 0.56011176, + 0.60319728, + 0.60319728, + 0.56011176, + 0.58165452, + 0.60319728, + 0.61755912, + 0.59601636, + 0.59601636, + 0.56011176, + 0.62474004, + 0.61755912, + 0.6103782, + 0.6103782, + 0.58165452, + 0.63192096, + 0.6103782, + 0.59601636, + 0.6103782, + 0.62474004, + 0.6103782, + 0.63910188, + 0.58883544, + 0.62474004, + 0.60319728, + 0.61755912, + 0.59601636, + 0.60319728, + 0.61755912, + 0.58883544, + 0.63192096, + 0.63910188, + 0.6462828, + 0.62474004, + 0.63192096, + 0.59601636, + 0.65346372, + 0.65346372, + 0.63192096, + 0.6462828, + 0.6462828, + 0.63192096, + 0.65346372, + 0.66782556, + 0.66782556, + 0.62474004, + 0.65346372, + 0.63910188, + 0.66782556, + 0.66064464, + 0.66782556, + 0.6462828, + 0.63910188, + 0.63192096, + 0.68936832, + 0.63910188, + 0.68936832, + 0.67500648, + 0.6821874, + 0.6821874, + 0.67500648, + 0.70373016, + 0.66782556, + 0.68936832, + 0.68936832, + 0.68936832, + 0.70373016, + 0.718092, + 0.70373016, + 0.718092, + 0.72527292, + 0.69654924, + 0.71091108, + 0.7539966, + 0.73245384, + 0.718092, + 0.72527292, + 0.73245384, + 0.73245384, + 0.72527292, + 0.6821874, + 0.718092, + 0.73245384, + 0.718092, + 0.72527292, + 0.73963476, + 0.718092, + 0.76835844, + 0.73963476, + 0.73245384, + 0.70373016, + 0.74681568, + 0.72527292, + 0.78272028, + 0.73245384, + 0.77553936, + 0.7899012, + 0.76117752, + 0.78272028, + 0.7899012, + 0.78272028, + 0.77553936, + 0.7899012, + 0.76835844, + 0.80426304, + 0.81862488, + 0.7899012, + 0.78272028, + 0.85452948, + 0.7899012, + 0.78272028, + 0.78272028, + 0.83298672, + 0.77553936, + 0.7899012, + 0.81144396, + 0.81862488, + 0.80426304, + 0.76835844, + 0.84734856, + 0.81144396, + 0.81862488, + 0.84734856, + 0.83298672, + 0.84734856, + 0.8258058, + 0.84016764, + 0.85452948, + 0.84016764, + 0.8617104, + 0.83298672, + 0.85452948, + 0.87607224, + 0.85452948, + 0.86889132, + 0.88325316, + 0.86889132, + 0.8617104, + 0.86889132, + 0.84734856, + 0.87607224, + 0.87607224, + 0.8617104, + 0.90479592, + 0.84734856, + 0.91197684, + 0.87607224, + 0.87607224, + 0.87607224, + 0.88325316, + 0.8617104, + 0.90479592, + 0.90479592, + 0.90479592, + 0.897615, + 0.90479592, + 0.91197684, + 0.88325316, + 0.90479592, + 0.87607224, + 0.89043408, + 0.90479592, + 0.91197684, + 0.89043408, + 0.90479592, + 0.90479592, + 0.94070052, + 0.91915776, + 0.897615, + 0.91197684, + 0.90479592, + 0.91197684, + 0.89043408, + 0.897615, + 0.91197684, + 0.95506236, + 0.94788144, + 0.9335196, + 0.91197684, + 0.95506236, + 0.9335196, + 0.9335196, + 0.90479592, + 0.9694242, + 0.9335196, + 0.91197684, + 0.96224328, + 0.94788144, + 0.91915776, + 0.96224328, + 0.94070052, + 0.9694242, + 0.97660512, + 0.94788144, + 0.95506236, + 0.94070052, + 0.90479592, + 0.94070052, + 0.9694242, + 0.9694242, + 0.96224328, + 0.95506236, + 0.95506236, + 0.97660512, + 0.96224328, + 0.99096696, + 0.9694242, + 0.96224328, + 0.98378604, + 0.98378604, + 0.97660512, + 0.97660512, + 1.02687156, + 0.96224328, + 0.9694242, + 0.9694242, + 1.01969064, + 0.99814788, + 0.98378604, + 1.01969064, + 1.02687156, + 0.9694242, + 1.01969064, + 1.01969064, + 0.99814788, + 1.0053288, + 1.0053288, + 1.04841432, + 1.02687156, + 0.99096696, + 1.06995708, + 1.02687156, + 0.99814788, + 1.02687156, + 1.0412334, + 1.04841432, + 1.08431892, + 1.08431892, + 1.03405248, + 1.077138, + 1.02687156, + 1.04841432, + 1.06995708, + 1.06995708, + 1.05559524, + 1.077138, + 1.04841432, + 1.01969064, + 1.10586168, + 1.077138, + 1.1130426, + 1.02687156, + 1.08431892, + 1.06995708, + 1.05559524, + 1.08431892, + 1.06995708, + 1.06277616, + 1.1489472, + 1.08431892, + 1.077138, + 1.09868076, + 1.1130426, + 1.1130426, + 1.1130426, + 1.14176628, + 1.15612812, + 1.12740444, + 1.13458536, + 1.1130426, + 1.14176628, + 1.13458536, + 1.12740444, + 1.1489472, + 1.15612812, + 1.1130426, + 1.12740444, + 1.12740444, + 1.14176628, + 1.19203272, + 1.15612812, + 1.1489472, + 1.12740444, + 1.15612812, + 1.12740444, + 1.1489472, + 1.16330904, + 1.17048996, + 1.14176628, + 1.13458536, + 1.17767088, + 1.1489472, + 1.14176628, + 1.17767088, + 1.16330904, + 1.17767088, + 1.15612812, + 1.19203272, + 1.1848518, + 1.17767088, + 1.19203272, + 1.2207564, + 1.17048996, + 1.1848518, + 1.20639456, + 1.19203272, + 1.21357548, + 1.19921364, + 1.20639456, + 1.17048996, + 1.23511824, + 1.17767088, + 1.16330904, + 1.21357548, + 1.20639456, + 1.20639456, + 1.23511824, + 1.20639456, + 1.23511824, + 1.26384192, + 1.2207564, + 1.2207564, + 1.2207564, + 1.22793732, + 1.17767088, + 1.22793732, + 1.23511824, + 1.21357548, + 1.17767088, + 1.2207564, + 1.22793732, + 1.24948008, + 1.22793732, + 1.19203272, + 1.19921364, + 1.24229916, + 1.24948008, + 1.26384192, + 1.24229916, + 1.23511824, + 1.22793732, + 1.22793732, + 1.26384192, + 1.256661, + 1.2925656, + 1.2925656, + 1.24948008, + 1.256661, + 1.23511824, + 1.24948008, + 1.27820376, + 1.24948008, + 1.26384192, + 1.24948008, + 1.26384192, + 1.24229916, + 1.23511824, + 1.24229916, + 1.24229916, + 1.256661, + 1.28538468, + 1.27820376, + 1.30692744, + 1.28538468, + 1.23511824, + 1.28538468, + 1.2925656, + 1.23511824, + 1.2925656, + 1.256661, + 1.27102284, + 1.256661, + 1.256661, + 1.28538468, + 1.2925656, + 1.29974652, + 1.27102284, + 1.31410836, + 1.27102284, + 1.27820376, + 1.27102284, + 1.30692744, + 1.22793732, + 1.2925656, + 1.29974652, + 1.27102284, + 1.29974652, + 1.31410836, + 1.2925656, + 1.27820376, + 1.30692744, + 1.2925656, + 1.30692744, + 1.30692744, + 1.31410836, + 1.2925656, + 1.31410836, + 1.32128928, + 1.30692744, + 1.2925656, + 1.2925656, + 1.27102284, + 1.3284702, + 1.34283204, + 1.31410836, + 1.27102284, + 1.30692744, + 1.33565112, + 1.2925656, + 1.29974652, + 1.2925656, + 1.3284702, + 1.34283204, + 1.28538468, + 1.3284702, + 1.3284702, + 1.30692744, + 1.2925656, + 1.29974652, + 1.2925656, + 1.32128928, + 1.3284702, + 1.32128928, + 1.33565112, + 1.30692744, + 1.3643748, + 1.32128928, + 1.3643748, + 1.32128928, + 1.33565112, + 1.3284702, + 1.3284702, + 1.34283204, + 1.33565112, + 1.33565112, + 1.35719388, + 1.35719388, + 1.33565112, + 1.37873664, + 1.32128928, + 1.3643748, + 1.32128928, + 1.35001296, + 1.37155572, + 1.32128928, + 1.29974652, + 1.34283204, + 1.34283204, + 1.33565112, + 1.3643748, + 1.33565112, + 1.37873664, + 1.3643748, + 1.35001296, + 1.33565112, + 1.34283204, + 1.34283204, + 1.33565112, + 1.33565112, + 1.3643748, + 1.35719388, + 1.35719388, + 1.35001296, + 1.35001296, + 1.35719388, + 1.3643748, + 1.3643748, + 1.35001296, + 1.3643748, + 1.35001296, + 1.34283204, + 1.37155572, + 1.35719388, + 1.38591756, + 1.3643748, + 1.33565112, + 1.33565112, + 1.38591756, + 1.37873664, + 1.35001296, + 1.37155572, + 1.35719388, + 1.35719388, + 1.35719388, + 1.35001296, + 1.35001296, + 1.35719388, + 1.37873664, + 1.3643748, + 1.29974652, + 1.34283204, + 1.37873664, + 1.35719388, + 1.3643748, + 1.33565112, + 1.35719388, + 1.32128928, + 1.34283204, + 1.31410836, + 1.38591756, + 1.35719388, + 1.3643748, + 1.3643748, + 1.38591756, + 1.35719388, + 1.35001296, + 1.35719388, + 1.33565112, + 1.34283204, + 1.35719388}}; + +const float GYROSCOPE_DATA[MOTION_SENSOR_AXIS_NUM][IMU_DATA_SIZE] = { + {-0.000152716, + 0, + -0.000305433, + 0, + 0, + 0, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0, + 0, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000305433, + 0, + -0.000152716, + 0.000152716, + -0.000152716, + 0, + 0, + 0.000305433, + 0, + -0.000152716, + 0.000152716, + 0, + -0.000305433, + 0.000152716, + 0.000152716, + -0.000152716, + 0.00122173, + 0.003054326, + 0.00412334, + 0.005650503, + 0.007177667, + 0.007788532, + 0.009315695, + 0.011301007, + 0.012675454, + 0.015424347, + 0.01695151, + 0.019394971, + 0.021685716, + 0.023976461, + 0.026114489, + 0.025961773, + 0.028710666, + 0.031764992, + 0.03436117, + 0.037415496, + 0.040469822, + 0.043065999, + 0.044135013, + 0.04550946, + 0.047342056, + 0.047036623, + 0.04841107, + 0.049938233, + 0.051770829, + 0.053450708, + 0.055130588, + 0.055894169, + 0.056199602, + 0.056352318, + 0.056352318, + 0.056352318, + 0.056810467, + 0.0571159, + 0.0571159, + 0.0571159, + 0.056352318, + 0.055588737, + 0.055130588, + 0.054367006, + 0.053450708, + 0.053450708, + 0.05253441, + 0.051770829, + 0.05131268, + 0.050701815, + 0.049785517, + 0.049174652, + 0.04841107, + 0.047494772, + 0.046731191, + 0.046883907, + 0.045967609, + 0.04550946, + 0.044440446, + 0.043524148, + 0.042455134, + 0.03970624, + 0.036957347, + 0.034666602, + 0.03145956, + 0.032070425, + 0.029168815, + 0.026572638, + 0.02443461, + 0.021838432, + 0.018784106, + 0.016340645, + 0.013744468, + 0.010842858, + 0.008552113, + 0.008552113, + 0.006108652, + 0.003207043, + 0.000610865, + -0.001832596, + -0.004428773, + -0.00702495, + -0.009468411, + -0.012064588, + -0.014813482, + -0.014508049, + -0.017104227, + -0.019700404, + -0.022143865, + -0.023060163, + -0.022296581, + -0.021991149, + -0.021380283, + -0.020922134, + -0.020616702, + -0.020463985, + -0.020005837, + -0.019700404, + -0.019089539, + -0.01863139, + -0.018325957, + -0.018020525, + -0.017409659, + -0.016798794, + -0.016493361, + -0.016340645, + -0.016187929, + -0.015577064, + -0.015118915, + -0.014660766, + -0.014355333, + -0.0140499, + -0.013744468, + -0.013286319, + -0.01282817, + -0.013133603, + -0.01282817, + -0.012217305, + -0.011759156, + -0.011911872, + -0.011759156, + -0.010995574, + -0.011148291, + -0.010231993, + -0.010231993, + -0.010690142, + -0.009621128, + -0.009468411, + -0.008857546, + -0.009010262, + -0.008552113, + -0.008399397, + -0.008246681, + -0.008093964, + -0.007788532, + -0.007788532, + -0.007330383, + -0.007330383, + -0.00702495, + -0.006872234, + -0.006566801, + -0.006261369, + -0.006108652, + -0.006261369, + -0.005650503, + -0.00580322, + -0.005650503, + -0.005345071, + -0.005039638, + -0.004886922, + -0.004886922, + -0.004734206, + -0.004428773, + -0.004581489, + -0.004428773, + -0.004276057, + -0.004276057, + -0.003817908, + -0.003970624, + -0.003970624, + -0.003665191, + -0.003359759, + -0.003207043, + -0.003207043, + -0.003054326, + -0.003054326, + -0.00290161, + -0.00290161, + -0.002748894, + -0.002748894, + -0.002443461, + -0.002290745, + -0.002290745, + -0.002138028, + -0.002138028, + -0.002290745, + -0.002138028, + -0.001832596, + -0.001985312, + -0.001985312, + -0.001832596, + -0.001679879, + -0.001679879, + -0.001374447, + -0.001374447, + -0.001374447, + -0.001374447, + -0.001527163, + -0.001374447, + -0.00122173, + -0.00122173, + -0.001069014, + -0.001069014, + -0.000763582, + -0.000916298, + -0.00122173, + -0.00122173, + -0.000916298, + -0.000916298, + -0.000763582, + -0.000763582, + -0.00122173, + -0.000763582, + -0.000916298, + -0.000305433, + -0.000763582, + -0.000763582, + -0.000763582, + -0.000458149, + -0.000610865, + -0.000305433, + -0.000458149, + -0.000458149, + -0.000458149, + -0.000152716, + -0.000458149, + -0.000610865, + -0.000458149, + -0.000305433, + -0.000458149, + -0.000152716, + -0.000610865, + -0.000305433, + -0.000305433, + -0.000305433, + -0.000305433, + -0.000152716, + -0.000152716, + -0.000305433, + -0.000458149, + -0.000458149, + 0.000152716, + 0.000152716, + 0.000458149, + -0.000305433, + -0.000152716, + -0.000305433, + -0.000458149, + -0.000305433, + -0.000305433, + 0.000152716, + 0, + -0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + -0.000152716, + -0.000152716, + -0.000305433, + -0.000152716, + -0.000152716, + 0, + 0.000152716, + -0.000305433, + 0.000152716, + -0.000305433, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + -0.000305433, + 0, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + -0.000152716, + 0, + 0, + 0.000152716, + -0.000305433, + -0.000305433, + -0.000152716, + 0, + 0, + 0, + 0, + 0, + -0.000152716, + 0, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + -0.000152716, + -0.000305433, + 0, + 0.000305433, + 0, + -0.000152716, + -0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + 0, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + -0.000152716, + -0.000305433, + -0.000152716, + -0.000152716, + 0, + 0, + -0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000458149, + -0.000152716, + -0.000305433, + 0, + -0.000305433, + 0, + 0, + -0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000458149, + 0.000152716, + 0, + 0.000305433, + 0, + -0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0, + -0.000152716, + -0.000458149, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000305433, + -0.000152716, + -0.000152716, + 0, + 0.000152716, + -0.000152716, + -0.000152716, + 0, + 0.000152716, + -0.000305433, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000152716, + 0.000458149, + 0, + -0.000152716, + 0, + 0, + -0.000152716, + -0.000152716, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0, + -0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + -0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000305433, + 0, + -0.000152716, + 0.000152716, + -0.000152716, + -0.000305433, + 0.000305433, + -0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + -0.000305433, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0, + 0, + 0.000152716, + 0, + 0, + -0.000458149, + 0.000152716, + 0, + -0.000152716, + -0.000305433, + 0, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + -0.000152716, + -0.000152716, + 0, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000152716, + -0.000152716, + 0, + 0, + 0, + -0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0, + -0.000305433, + -0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + -0.000152716, + -0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + -0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0, + 0.000152716, + 0, + -0.000152716, + 0, + 0, + 0.000152716, + 0, + -0.000152716, + 0.000610865, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + -0.000152716, + 0, + -0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + -0.000152716, + 0.000305433, + -0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + -0.000305433, + 0, + 0.000305433, + 0, + -0.000305433, + 0, + 0.000152716, + 0, + 0, + -0.000305433, + 0, + 0, + 0, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0, + -0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + -0.000152716, + 0, + -0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000152716, + -0.000305433, + 0.000152716, + 0, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000152716, + -0.000305433, + 0, + -0.000152716, + -0.000152716, + 0.000152716, + -0.000305433, + 0, + -0.000152716, + 0.000305433, + 0.000152716, + -0.000305433, + -0.000305433, + -0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + -0.000305433, + 0, + 0, + 0, + 0.000305433, + -0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + 0, + 0, + 0, + 0, + -0.000152716, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000152716, + -0.000152716, + 0, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + -0.000305433, + 0, + 0, + 0.000152716, + 0, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + -0.000152716, + 0, + 0.000152716, + -0.000152716, + -0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0, + 0, + 0, + 0, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0, + -0.000305433, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000305433, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000458149, + 0.000152716, + -0.000152716, + 0, + -0.000152716, + -0.000305433, + 0, + -0.000305433, + 0, + -0.000152716, + -0.000152716, + 0, + 0, + -0.000152716, + -0.000305433, + -0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0, + 0, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000305433, + 0, + 0.000152716, + 0, + -0.000152716, + 0, + 0, + -0.000152716, + 0.000152716, + -0.000152716, + -0.000305433, + -0.000305433, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + -0.000152716, + 0.000305433, + 0, + 0, + -0.000152716, + 0, + 0.000152716, + 0, + 0, + 0, + 0.000152716, + 0, + 0, + -0.000152716, + 0, + 0, + 0.000305433, + 0, + 0.000305433, + 0, + 0, + 0, + 0.000305433, + -0.000152716, + 0, + 0.000458149, + 0, + 0, + 0.000152716, + -0.000152716, + 0, + 0, + -0.000152716, + 0, + -0.000152716, + 0.000458149, + -0.000305433, + -0.000152716, + -0.000152716, + 0.000152716, + -0.000305433, + -0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0, + 0, + -0.000152716, + 0, + -0.000305433, + -0.000305433, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + -0.000305433, + 0, + 0.000458149, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000305433, + -0.000152716, + 0.000305433, + -0.000152716, + -0.000152716, + -0.000152716, + 0, + 0.000305433, + 0, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0, + -0.000305433, + -0.000152716, + 0.000152716, + 0, + 0, + -0.000152716, + 0.000305433, + -0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + -0.000305433, + 0.000152716, + 0, + -0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000458149, + 0.000305433, + 0.000458149, + -0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0, + -0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000458149, + -0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + -0.000305433, + 0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000610865, + -0.000152716, + 0.000305433, + 0, + -0.000152716, + -0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + -0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + 0, + 0, + 0.000152716, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + -0.000152716, + -0.000152716, + -0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + 0, + 0.000458149, + 0.000152716, + 0, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + 0.000305433, + -0.000152716, + 0, + 0, + 0.000305433, + -0.000152716, + -0.000152716, + -0.000458149, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + 0, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000305433, + 0.000610865, + 0.000152716, + 0.000458149, + 0, + 0, + 0.000152716, + 0, + 0, + -0.000305433, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000305433, + -0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + 0, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + -0.000152716, + 0.000305433, + 0.000610865, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0, + 0.000305433, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + -0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + -0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000458149, + 0, + 0, + 0.000152716, + 0, + 0.000458149, + -0.000152716, + 0, + 0.000305433, + 0.000305433, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0, + 0, + 0, + 0.000458149, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + -0.000152716, + -0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0, + 0, + 0.000763582, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + -0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000152716, + 0, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000458149, + 0, + 0.000152716, + 0.000305433, + -0.000152716, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000610865, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0, + 0, + 0, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0, + 0.000458149, + -0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0, + 0.000305433, + 0.000458149, + -0.000152716, + 0.000305433, + 0, + 0, + 0.000458149, + 0, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000610865, + 0.000305433, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0, + -0.000152716, + 0.000305433, + -0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000458149, + 0.000763582, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000610865, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0, + 0, + 0, + 0.000305433, + -0.000152716, + 0.000152716, + -0.000305433, + -0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000458149, + -0.000152716, + 0.000458149, + -0.000305433, + 0.000458149, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0, + 0, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0, + 0, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0, + 0, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000610865, + 0.000458149, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0, + -0.000152716, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0, + 0, + 0.000305433, + 0, + 0, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000610865, + 0, + 0.000152716, + 0.000610865, + 0.000305433, + 0.000458149, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0, + 0.000458149, + 0.000152716, + 0.000305433, + 0, + -0.000152716, + 0, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000610865, + 0.000305433, + 0, + 0.000610865, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000610865, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + 0, + 0.000152716, + 0, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000458149, + 0.000610865, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000458149, + -0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0, + 0, + 0.000305433, + 0.000305433, + -0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0, + 0.000305433, + 0, + 0.000610865, + 0.000305433, + 0.000610865, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000305433, + 0, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0, + 0, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000610865, + 0.000458149, + 0, + 0.000305433, + 0, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + -0.000152716, + 0.000458149, + 0, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000305433, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000458149, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + 0.000458149, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000458149, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000610865, + 0.000458149, + 0, + 0.000458149, + 0.000152716, + 0.000610865, + 0, + 0.000610865, + 0, + 0, + 0.000458149, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000305433, + 0.000458149, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + -0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0, + 0, + 0.000152716, + -0.000152716, + 0, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + -0.000152716, + 0, + 0, + 0.000458149, + 0.000305433, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000305433, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000305433, + -0.000152716, + 0, + 0.000152716, + 0, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0.000458149, + 0.000305433, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000458149, + -0.000152716, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + -0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0, + 0, + 0, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + 0.000610865, + 0, + 0.000610865, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000610865, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0, + -0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000610865, + 0, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000458149}, + {-0.000152716, + -0.000610865, + 0, + -0.000152716, + 0.000305433, + 0, + 0.000152716, + -0.000152716, + 0, + 0, + 0.000152716, + 0, + -0.000152716, + 0, + 0, + -0.000305433, + 0, + 0, + 0, + 0.000305433, + -0.000152716, + -0.000152716, + 0, + -0.000152716, + -0.000458149, + -0.000152716, + 0, + 0.000152716, + -0.000305433, + -0.000152716, + -0.000152716, + 0, + 0.000305433, + 0.000152716, + 0, + -0.010079276, + -0.023060163, + -0.035888333, + -0.048716503, + -0.062308254, + -0.062308254, + -0.076052722, + -0.089491757, + -0.103083509, + -0.11667526, + -0.130877877, + -0.144775061, + -0.158824962, + -0.171805848, + -0.185550316, + -0.1853976, + -0.198989351, + -0.21227567, + -0.225256557, + -0.238237443, + -0.251218329, + -0.263435634, + -0.27519479, + -0.286953946, + -0.298254953, + -0.29794952, + -0.308792378, + -0.31948252, + -0.329867229, + -0.338572058, + -0.347582321, + -0.355829001, + -0.362853951, + -0.369573469, + -0.375529405, + -0.375987554, + -0.381485341, + -0.386066831, + -0.390037455, + -0.393244497, + -0.395687958, + -0.397367837, + -0.397978703, + -0.397978703, + -0.397825986, + -0.39767327, + -0.396146107, + -0.393855362, + -0.390953752, + -0.386983128, + -0.38194349, + -0.37614027, + -0.369268036, + -0.361479505, + -0.353080108, + -0.352927391, + -0.343306264, + -0.332157973, + -0.320246101, + -0.307570647, + -0.295353343, + -0.284357768, + -0.272140464, + -0.258701428, + -0.244193379, + -0.244346095, + -0.229074464, + -0.211970238, + -0.194560578, + -0.175929189, + -0.156381501, + -0.136070232, + -0.114689948, + -0.0926988, + -0.070249502, + -0.070249502, + -0.047647489, + -0.024281893, + -0.000458149, + 0.023365595, + 0.047494772, + 0.071013084, + 0.094531396, + 0.117591558, + 0.140040856, + 0.140193572, + 0.162337437, + 0.182801423, + 0.202959975, + 0.2209805, + 0.23793201, + 0.25366179, + 0.267100825, + 0.27931813, + 0.283899619, + 0.283899619, + 0.286953946, + 0.287870244, + 0.285884931, + 0.282525173, + 0.276721953, + 0.269086137, + 0.25946501, + 0.24785857, + 0.23380867, + 0.234114103, + 0.218537039, + 0.201280096, + 0.181732408, + 0.161268423, + 0.139429991, + 0.116064395, + 0.091935218, + 0.067195176, + 0.041844269, + 0.041996985, + 0.016035213, + -0.009162979, + -0.034513886, + -0.059406644, + -0.083535821, + -0.106290551, + -0.127823551, + -0.147676671, + -0.166308061, + -0.166002628, + -0.182648706, + -0.19654589, + -0.208610479, + -0.21807889, + -0.22510384, + -0.230143478, + -0.232128791, + -0.231976074, + -0.229379897, + -0.229227181, + -0.224492975, + -0.217162592, + -0.206625167, + -0.195171444, + -0.180968827, + -0.165391763, + -0.147523955, + -0.129045282, + -0.109497594, + -0.109039445, + -0.087811878, + -0.066584311, + -0.044898595, + -0.022602014, + -0.00122173, + 0.019394971, + 0.039400808, + 0.05833763, + 0.075594573, + 0.075136424, + 0.090713488, + 0.104610672, + 0.116064395, + 0.12538009, + 0.132099608, + 0.136070232, + 0.138055544, + 0.136375664, + 0.133168622, + 0.133474055, + 0.127823551, + 0.119424154, + 0.109344878, + 0.097127573, + 0.082924956, + 0.067806041, + 0.051159964, + 0.033750304, + 0.016035213, + 0.015882496, + -0.002290745, + -0.019547688, + -0.037110063, + -0.05253441, + -0.067806041, + -0.08109236, + -0.093156949, + -0.102319927, + -0.110108459, + -0.110566608, + -0.115911679, + -0.118660573, + -0.119424154, + -0.118049707, + -0.114079083, + -0.108275863, + -0.100181899, + -0.090102623, + -0.078648899, + -0.078954332, + -0.065668013, + -0.052076262, + -0.038026361, + -0.023823744, + -0.009773844, + 0.003970624, + 0.01695151, + 0.02855795, + 0.038789943, + 0.039095375, + 0.04718934, + 0.053908857, + 0.058795779, + 0.061391956, + 0.062002822, + 0.060322942, + 0.056810467, + 0.051465396, + 0.044745879, + 0.044593162, + 0.036499198, + 0.02687807, + 0.016493361, + 0.005650503, + -0.005345071, + -0.015882496, + -0.025961773, + -0.0355829, + -0.043829581, + -0.043676865, + -0.051007247, + -0.056352318, + -0.060628375, + -0.062460971, + -0.063377268, + -0.061391956, + -0.059101212, + -0.054519723, + -0.049021935, + -0.049021935, + -0.04260785, + -0.034972035, + -0.026725354, + -0.01863139, + -0.010537425, + -0.001985312, + 0.004581489, + 0.011301007, + 0.016187929, + 0.016187929, + 0.020463985, + 0.023212879, + 0.024892758, + 0.024740042, + 0.023365595, + 0.020616702, + 0.016798794, + 0.012522737, + 0.00702495, + 0.006872234, + 0.000916298, + -0.005039638, + -0.010995574, + -0.016646078, + -0.022143865, + -0.026419922, + -0.030390546, + -0.032986723, + -0.035124751, + -0.034666602, + -0.0355829, + -0.034819319, + -0.033139439, + -0.031001411, + -0.027488936, + -0.023823744, + -0.019547688, + -0.014508049, + -0.010690142, + -0.010690142, + -0.005955936, + -0.002290745, + 0.00122173, + 0.003665191, + 0.005650503, + 0.006872234, + 0.007330383, + 0.006566801, + 0.005192355, + 0.005345071, + 0.003512475, + 0.000763582, + -0.002596177, + -0.005192355, + -0.008552113, + -0.011911872, + -0.014966198, + -0.017256943, + -0.019547688, + -0.019394971, + -0.020922134, + -0.021533, + -0.022296581, + -0.021685716, + -0.020769418, + -0.019242255, + -0.017256943, + -0.015577064, + -0.013286319, + -0.013133603, + -0.010690142, + -0.008552113, + -0.006261369, + -0.004581489, + -0.00290161, + -0.002290745, + -0.001527163, + -0.001527163, + -0.001985312, + -0.001985312, + -0.002748894, + -0.003817908, + -0.005345071, + -0.006872234, + -0.008246681, + -0.010079276, + -0.011453723, + -0.012980886, + -0.0140499, + -0.013897184, + -0.014966198, + -0.015271631, + -0.01572978, + -0.015577064, + -0.014966198, + -0.014202617, + -0.013744468, + -0.012217305, + -0.011148291, + -0.011453723, + -0.010079276, + -0.008857546, + -0.007941248, + -0.00702495, + -0.006261369, + -0.00580322, + -0.005650503, + -0.005497787, + -0.005345071, + -0.005497787, + -0.005650503, + -0.006261369, + -0.006872234, + -0.007788532, + -0.008246681, + -0.009315695, + -0.010079276, + -0.010842858, + -0.011301007, + -0.010842858, + -0.011301007, + -0.011911872, + -0.012217305, + -0.012370021, + -0.011301007, + -0.011453723, + -0.010842858, + -0.010384709, + -0.010231993, + -0.00992656, + -0.009315695, + -0.00870483, + -0.008093964, + -0.008246681, + -0.007788532, + -0.007635815, + -0.007941248, + -0.007635815, + -0.007788532, + -0.007635815, + -0.007788532, + -0.008093964, + -0.008552113, + -0.009162979, + -0.009468411, + -0.00992656, + -0.00992656, + -0.010384709, + -0.010384709, + -0.010384709, + -0.010384709, + -0.010537425, + -0.010690142, + -0.010537425, + -0.010690142, + -0.010842858, + -0.010690142, + -0.010537425, + -0.010842858, + -0.010842858, + -0.010690142, + -0.01160644, + -0.011453723, + -0.011911872, + -0.011911872, + -0.012217305, + -0.012217305, + -0.012522737, + -0.012370021, + -0.012370021, + -0.012675454, + -0.012675454, + -0.012675454, + -0.012217305, + -0.011911872, + -0.012217305, + -0.012217305, + -0.011759156, + -0.01160644, + -0.011911872, + -0.011759156, + -0.011759156, + -0.011759156, + -0.01160644, + -0.011453723, + -0.011453723, + -0.011301007, + -0.01160644, + -0.01160644, + -0.011453723, + -0.011911872, + -0.011911872, + -0.012064588, + -0.012064588, + -0.012064588, + -0.012522737, + -0.012217305, + -0.012522737, + -0.012522737, + -0.012522737, + -0.012522737, + -0.012217305, + -0.012522737, + -0.012675454, + -0.012522737, + -0.012217305, + -0.012522737, + -0.012217305, + -0.012064588, + -0.012064588, + -0.012217305, + -0.011453723, + -0.01160644, + -0.011453723, + -0.011148291, + -0.010384709, + -0.010079276, + -0.009773844, + -0.009010262, + -0.009162979, + -0.009315695, + -0.00870483, + -0.009010262, + -0.008857546, + -0.00870483, + -0.008857546, + -0.009162979, + -0.009468411, + -0.009621128, + -0.00992656, + -0.009773844, + -0.010690142, + -0.010842858, + -0.010842858, + -0.011148291, + -0.011148291, + -0.011301007, + -0.011453723, + -0.01160644, + -0.011453723, + -0.011453723, + -0.011301007, + -0.011148291, + -0.011301007, + -0.010842858, + -0.010842858, + -0.010995574, + -0.010384709, + -0.010537425, + -0.010537425, + -0.010079276, + -0.010537425, + -0.010079276, + -0.009773844, + -0.009773844, + -0.010384709, + -0.010079276, + -0.010231993, + -0.010384709, + -0.00992656, + -0.010384709, + -0.010384709, + -0.011148291, + -0.010537425, + -0.010995574, + -0.010995574, + -0.010995574, + -0.011148291, + -0.011301007, + -0.01160644, + -0.011301007, + -0.011148291, + -0.011301007, + -0.011148291, + -0.011301007, + -0.011301007, + -0.010995574, + -0.010995574, + -0.010690142, + -0.010995574, + -0.010384709, + -0.011148291, + -0.010995574, + -0.010842858, + -0.010995574, + -0.010690142, + -0.010690142, + -0.010842858, + -0.010690142, + -0.010995574, + -0.010995574, + -0.010842858, + -0.010995574, + -0.010995574, + -0.010690142, + -0.011301007, + -0.010995574, + -0.011301007, + -0.011453723, + -0.011301007, + -0.01160644, + -0.011301007, + -0.011301007, + -0.011301007, + -0.011301007, + -0.01160644, + -0.011453723, + -0.011453723, + -0.011453723, + -0.011301007, + -0.011911872, + -0.011453723, + -0.011453723, + -0.011453723, + -0.011453723, + -0.01160644, + -0.011301007, + -0.011453723, + -0.010995574, + -0.011301007, + -0.011301007, + -0.011301007, + -0.011453723, + -0.011453723, + -0.011453723, + -0.01160644, + -0.01160644, + -0.01160644, + -0.01160644, + -0.01160644, + -0.011911872, + -0.011759156, + -0.011759156, + -0.012064588, + -0.011759156, + -0.01160644, + -0.011759156, + -0.012064588, + -0.011911872, + -0.012064588, + -0.011759156, + -0.011911872, + -0.01160644, + -0.011759156, + -0.011911872, + -0.011911872, + -0.011759156, + -0.011759156, + -0.011759156, + -0.011911872, + -0.011759156, + -0.011759156, + -0.011911872, + -0.011911872, + -0.012064588, + -0.012064588, + -0.011911872, + -0.012064588, + -0.011911872, + -0.012064588, + -0.011911872, + -0.011911872, + -0.012370021, + -0.011911872, + -0.011911872, + -0.012064588, + -0.012370021, + -0.012217305, + -0.012217305, + -0.011911872, + -0.012370021, + -0.012064588, + -0.012522737, + -0.012064588, + -0.012064588, + -0.012217305, + -0.012370021, + -0.012370021, + -0.012675454, + -0.012217305, + -0.012370021, + -0.012522737, + -0.012370021, + -0.012370021, + -0.012370021, + -0.012522737, + -0.012522737, + -0.012217305, + -0.012370021, + -0.012675454, + -0.012370021, + -0.012370021, + -0.012370021, + -0.012522737, + -0.012675454, + -0.012522737, + -0.012522737, + -0.012217305, + -0.012980886, + -0.012522737, + -0.012522737, + -0.012522737, + -0.012675454, + -0.012522737, + -0.012675454, + -0.01282817, + -0.012675454, + -0.01282817, + -0.01282817, + -0.012980886, + -0.012522737, + -0.012675454, + -0.012675454, + -0.012675454, + -0.01282817, + -0.01282817, + -0.012675454, + -0.012980886, + -0.01282817, + -0.01282817, + -0.012980886, + -0.01282817, + -0.013133603, + -0.012675454, + -0.012980886, + -0.012980886, + -0.012980886, + -0.012675454, + -0.01282817, + -0.012980886, + -0.012980886, + -0.012980886, + -0.012980886, + -0.01282817, + -0.01282817, + -0.013286319, + -0.013286319, + -0.012675454, + -0.012980886, + -0.013286319, + -0.013439035, + -0.012980886, + -0.013133603, + -0.013133603, + -0.013286319, + -0.013591752, + -0.012980886, + -0.013286319, + -0.013439035, + -0.013133603, + -0.013439035, + -0.013286319, + -0.013591752, + -0.013286319, + -0.013591752, + -0.013744468, + -0.013286319, + -0.013591752, + -0.013744468, + -0.013744468, + -0.013439035, + -0.013591752, + -0.013744468, + -0.013897184, + -0.0140499, + -0.014355333, + -0.013897184, + -0.0140499, + -0.0140499, + -0.014202617, + -0.014202617, + -0.0140499, + -0.014202617, + -0.014355333, + -0.0140499, + -0.014202617, + -0.014202617, + -0.014202617, + -0.0140499, + -0.014202617, + -0.0140499, + -0.014202617, + -0.014355333, + -0.014508049, + -0.0140499, + -0.014202617, + -0.0140499, + -0.014202617, + -0.014202617, + -0.0140499, + -0.014508049, + -0.014202617, + -0.014355333, + -0.014355333, + -0.014508049, + -0.014508049, + -0.014355333, + -0.014202617, + -0.014355333, + -0.014355333, + -0.0140499, + -0.014508049, + -0.014355333, + -0.014660766, + -0.014202617, + -0.014813482, + -0.014813482, + -0.014508049, + -0.014355333, + -0.014813482, + -0.014813482, + -0.014813482, + -0.014813482, + -0.015118915, + -0.014660766, + -0.014813482, + -0.014813482, + -0.014660766, + -0.015118915, + -0.014813482, + -0.014813482, + -0.015118915, + -0.014813482, + -0.014966198, + -0.014966198, + -0.014813482, + -0.015118915, + -0.014966198, + -0.014966198, + -0.015118915, + -0.014813482, + -0.014813482, + -0.014813482, + -0.014813482, + -0.014660766, + -0.014966198, + -0.014813482, + -0.014966198, + -0.014813482, + -0.015271631, + -0.015424347, + -0.014966198, + -0.014966198, + -0.014813482, + -0.015118915, + -0.015577064, + -0.015118915, + -0.015118915, + -0.015577064, + -0.015271631, + -0.015271631, + -0.015424347, + -0.015424347, + -0.01572978, + -0.015118915, + -0.015271631, + -0.015424347, + -0.015577064, + -0.015424347, + -0.015577064, + -0.015577064, + -0.015424347, + -0.015882496, + -0.015577064, + -0.015577064, + -0.015882496, + -0.016035213, + -0.015577064, + -0.015424347, + -0.01572978, + -0.015577064, + -0.015882496, + -0.015577064, + -0.01572978, + -0.015882496, + -0.015577064, + -0.01572978, + -0.015882496, + -0.015882496, + -0.016035213, + -0.01572978, + -0.015882496, + -0.01572978, + -0.015882496, + -0.016035213, + -0.016187929, + -0.016340645, + -0.016187929, + -0.016187929, + -0.016035213, + -0.016035213, + -0.016187929, + -0.015882496, + -0.015882496, + -0.016187929, + -0.015882496, + -0.016035213, + -0.016340645, + -0.016187929, + -0.016646078, + -0.016340645, + -0.016646078, + -0.016340645, + -0.016187929, + -0.016187929, + -0.016340645, + -0.016493361, + -0.016187929, + -0.016493361, + -0.016340645, + -0.016493361, + -0.016340645, + -0.016340645, + -0.016493361, + -0.016340645, + -0.016646078, + -0.016340645, + -0.016493361, + -0.016646078, + -0.01695151, + -0.016646078, + -0.016646078, + -0.016798794, + -0.016646078, + -0.016798794, + -0.016340645, + -0.016798794, + -0.016646078, + -0.016798794, + -0.01695151, + -0.016798794, + -0.017104227, + -0.016493361, + -0.016646078, + -0.01695151, + -0.01695151, + -0.017256943, + -0.016798794, + -0.016798794, + -0.017104227, + -0.017104227, + -0.017104227, + -0.017256943, + -0.017256943, + -0.017256943, + -0.01695151, + -0.017104227, + -0.01695151, + -0.01695151, + -0.016798794, + -0.017256943, + -0.017256943, + -0.017256943, + -0.017104227, + -0.01695151, + -0.017104227, + -0.017256943, + -0.017104227, + -0.017256943, + -0.017562376, + -0.017409659, + -0.017562376, + -0.017562376, + -0.017409659, + -0.017409659, + -0.017562376, + -0.017562376, + -0.017715092, + -0.017715092, + -0.017715092, + -0.017715092, + -0.017867808, + -0.017867808, + -0.017562376, + -0.017867808, + -0.018020525, + -0.017715092, + -0.017409659, + -0.017867808, + -0.017715092, + -0.018020525, + -0.018020525, + -0.018173241, + -0.018325957, + -0.018173241, + -0.018020525, + -0.018020525, + -0.018020525, + -0.018020525, + -0.018020525, + -0.018020525, + -0.018020525, + -0.018020525, + -0.018173241, + -0.018173241, + -0.018173241, + -0.017867808, + -0.018325957, + -0.018325957, + -0.018173241, + -0.018020525, + -0.018478673, + -0.018325957, + -0.018173241, + -0.018173241, + -0.018478673, + -0.018173241, + -0.01863139, + -0.018478673, + -0.018478673, + -0.018325957, + -0.018784106, + -0.018478673, + -0.018478673, + -0.01863139, + -0.01863139, + -0.01863139, + -0.018478673, + -0.01863139, + -0.018784106, + -0.01863139, + -0.018784106, + -0.018784106, + -0.018936822, + -0.018936822, + -0.018784106, + -0.018936822, + -0.019089539, + -0.018784106, + -0.018784106, + -0.019089539, + -0.019089539, + -0.018936822, + -0.018936822, + -0.019242255, + -0.019089539, + -0.019089539, + -0.019242255, + -0.019089539, + -0.019089539, + -0.019089539, + -0.019089539, + -0.019089539, + -0.018784106, + -0.019242255, + -0.019242255, + -0.019394971, + -0.019089539, + -0.019242255, + -0.019394971, + -0.019547688, + -0.019242255, + -0.019394971, + -0.019394971, + -0.019547688, + -0.019700404, + -0.019700404, + -0.019547688, + -0.01985312, + -0.019394971, + -0.019394971, + -0.019547688, + -0.01985312, + -0.020005837, + -0.01985312, + -0.019700404, + -0.020005837, + -0.020005837, + -0.01985312, + -0.019700404, + -0.020005837, + -0.020005837, + -0.020158553, + -0.020005837, + -0.020005837, + -0.020311269, + -0.020005837, + -0.020311269, + -0.01985312, + -0.020311269, + -0.020005837, + -0.020311269, + -0.020005837, + -0.020158553, + -0.020311269, + -0.020311269, + -0.020158553, + -0.020005837, + -0.020311269, + -0.020005837, + -0.020463985, + -0.020463985, + -0.020616702, + -0.020463985, + -0.020311269, + -0.020463985, + -0.020311269, + -0.020769418, + -0.020616702, + -0.020616702, + -0.020463985, + -0.020769418, + -0.020463985, + -0.020922134, + -0.020616702, + -0.020616702, + -0.020769418, + -0.020769418, + -0.021074851, + -0.020769418, + -0.020769418, + -0.021074851, + -0.020769418, + -0.020922134, + -0.021227567, + -0.021074851, + -0.020922134, + -0.021227567, + -0.021227567, + -0.021074851, + -0.021380283, + -0.021380283, + -0.021074851, + -0.021074851, + -0.021380283, + -0.021074851, + -0.021685716, + -0.020922134, + -0.021685716, + -0.021380283, + -0.021380283, + -0.021685716, + -0.021685716, + -0.021227567, + -0.021533, + -0.021533, + -0.021685716, + -0.021838432, + -0.021838432, + -0.021838432, + -0.022143865, + -0.021685716, + -0.021685716, + -0.021991149, + -0.021838432, + -0.021991149, + -0.021685716, + -0.021838432, + -0.021991149, + -0.021685716, + -0.022296581, + -0.021991149, + -0.021991149, + -0.022296581, + -0.022449298, + -0.021838432, + -0.021991149, + -0.022143865, + -0.022143865, + -0.022143865, + -0.022296581, + -0.022602014, + -0.022143865, + -0.022296581, + -0.022449298, + -0.022449298, + -0.022449298, + -0.022143865, + -0.022602014, + -0.022449298, + -0.02275473, + -0.02275473, + -0.022907446, + -0.022602014, + -0.02275473, + -0.023060163, + -0.022296581, + -0.022602014, + -0.02275473, + -0.022602014, + -0.023060163, + -0.02275473, + -0.022907446, + -0.02275473, + -0.022907446, + -0.023060163, + -0.023212879, + -0.022907446, + -0.023060163, + -0.023212879, + -0.023365595, + -0.023365595, + -0.023212879, + -0.023365595, + -0.023365595, + -0.023365595, + -0.023365595, + -0.023671028, + -0.023671028, + -0.023365595, + -0.023212879, + -0.023212879, + -0.023518312, + -0.023518312, + -0.023976461, + -0.023823744, + -0.023671028, + -0.023365595, + -0.023823744, + -0.023823744, + -0.023823744, + -0.023976461, + -0.023671028, + -0.023671028, + -0.023823744, + -0.023823744, + -0.024129177, + -0.023823744, + -0.024281893, + -0.024281893, + -0.023823744, + -0.024129177, + -0.023976461, + -0.024281893, + -0.02443461, + -0.024281893, + -0.024281893, + -0.024281893, + -0.024281893, + -0.02443461, + -0.02443461, + -0.024587326, + -0.024587326, + -0.024740042, + -0.02443461, + -0.024740042, + -0.024740042, + -0.024892758, + -0.024892758, + -0.024892758, + -0.024892758, + -0.024892758, + -0.024587326, + -0.025045475, + -0.024892758, + -0.024892758, + -0.024892758, + -0.024892758, + -0.025045475, + -0.02565634, + -0.025198191, + -0.025198191, + -0.025198191, + -0.02565634, + -0.025350907, + -0.025350907, + -0.025350907, + -0.025503624, + -0.025503624, + -0.025350907, + -0.025350907, + -0.025350907, + -0.02565634, + -0.025809056, + -0.025350907, + -0.025809056, + -0.02565634, + -0.025503624, + -0.025809056, + -0.02565634, + -0.025809056, + -0.02565634, + -0.02565634, + -0.025961773, + -0.025809056, + -0.025961773, + -0.026267205, + -0.026114489, + -0.025961773, + -0.026267205, + -0.026267205, + -0.026114489, + -0.026572638, + -0.026572638, + -0.026419922, + -0.026419922, + -0.026267205, + -0.026572638, + -0.026267205, + -0.026572638, + -0.026419922, + -0.02687807, + -0.026419922, + -0.026267205, + -0.026419922, + -0.027030787, + -0.027030787, + -0.027030787, + -0.02687807, + -0.027030787, + -0.026725354, + -0.027030787, + -0.026725354, + -0.027030787, + -0.027336219, + -0.027030787, + -0.027030787, + -0.027183503, + -0.02687807, + -0.027183503, + -0.027183503, + -0.027030787, + -0.027488936, + -0.027488936, + -0.027183503, + -0.027183503, + -0.027488936, + -0.027183503, + -0.027641652, + -0.027488936, + -0.027641652, + -0.027641652, + -0.027794368, + -0.027947085, + -0.027488936, + -0.027947085, + -0.027794368, + -0.027947085, + -0.027794368, + -0.027947085, + -0.028099801, + -0.028099801, + -0.027794368, + -0.028405234, + -0.027947085, + -0.027947085, + -0.028099801, + -0.028252517, + -0.028252517, + -0.028099801, + -0.028252517, + -0.028252517, + -0.028405234, + -0.028099801, + -0.028099801, + -0.028252517, + -0.027947085, + -0.02855795, + -0.028252517, + -0.028252517, + -0.02855795, + -0.028099801, + -0.028252517, + -0.028405234, + -0.028405234, + -0.028710666, + -0.02855795, + -0.02855795, + -0.02855795, + -0.02855795, + -0.02855795, + -0.029168815, + -0.029016099, + -0.028863383, + -0.029016099, + -0.029016099, + -0.028863383, + -0.029016099, + -0.02855795, + -0.029321531, + -0.028863383, + -0.029321531, + -0.029321531, + -0.029168815, + -0.029168815, + -0.029321531, + -0.029168815, + -0.029626964, + -0.029321531, + -0.029321531, + -0.02977968, + -0.02977968, + -0.030085113, + -0.030237829, + -0.030237829, + -0.029932397, + -0.030237829, + -0.030237829, + -0.030237829, + -0.030390546, + -0.030237829, + -0.030237829, + -0.030085113, + -0.030237829, + -0.030543262, + -0.030543262, + -0.030543262, + -0.030543262, + -0.030695978, + -0.030695978, + -0.030695978, + -0.030543262, + -0.030695978, + -0.030543262, + -0.030848695, + -0.031001411, + -0.030848695, + -0.031306843, + -0.031306843, + -0.030695978, + -0.031001411, + -0.031001411, + -0.031154127, + -0.031306843, + -0.031154127, + -0.031154127, + -0.03145956, + -0.03145956, + -0.031154127, + -0.031154127, + -0.031154127, + -0.03145956, + -0.031154127, + -0.031612276, + -0.031764992, + -0.031764992, + -0.031306843, + -0.031764992, + -0.031764992, + -0.032375858, + -0.031917709, + -0.032070425, + -0.031917709, + -0.032070425, + -0.031764992, + -0.032223141, + -0.032223141, + -0.031764992, + -0.032528574, + -0.031764992, + -0.032375858, + -0.032528574, + -0.032528574, + -0.03268129, + -0.032375858, + -0.03268129, + -0.032834007, + -0.03268129, + -0.03268129, + -0.032834007, + -0.032528574, + -0.03268129, + -0.032986723, + -0.033139439, + -0.033139439, + -0.033139439, + -0.033597588, + -0.032986723, + -0.033139439, + -0.032986723, + -0.033444872, + -0.032986723, + -0.033444872, + -0.033597588, + -0.033444872, + -0.033444872, + -0.033597588, + -0.033597588, + -0.033903021, + -0.034055737, + -0.034208453, + -0.033750304, + -0.033903021, + -0.034208453, + -0.033444872, + -0.03436117, + -0.034055737, + -0.034055737, + -0.034208453, + -0.03436117, + -0.034055737, + -0.03436117, + -0.034513886, + -0.034666602, + -0.034819319, + -0.034513886, + -0.034666602, + -0.034972035, + -0.034666602, + -0.034819319, + -0.034819319, + -0.034819319, + -0.034972035, + -0.035124751, + -0.034972035, + -0.035124751, + -0.035124751, + -0.035277468, + -0.035430184, + -0.035124751, + -0.035430184, + -0.035277468, + -0.035430184, + -0.035888333, + -0.035430184, + -0.0355829, + -0.036041049, + -0.0355829, + -0.0355829, + -0.036193765, + -0.035735616, + -0.035735616, + -0.036193765, + -0.036041049, + -0.035735616, + -0.035888333, + -0.036346482, + -0.036193765, + -0.036346482, + -0.036193765, + -0.036499198, + -0.036499198, + -0.036499198, + -0.036499198, + -0.036499198, + -0.036957347, + -0.036346482, + -0.036957347, + -0.03726278, + -0.036804631, + -0.03726278, + -0.037110063, + -0.03726278, + -0.03726278, + -0.03726278, + -0.03726278, + -0.03726278, + -0.037415496, + -0.037415496, + -0.037568212, + -0.037415496, + -0.037720928, + -0.037720928, + -0.037568212, + -0.037720928, + -0.037873645, + -0.038026361, + -0.037873645, + -0.038331794, + -0.038331794, + -0.038179077, + -0.03848451, + -0.038331794, + -0.038637226, + -0.038331794, + -0.038026361, + -0.038637226, + -0.038637226, + -0.038789943, + -0.038789943, + -0.038789943, + -0.038789943, + -0.038789943, + -0.038942659, + -0.038637226, + -0.038789943, + -0.038942659, + -0.039248092, + -0.039095375, + -0.039095375, + -0.039400808, + -0.039095375, + -0.039553524, + -0.039553524, + -0.03970624, + -0.039553524, + -0.039400808, + -0.039553524, + -0.039553524, + -0.039553524, + -0.040011673, + -0.039858957, + -0.040011673, + -0.040011673, + -0.040164389, + -0.040469822, + -0.040469822, + -0.040164389, + -0.040469822, + -0.040469822, + -0.040469822, + -0.040622538, + -0.040622538, + -0.040775255, + -0.040775255, + -0.041233404, + -0.040775255, + -0.040927971, + -0.041233404, + -0.041233404, + -0.041233404, + -0.041080687, + -0.04138612, + -0.041233404, + -0.04138612, + -0.041538836, + -0.041538836, + -0.041538836, + -0.041844269, + -0.041996985, + -0.041844269, + -0.041996985, + -0.041844269, + -0.042302418, + -0.042149701, + -0.042455134, + -0.042455134, + -0.042455134, + -0.042455134, + -0.042455134, + -0.042455134, + -0.04260785, + -0.043065999, + -0.042760567, + -0.042760567, + -0.042760567, + -0.043065999, + -0.043065999, + -0.043065999, + -0.043065999, + -0.043524148, + -0.043218716, + -0.043218716, + -0.043676865, + -0.043371432, + -0.043676865, + -0.043676865, + -0.043524148, + -0.043982297, + -0.043829581, + -0.043982297, + -0.043982297, + -0.043982297, + -0.043982297, + -0.044135013, + -0.044135013, + -0.044135013, + -0.044593162, + -0.044440446, + -0.044898595, + -0.044745879, + -0.044745879, + -0.045051311, + -0.044745879, + -0.044593162, + -0.044898595, + -0.044898595, + -0.044898595, + -0.045204028, + -0.045204028, + -0.045356744, + -0.045356744, + -0.04550946, + -0.045662177, + -0.045662177, + -0.045662177, + -0.045967609, + -0.046273042, + -0.046120325, + -0.045967609, + -0.046120325, + -0.046425758, + -0.046273042, + -0.046578474, + -0.046273042, + -0.046578474, + -0.046425758, + -0.046425758, + -0.046425758, + -0.046578474, + -0.046731191, + -0.046731191, + -0.04718934, + -0.047342056, + -0.046883907, + -0.047036623, + -0.047342056, + -0.047647489, + -0.047494772, + -0.047647489, + -0.047342056, + -0.047342056, + -0.047647489, + -0.047647489, + -0.047952921, + -0.048258354, + -0.04841107, + -0.047952921, + -0.048258354, + -0.048105638, + -0.04841107, + -0.048716503, + -0.048258354, + -0.048563786, + -0.048869219, + -0.048716503, + -0.048716503, + -0.049174652, + -0.048869219, + -0.048869219, + -0.049174652, + -0.049327368, + -0.049174652, + -0.049174652, + -0.049480084, + -0.049938233, + -0.049632801, + -0.049785517, + -0.050243666, + -0.049632801, + -0.050396382, + -0.049938233, + -0.05009095, + -0.050243666, + -0.050243666, + -0.050396382, + -0.050854531, + -0.050549098, + -0.050854531, + -0.050701815, + -0.050549098, + -0.051007247, + -0.051007247, + -0.050854531, + -0.051159964, + -0.05131268, + -0.05131268, + -0.051159964, + -0.05131268, + -0.051618113, + -0.051770829, + -0.051923545, + -0.051770829, + -0.052381694, + -0.052076262, + -0.052381694, + -0.052381694, + -0.052381694, + -0.05253441, + -0.052228978, + -0.052839843, + -0.052687127, + -0.052992559, + -0.052839843, + -0.053145276, + -0.053145276, + -0.052992559, + -0.053297992, + -0.053603425, + -0.053145276, + -0.053450708, + -0.053603425, + -0.053756141, + -0.053603425, + -0.053756141, + -0.053756141, + -0.054061574, + -0.05421429, + -0.054061574, + -0.05421429, + -0.054519723, + -0.054672439, + -0.054672439, + -0.054519723, + -0.054672439, + -0.054825155, + -0.054672439, + -0.055130588, + -0.05543602, + -0.055130588, + -0.055130588, + -0.055283304, + -0.055283304, + -0.05543602, + -0.05543602, + -0.055741453, + -0.055588737, + -0.056199602, + -0.056199602, + -0.056199602, + -0.056046886, + -0.056199602, + -0.056505035, + -0.056199602, + -0.056352318, + -0.056657751, + -0.056657751, + -0.057268616, + -0.056963183, + -0.056963183, + -0.057268616, + -0.057421332, + -0.057421332, + -0.057574049, + -0.057268616, + -0.057879481, + -0.057726765, + -0.057879481, + -0.058184914, + -0.058490347, + -0.058184914, + -0.05833763, + -0.05833763, + -0.058490347, + -0.058490347, + -0.058490347, + -0.058948495, + -0.058795779, + -0.059101212, + -0.059101212, + -0.059253928, + -0.059253928, + -0.059406644, + -0.059406644, + -0.059253928, + -0.059712077, + -0.059559361, + -0.059864793, + -0.060322942, + -0.059864793, + -0.060475659, + -0.060475659, + -0.060475659, + -0.060475659, + -0.060628375, + -0.060628375, + -0.060781091, + -0.06123924, + -0.060933808, + -0.060933808, + -0.06123924, + -0.06123924, + -0.061391956, + -0.061697389, + -0.061544673, + -0.061850105, + -0.061850105, + -0.062155538, + -0.062308254, + -0.061850105, + -0.062460971, + -0.062460971, + -0.062613687, + -0.062613687, + -0.062766403, + -0.063071836, + -0.063071836, + -0.063071836, + -0.063224552, + -0.063377268, + -0.063377268, + -0.063377268, + -0.063835417, + -0.063988134, + -0.063835417, + -0.063835417, + -0.064293566, + -0.064293566, + -0.064293566, + -0.064446283, + -0.064446283, + -0.064446283, + -0.064904432, + -0.065057148, + -0.065057148, + -0.065209864, + -0.065209864, + -0.06536258, + -0.065820729, + -0.06536258, + -0.065668013, + -0.065820729, + -0.065973446, + -0.066431595, + -0.065973446, + -0.066126162, + -0.066737027, + -0.066431595, + -0.066737027, + -0.066584311, + -0.066431595, + -0.066889744, + -0.06704246, + -0.067347893, + -0.06704246, + -0.067347893, + -0.067500609, + -0.067806041, + -0.068111474, + -0.067500609, + -0.068111474, + -0.068111474, + -0.068416907, + -0.068416907, + -0.068722339, + -0.069027772, + -0.068722339, + -0.068875056, + -0.069485921, + -0.069027772, + -0.069333205, + -0.069485921, + -0.069333205, + -0.069638637, + -0.070249502, + -0.070096786, + -0.069791353, + -0.070096786, + -0.070402219, + -0.070249502, + -0.070402219, + -0.070707651, + -0.070554935, + -0.070707651, + -0.0711658, + -0.070860368, + -0.071318517, + -0.071318517, + -0.071776665, + -0.071318517, + -0.072082098, + -0.072082098, + -0.072082098, + -0.072082098, + -0.072234814, + -0.072540247, + -0.072692963, + -0.072692963, + -0.072540247, + -0.072692963, + -0.073151112, + -0.073303829, + -0.073456545, + -0.073609261, + -0.073456545, + -0.073609261, + -0.073761978, + -0.074220126, + -0.074220126, + -0.074372843, + -0.074525559, + -0.074525559, + -0.074525559, + -0.074678275, + -0.075289141, + -0.075136424, + -0.075136424, + -0.075441857, + -0.075900006, + -0.075594573, + -0.075900006, + -0.076052722, + -0.076205438, + -0.075900006, + -0.076052722, + -0.076510871, + -0.076816304, + -0.076816304, + -0.077121736, + -0.077121736, + -0.077274453, + -0.077274453, + -0.077121736, + -0.077579885, + -0.077885318, + -0.077732602, + -0.077885318, + -0.078038034, + -0.078343467, + -0.078343467, + -0.078648899, + -0.078801616, + -0.078343467, + -0.078801616, + -0.079107048, + -0.079259765, + -0.079107048, + -0.079717914, + -0.079717914, + -0.079717914, + -0.080023346, + -0.080176063, + -0.080328779, + -0.080786928, + -0.080328779, + -0.080634211, + -0.081245077, + -0.080939644, + -0.081245077, + -0.08109236, + -0.081397793, + -0.081550509, + -0.081550509, + -0.082161375, + -0.081855942, + -0.082161375, + -0.082619523, + -0.082619523, + -0.082924956, + -0.082466807, + -0.082924956, + -0.082924956, + -0.083077672, + -0.083383105, + -0.083383105, + -0.083688538, + -0.083688538, + -0.084146687, + -0.084146687, + -0.084299403, + -0.084757552, + -0.084757552, + -0.084757552, + -0.085062984, + -0.084910268, + -0.085215701, + -0.085368417, + -0.085826566, + -0.085826566, + -0.085979282, + -0.085826566, + -0.086284715, + -0.086284715, + -0.086590148, + -0.086742864, + -0.086437431, + -0.08689558, + -0.087506445, + -0.087506445, + -0.087353729, + -0.087659162, + -0.087811878, + -0.088270027, + -0.088270027, + -0.088422743, + -0.088728176, + -0.088728176, + -0.088728176, + -0.088880892, + -0.088422743, + -0.089186325, + -0.089339041, + -0.089644474, + -0.089491757, + -0.089491757, + -0.089949906, + -0.090408055, + -0.090255339, + -0.090713488, + -0.090560772, + -0.090866204, + -0.090713488, + -0.091171637, + -0.091629786, + -0.091782502, + -0.091782502, + -0.092087935, + -0.091782502, + -0.0926988, + -0.092393367, + -0.092393367, + -0.092546084, + -0.092851516, + -0.093156949, + -0.093004233, + -0.093462381, + -0.093309665, + -0.093767814, + -0.093767814, + -0.09392053, + -0.094225963, + -0.094531396, + -0.094531396, + -0.094836828, + -0.094836828, + -0.094836828, + -0.095142261, + -0.09560041, + -0.095753126, + -0.09560041, + -0.095753126, + -0.095753126, + -0.096211275, + -0.096669424, + -0.096974857, + -0.09682214, + -0.096974857, + -0.097280289, + -0.097280289, + -0.097127573, + -0.097433005, + -0.097738438, + -0.097891154, + -0.097891154, + -0.09850202, + -0.098654736, + -0.09850202, + -0.098960169, + -0.099112885, + -0.099265601, + -0.099265601, + -0.099876466, + -0.099265601, + -0.099876466, + -0.100181899, + -0.100181899, + -0.100640048, + -0.100792764, + -0.101250913, + -0.100792764, + -0.101250913, + -0.101098197, + -0.101709062, + -0.102014495, + -0.102014495, + -0.102472644, + -0.101861778, + -0.102472644, + -0.102930793, + -0.102778076, + -0.102778076, + -0.103388942, + -0.103388942, + -0.103388942, + -0.10384709, + -0.10384709, + -0.104305239, + -0.104305239, + -0.104763388, + -0.104763388, + -0.104916105, + -0.105068821, + -0.105374254, + -0.10552697, + -0.105679686, + -0.105679686, + -0.106137835, + -0.106443268, + -0.106595984, + -0.106290551, + -0.106443268, + -0.106901417, + -0.107054133, + -0.107359566, + -0.107664998, + -0.107512282, + -0.107664998, + -0.10842858, + -0.10842858, + -0.10842858, + -0.108886729, + -0.108734012, + -0.108886729, + -0.109497594, + -0.109344878, + -0.10965031, + -0.10965031, + -0.109955743, + -0.110108459, + -0.110108459, + -0.110719324, + -0.110872041, + -0.111024757, + -0.111177473, + -0.11133019, + -0.112093771, + -0.111788339, + -0.112246488, + -0.11255192, + -0.112399204, + -0.112704636, + -0.112857353, + -0.112704636, + -0.113010069, + -0.113468218, + -0.113315502, + -0.114079083, + -0.114079083, + -0.113926367, + -0.114384516, + -0.114689948, + -0.114537232, + -0.114842665, + -0.115148097, + -0.115148097, + -0.11545353, + -0.115758963, + -0.115606246, + -0.115758963, + -0.116064395, + -0.116369828, + -0.116522544, + -0.116522544, + -0.116827977, + -0.117133409, + -0.117133409, + -0.117744275, + -0.118049707, + -0.118049707, + -0.117896991, + -0.11835514, + -0.11835514, + -0.118660573, + -0.119118721, + -0.118966005, + -0.119424154, + -0.11957687, + -0.119882303, + -0.120187736, + -0.120340452, + -0.120035019, + -0.120493168, + -0.120340452, + -0.120798601, + -0.12125675, + -0.121409466, + -0.121867615, + -0.122020331, + -0.122020331, + -0.122325764, + -0.12247848, + -0.12247848, + -0.122936629, + -0.123089345, + -0.123242062, + -0.123700211, + -0.123700211, + -0.124311076, + -0.124311076, + -0.124311076, + -0.124463792, + -0.124769225, + -0.125074658, + -0.12538009, + -0.12538009, + -0.12538009, + -0.125838239, + -0.125990955, + -0.126296388, + -0.126296388, + -0.126601821, + -0.126754537, + -0.126907253, + -0.127518118, + -0.127670835, + -0.127976267, + -0.128128984, + -0.1282817, + -0.128434416, + -0.128434416, + -0.128892565, + -0.129197998, + -0.12950343, + -0.12950343, + -0.129961579, + -0.130114296, + -0.130267012, + -0.130877877, + -0.130725161, + -0.13118331, + -0.130725161, + -0.13118331, + -0.131946891, + -0.131794175, + -0.132252324, + -0.132557757, + -0.132557757, + -0.133015906, + -0.132863189, + -0.132863189, + -0.133015906, + -0.133474055, + -0.133932203, + -0.133932203, + -0.134543069, + -0.134543069, + -0.134848501, + -0.135001218, + -0.135001218, + -0.135459367, + -0.13530665, + -0.135917515, + -0.135764799, + -0.136528381, + -0.136528381, + -0.136833813, + -0.137139246, + -0.137139246, + -0.137291962, + -0.137444679, + -0.137444679, + -0.137750111, + -0.138360976, + -0.138666409, + -0.138819125, + -0.138819125, + -0.139429991, + -0.139582707, + -0.139277274, + -0.139582707, + -0.139735423, + -0.140499005, + -0.140040856, + -0.140346288, + -0.140957154, + -0.141262586, + -0.141262586, + -0.141568019, + -0.141873452, + -0.141720735, + -0.141720735, + -0.142026168, + -0.142484317, + -0.142484317, + -0.142789749, + -0.143247898, + -0.143247898, + -0.143706047, + -0.143706047, + -0.143553331, + -0.14401148, + -0.14401148, + -0.144316913, + -0.144316913, + -0.144622345, + -0.145080494, + -0.145385927, + -0.145385927, + -0.145538643, + -0.145996792, + -0.145691359, + -0.146302225, + -0.146607657, + -0.146607657, + -0.146760373, + -0.147218522, + -0.147371239, + -0.147523955, + -0.147676671, + -0.147829388, + -0.147829388, + -0.147982104, + -0.148287537, + -0.148592969, + -0.149203834, + -0.149051118, + -0.149051118, + -0.149356551, + -0.1498147, + -0.149661983, + -0.150272849, + -0.150272849, + -0.150120132, + -0.150578281, + -0.151189146, + -0.15103643, + -0.151189146, + -0.151494579, + -0.151800012, + -0.151952728, + -0.152105444, + -0.151952728, + -0.152563593, + -0.152410877, + -0.152410877, + -0.153174458, + -0.153327175, + -0.153632607, + -0.15393804, + -0.153632607, + -0.15393804, + -0.154396189, + -0.154090756, + -0.154701622, + -0.155159771, + -0.155312487, + -0.154854338, + -0.155465203, + -0.155770636, + -0.155770636, + -0.155617919, + -0.156381501, + -0.156228785, + -0.156381501, + -0.156686934, + -0.156992366, + -0.156992366, + -0.157603231, + -0.157603231, + -0.157755948, + -0.15806138, + -0.15806138, + -0.158519529, + -0.158519529, + -0.158824962, + -0.158977678, + -0.159130395, + -0.159283111, + -0.159435827, + -0.159283111, + -0.159893976, + -0.160046692, + -0.160199409, + -0.160504841, + -0.160504841, + -0.16096299, + -0.160810274, + -0.16096299, + -0.161421139, + -0.161421139, + -0.161573856, + -0.161879288, + -0.162490153, + -0.162337437, + -0.162490153, + -0.162795586, + -0.163101019, + -0.163101019, + -0.163253735, + -0.163253735, + -0.163559168, + -0.163559168, + -0.164017316, + -0.164170033, + -0.164475465, + -0.164780898, + -0.164933614, + -0.164933614, + -0.165391763, + -0.165239047, + -0.165239047, + -0.165391763, + -0.165697196, + -0.166155345, + -0.166155345, + -0.166613494, + -0.166460777, + -0.166460777, + -0.166613494, + -0.16676621, + -0.167071643, + -0.167682508, + -0.167835224, + -0.167682508, + -0.167682508, + -0.168140657, + -0.168140657, + -0.168293373, + -0.168598806, + -0.168904238, + -0.168751522, + -0.168751522, + -0.169209671, + -0.169362387, + -0.169362387, + -0.16966782, + -0.169973253, + -0.169973253, + -0.170278685, + -0.170278685, + -0.170125969, + -0.170278685, + -0.170584118, + -0.170431401, + -0.171194983, + -0.171042267, + -0.171194983, + -0.171347699, + -0.171500416, + -0.171500416, + -0.171653132, + -0.171805848, + -0.172111281, + -0.172263997, + -0.172263997, + -0.172722146, + -0.17256943, + -0.172874862, + -0.173027579, + -0.172874862, + -0.172874862, + -0.173485728, + -0.173333011, + -0.173638444, + -0.173638444, + -0.17379116, + -0.174096593, + -0.174402026, + -0.173943877, + -0.173943877, + -0.174096593, + -0.174554742, + -0.174707458, + -0.174707458, + -0.174860174, + -0.174860174, + -0.175165607, + -0.175165607, + -0.175318323, + -0.175165607, + -0.175623756, + -0.17547104, + -0.175776472, + -0.175776472, + -0.176081905, + -0.176234621, + -0.175929189, + -0.176387338, + -0.176540054, + -0.176540054, + -0.176540054, + -0.176540054, + -0.17669277, + -0.17669277, + -0.177150919, + -0.176998203, + -0.177150919, + -0.177150919, + -0.177609068, + -0.177456352, + -0.177456352, + -0.177609068, + -0.177609068, + -0.177761784, + -0.177761784, + -0.178067217, + -0.178219933, + -0.178219933, + -0.178525366, + -0.178067217, + -0.178678082, + -0.178830798, + -0.178525366, + -0.178678082, + -0.178525366, + -0.178830798, + -0.178678082, + -0.179288947, + -0.178983515, + -0.179136231, + -0.179288947, + -0.178983515, + -0.179288947, + -0.178983515, + -0.179441664, + -0.17959438, + -0.179747096, + -0.179441664, + -0.17959438, + -0.179899813, + -0.179747096, + -0.180205245, + -0.179747096, + -0.180205245, + -0.17959438, + -0.179899813, + -0.180205245, + -0.180357962, + -0.180357962, + -0.180357962, + -0.180663394, + -0.180510678, + -0.180357962, + -0.180205245, + -0.180510678, + -0.180663394, + -0.180510678, + -0.180816111, + -0.180816111, + -0.180816111, + -0.181121543, + -0.180968827, + -0.180816111, + -0.181274259, + -0.180663394, + -0.180968827, + -0.180663394, + -0.181274259, + -0.181121543, + -0.181274259, + -0.181121543, + -0.180968827, + -0.180968827, + -0.180968827, + -0.180968827, + -0.181274259, + -0.181426976, + -0.181426976, + -0.181274259}, + {-0.000152716, + -0.000152716, + -0.000152716, + 0, + 0.000152716, + 0, + -0.000305433, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000305433, + 0, + 0, + 0, + 0, + 0.000152716, + -0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + -0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000763582, + 0.000763582, + 0.000763582, + 0.000763582, + 0.001069014, + 0.000916298, + 0.001069014, + 0.00122173, + 0.001527163, + 0.001832596, + 0.002443461, + 0.002290745, + 0.002596177, + 0.00290161, + 0.003359759, + 0.003512475, + 0.003665191, + 0.003970624, + 0.004581489, + 0.004886922, + 0.005497787, + 0.00580322, + 0.00580322, + 0.006719518, + 0.006872234, + 0.007330383, + 0.007635815, + 0.008246681, + 0.008552113, + 0.00870483, + 0.009621128, + 0.010079276, + 0.009621128, + 0.009773844, + 0.010690142, + 0.010842858, + 0.010995574, + 0.010995574, + 0.011301007, + 0.011453723, + 0.011759156, + 0.01160644, + 0.011301007, + 0.01160644, + 0.01160644, + 0.011453723, + 0.011301007, + 0.010995574, + 0.011148291, + 0.010842858, + 0.010537425, + 0.010384709, + 0.010690142, + 0.00992656, + 0.00992656, + 0.009010262, + 0.008857546, + 0.008093964, + 0.007483099, + 0.006719518, + 0.006108652, + 0.005345071, + 0.005192355, + 0.004276057, + 0.003359759, + 0.002443461, + 0.001527163, + 0.000763582, + -0.000305433, + -0.00122173, + -0.002138028, + -0.002748894, + -0.002748894, + -0.003970624, + -0.004734206, + -0.005497787, + -0.006108652, + -0.006872234, + -0.007483099, + -0.008399397, + -0.008857546, + -0.009162979, + -0.00870483, + -0.009162979, + -0.009468411, + -0.009621128, + -0.009468411, + -0.009468411, + -0.009468411, + -0.009162979, + -0.009010262, + -0.008857546, + -0.00870483, + -0.008552113, + -0.007788532, + -0.007788532, + -0.006872234, + -0.006719518, + -0.00580322, + -0.005345071, + -0.004581489, + -0.003970624, + -0.003512475, + -0.003207043, + -0.002290745, + -0.001527163, + -0.000610865, + 0, + 0.000458149, + 0.001527163, + 0.002138028, + 0.00290161, + 0.002748894, + 0.003665191, + 0.003970624, + 0.005039638, + 0.005497787, + 0.005955936, + 0.006108652, + 0.005955936, + 0.006566801, + 0.006566801, + 0.006872234, + 0.00702495, + 0.006872234, + 0.006566801, + 0.006566801, + 0.006414085, + 0.005650503, + 0.005192355, + 0.005192355, + 0.004734206, + 0.005039638, + 0.003817908, + 0.003665191, + 0.003054326, + 0.002290745, + 0.001679879, + 0.001069014, + 0.000152716, + -0.000458149, + -0.000458149, + -0.00122173, + -0.001374447, + -0.001832596, + -0.002443461, + -0.002748894, + -0.003207043, + -0.003512475, + -0.003512475, + -0.003817908, + -0.003970624, + -0.003665191, + -0.00412334, + -0.00412334, + -0.003817908, + -0.003512475, + -0.003512475, + -0.00290161, + -0.002748894, + -0.002290745, + -0.001832596, + -0.001374447, + -0.001069014, + -0.000610865, + -0.000305433, + 0.000305433, + 0.001069014, + 0.001527163, + 0.001985312, + 0.001985312, + 0.002596177, + 0.002290745, + 0.003054326, + 0.003207043, + 0.003207043, + 0.003054326, + 0.003359759, + 0.003512475, + 0.003054326, + 0.003207043, + 0.00290161, + 0.002443461, + 0.002443461, + 0.002138028, + 0.001679879, + 0.001374447, + 0.000916298, + 0.000763582, + 0.000152716, + -0.000458149, + -0.000305433, + -0.000763582, + -0.001069014, + -0.001374447, + -0.00122173, + -0.001374447, + -0.002290745, + -0.002138028, + -0.001832596, + -0.001832596, + -0.001832596, + -0.001832596, + -0.001527163, + -0.001679879, + -0.001069014, + -0.001069014, + -0.000305433, + -0.000152716, + 0, + 0.000305433, + 0.000610865, + 0.000763582, + 0.001069014, + 0.00122173, + 0.001527163, + 0.001832596, + 0.001679879, + 0.002138028, + 0.001679879, + 0.001985312, + 0.001527163, + 0.001679879, + 0.001527163, + 0.000916298, + 0.001527163, + 0.000610865, + 0.000916298, + 0.000305433, + 0.000152716, + -0.000610865, + -0.000610865, + -0.000305433, + -0.000610865, + -0.000458149, + -0.000763582, + -0.000916298, + -0.001069014, + -0.000763582, + -0.000610865, + -0.000458149, + -0.000305433, + -0.000763582, + 0, + -0.000305433, + -0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000916298, + 0.000916298, + 0.000610865, + 0.00122173, + 0.000916298, + 0.001069014, + 0.000763582, + 0.000763582, + 0.000763582, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + -0.000152716, + 0, + -0.000458149, + -0.000305433, + -0.000152716, + -0.000610865, + 0, + -0.000610865, + -0.000610865, + -0.000610865, + -0.000305433, + 0, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000458149, + 0, + 0.000763582, + 0.000305433, + 0.000763582, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + -0.000152716, + -0.000305433, + 0, + -0.000458149, + 0, + -0.000152716, + 0, + 0, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000610865, + 0.000763582, + 0.000152716, + 0.000458149, + 0.000458149, + 0, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000458149, + 0, + 0, + 0.000305433, + 0, + 0.000152716, + -0.000305433, + 0, + -0.000152716, + 0, + 0.000152716, + -0.000152716, + 0, + 0, + 0, + 0.000305433, + 0, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000305433, + 0, + 0, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + -0.000305433, + 0.000305433, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000610865, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000458149, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + -0.000152716, + 0, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0.000458149, + 0, + -0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + 0, + 0.000458149, + 0, + 0, + 0.000305433, + 0, + 0.000152716, + -0.000152716, + 0.000152716, + 0, + 0, + 0, + 0, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0, + 0.000305433, + 0, + 0, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000458149, + -0.000152716, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0, + 0.000152716, + 0, + 0.000305433, + -0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + 0, + 0, + 0, + 0, + 0.000610865, + 0.000458149, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0, + 0.000763582, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0, + -0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000305433, + 0, + 0, + 0.000305433, + 0.000152716, + 0, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000305433, + -0.000152716, + 0.000458149, + 0.000305433, + 0.000458149, + 0, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + -0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000763582, + 0, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000610865, + 0.000458149, + -0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000610865, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000152716, + -0.000152716, + 0, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000763582, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000305433, + -0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + -0.000152716, + 0.000610865, + 0.000305433, + 0.000305433, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000152716, + 0, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000458149, + 0.000305433, + 0.000458149, + 0, + 0.000458149, + -0.000152716, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000152716, + 0.000152716, + -0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000610865, + 0, + 0.000458149, + 0, + 0.000305433, + 0.000305433, + 0, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000458149, + 0, + 0, + 0.000152716, + -0.000152716, + -0.000152716, + 0.000152716, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000458149, + 0, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0, + 0, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000610865, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + -0.000152716, + 0.000458149, + 0, + 0.000305433, + 0, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000458149, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + -0.000152716, + 0.000152716, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000763582, + 0, + 0.000458149, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000610865, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000458149, + 0, + 0, + 0.000152716, + 0.000152716, + 0.000152716, + 0.000152716, + 0, + 0.000152716, + 0, + 0.000305433, + 0.000152716, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0, + 0.000458149, + 0, + 0, + 0.000152716, + 0, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000458149, + 0, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000305433, + 0, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000305433, + 0.000458149, + 0, + 0.000152716, + 0, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000458149, + 0, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000152716, + 0, + 0.000763582, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000152716, + 0, + 0.000458149, + 0.000305433, + 0, + 0.000458149, + 0.000610865, + 0, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000763582, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000152716, + 0.000152716, + 0.000610865, + 0.000458149, + 0.000458149, + 0, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000305433, + 0, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000763582, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000763582, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000458149, + 0.000152716, + 0.000763582, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000763582, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000763582, + 0.000152716, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000916298, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000152716, + 0.000152716, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000763582, + 0.000152716, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000152716, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000152716, + 0.000763582, + 0.000152716, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000916298, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000305433, + 0.000458149, + 0.000152716, + 0.000610865, + 0.000152716, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000152716, + 0.000610865, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000305433, + 0.000152716, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000763582, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000152716, + 0, + 0.000610865, + 0.000916298, + 0.000610865, + 0.000152716, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000305433, + 0.000763582, + 0.000610865, + 0.000152716, + 0.000763582, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000305433, + 0.000305433, + 0.000763582, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000305433, + 0.000458149, + 0.000305433, + 0.000305433, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000305433, + 0.000610865, + 0.000763582, + 0.000458149, + 0.000763582, + 0.000458149, + 0.000916298, + 0.000763582, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000916298, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000305433, + 0.000610865, + 0.000916298, + 0.000305433, + 0.000916298, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000152716, + 0.000305433, + 0.000152716, + 0.000763582, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000305433, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000763582, + 0.000610865, + 0.000763582, + 0.000458149, + 0.000458149, + 0.000763582, + 0.000763582, + 0.000305433, + 0.000916298, + 0.000305433, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000916298, + 0.000763582, + 0.000763582, + 0.000458149, + 0.000763582, + 0.000916298, + 0.000610865, + 0.000610865, + 0.000763582, + 0.000916298, + 0.000763582, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000305433, + 0.000610865, + 0.000610865, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000610865, + 0.000458149, + 0.000305433, + 0.000763582, + 0.000763582, + 0.000458149, + 0.000763582, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000763582, + 0.000916298, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000458149, + 0.000458149, + 0.000458149, + 0.000763582, + 0.000458149, + 0.000610865, + 0.000610865, + 0.000916298, + 0.000763582, + 0.000458149, + 0.000610865, + 0.000916298, + 0.000458149, + 0.000610865, + 0.000458149, + 0.001069014, + 0.000458149, + 0.00122173, + 0.001069014, + 0.000458149, + 0.000458149, + 0.001069014, + 0.000610865, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000916298, + 0.000152716, + 0.000916298, + 0.000610865, + 0.000763582, + 0.000916298, + 0.000916298, + 0.000610865, + 0.000305433, + 0.001069014, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000305433, + 0.000763582, + 0.000763582, + 0.000916298, + 0.000610865, + 0.000458149, + 0.000763582, + 0.000458149, + 0.000458149, + 0.000610865, + 0.000305433, + 0.000763582, + 0.001069014, + 0.000763582, + 0.001069014, + 0.000763582, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000763582, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000916298, + 0.000610865, + 0.000916298, + 0.000763582, + 0.000458149, + 0.000763582, + 0.001069014, + 0.000916298, + 0.000763582, + 0.000763582, + 0.000763582, + 0.000916298, + 0.000916298, + 0.000916298, + 0.000763582, + 0.000610865, + 0.000458149, + 0.000610865, + 0.000763582, + 0.000610865, + 0.000916298, + 0.000916298, + 0.000610865, + 0.000916298, + 0.000916298, + 0.000763582, + 0.000458149, + 0.000763582, + 0.000458149, + 0.000458149, + 0.000916298, + 0.000610865, + 0.000610865, + 0.000916298, + 0.000458149, + 0.000763582, + 0.000763582, + 0.000916298, + 0.000458149, + 0.000763582, + 0.000610865, + 0.000610865, + 0.000610865, + 0.000916298, + 0.000610865, + 0.00122173, + 0.000610865, + 0.000916298, + 0.000610865, + 0.000763582, + 0.000916298, + 0.000610865, + 0.000916298, + 0.000763582, + 0.000610865, + 0.000916298, + 0.000916298, + 0.000763582, + 0.000916298, + 0.000916298, + 0.000763582, + 0.000763582, + 0.000763582, + 0.000916298, + 0.000763582, + 0.000610865, + 0.000610865, + 0.000916298, + 0.000763582, + 0.000916298, + 0.000916298, + 0.000763582, + 0.000763582, + 0.000916298, + 0.001069014, + 0.000458149, + 0.001069014, + 0.000916298, + 0.001069014, + 0.001069014, + 0.001069014, + 0.000916298, + 0.00122173, + 0.000916298, + 0.000916298, + 0.001069014, + 0.001069014, + 0.000916298, + 0.000763582, + 0.000610865, + 0.000610865, + 0.000916298, + 0.00122173, + 0.000763582, + 0.000916298, + 0.000916298, + 0.000916298, + 0.00122173, + 0.001069014, + 0.000916298, + 0.000763582, + 0.000763582, + 0.000763582, + 0.000916298, + 0.000916298, + 0.000916298, + 0.000916298, + 0.000916298, + 0.000916298, + 0.000763582, + 0.001069014, + 0.000916298, + 0.000916298, + 0.00122173, + 0.001069014, + 0.000763582, + 0.00122173, + 0.001069014, + 0.00122173, + 0.000916298, + 0.000916298, + 0.000916298, + 0.000763582, + 0.000763582, + 0.000610865, + 0.00122173, + 0.000916298, + 0.000763582, + 0.00122173, + 0.000916298, + 0.000916298, + 0.00122173, + 0.000916298, + 0.000763582, + 0.000916298, + 0.000763582, + 0.001069014, + 0.001374447, + 0.001374447, + 0.001069014, + 0.000916298, + 0.00122173, + 0.001069014, + 0.000916298, + 0.000610865, + 0.001069014, + 0.000763582, + 0.000763582, + 0.00122173, + 0.000916298, + 0.000763582, + 0.000916298, + 0.001069014, + 0.000763582, + 0.000763582, + 0.00122173, + 0.000916298, + 0.001069014, + 0.000763582, + 0.00122173, + 0.000763582, + 0.001069014, + 0.000610865, + 0.001069014, + 0.000916298, + 0.001069014, + 0.00122173, + 0.000763582, + 0.000916298, + 0.001069014, + 0.001069014, + 0.001069014, + 0.000916298, + 0.000916298, + 0.00122173, + 0.000916298, + 0.001069014, + 0.000916298, + 0.00122173, + 0.001069014, + 0.001069014, + 0.001069014, + 0.00122173, + 0.000763582, + 0.001069014, + 0.000763582, + 0.001374447, + 0.001069014, + 0.00122173, + 0.001374447, + 0.00122173, + 0.001069014, + 0.00122173, + 0.00122173, + 0.00122173, + 0.001374447, + 0.000916298, + 0.001374447, + 0.001069014, + 0.00122173, + 0.001069014, + 0.000763582, + 0.00122173, + 0.00122173, + 0.00122173, + 0.000916298, + 0.000916298, + 0.001069014, + 0.00122173, + 0.000916298, + 0.001374447, + 0.00122173, + 0.001069014, + 0.000916298, + 0.00122173, + 0.000763582, + 0.001069014, + 0.001069014, + 0.00122173, + 0.001069014, + 0.001069014, + 0.001069014, + 0.00122173, + 0.001069014, + 0.00122173, + 0.000916298, + 0.00122173, + 0.000916298, + 0.00122173, + 0.001069014, + 0.00122173, + 0.000916298, + 0.001527163, + 0.001069014, + 0.001374447, + 0.000916298, + 0.00122173, + 0.00122173, + 0.001374447, + 0.000916298, + 0.000916298, + 0.00122173, + 0.001069014, + 0.00122173, + 0.001374447, + 0.00122173, + 0.00122173, + 0.001374447, + 0.001374447, + 0.00122173, + 0.000916298, + 0.001069014, + 0.00122173, + 0.000763582, + 0.001069014, + 0.00122173, + 0.001069014, + 0.00122173, + 0.001069014, + 0.00122173, + 0.00122173, + 0.00122173, + 0.00122173, + 0.00122173, + 0.00122173, + 0.00122173, + 0.001527163, + 0.001374447, + 0.001374447, + 0.000916298, + 0.000916298, + 0.001374447, + 0.00122173, + 0.001527163, + 0.001374447, + 0.00122173, + 0.001374447, + 0.001374447, + 0.001374447, + 0.001374447, + 0.000916298, + 0.00122173, + 0.000916298, + 0.001527163, + 0.001374447, + 0.001527163, + 0.001527163, + 0.001679879, + 0.00122173, + 0.001374447, + 0.001069014, + 0.00122173, + 0.001374447, + 0.00122173, + 0.00122173, + 0.001069014, + 0.001527163, + 0.001527163, + 0.001069014, + 0.001069014, + 0.001374447, + 0.001069014, + 0.001069014, + 0.00122173, + 0.001527163, + 0.001069014, + 0.001374447, + 0.001527163, + 0.001527163, + 0.001069014, + 0.001374447, + 0.001679879, + 0.00122173, + 0.00122173, + 0.001374447, + 0.001069014, + 0.00122173, + 0.001527163, + 0.001374447, + 0.001374447, + 0.00122173, + 0.001527163, + 0.001527163, + 0.00122173, + 0.00122173, + 0.001527163, + 0.001527163, + 0.001374447, + 0.001527163, + 0.001374447, + 0.001374447, + 0.001527163, + 0.00122173, + 0.001527163, + 0.00122173, + 0.00122173, + 0.001527163, + 0.00122173, + 0.000763582, + 0.001374447, + 0.001527163, + 0.001679879, + 0.00122173, + 0.001374447, + 0.001527163, + 0.00122173, + 0.001527163, + 0.001832596, + 0.001679879, + 0.001832596, + 0.001527163, + 0.001374447, + 0.00122173, + 0.00122173, + 0.001374447, + 0.001527163, + 0.001374447, + 0.001069014, + 0.001679879, + 0.001527163, + 0.001069014, + 0.00122173, + 0.001527163, + 0.001374447, + 0.001679879, + 0.001679879, + 0.001679879, + 0.001374447, + 0.001527163, + 0.001679879, + 0.00122173, + 0.001832596, + 0.00122173, + 0.001374447, + 0.001679879, + 0.001374447, + 0.001679879, + 0.001527163, + 0.001374447, + 0.001527163, + 0.001679879, + 0.001527163, + 0.001527163, + 0.001527163, + 0.001374447, + 0.00122173, + 0.001527163, + 0.001374447, + 0.001679879, + 0.001679879, + 0.001832596, + 0.001832596, + 0.001374447, + 0.001832596, + 0.001374447, + 0.001069014, + 0.001527163, + 0.001679879, + 0.001527163, + 0.001679879, + 0.001832596, + 0.00122173, + 0.001527163, + 0.001679879, + 0.001527163, + 0.001527163, + 0.001679879, + 0.001374447, + 0.001832596, + 0.001374447, + 0.001374447, + 0.001832596, + 0.001832596, + 0.001374447, + 0.001679879, + 0.00122173, + 0.001832596, + 0.001527163, + 0.001832596, + 0.001679879, + 0.001679879, + 0.001985312, + 0.001527163, + 0.001679879, + 0.001374447, + 0.001985312, + 0.001679879, + 0.00122173, + 0.001679879, + 0.001679879, + 0.001832596, + 0.001527163, + 0.001985312, + 0.001679879, + 0.001832596, + 0.001527163, + 0.001374447, + 0.001679879, + 0.001527163, + 0.001679879, + 0.001679879, + 0.001679879, + 0.001527163, + 0.001832596, + 0.001832596, + 0.001374447, + 0.001832596, + 0.001679879, + 0.001527163, + 0.001679879, + 0.001832596, + 0.001985312, + 0.001527163, + 0.001679879, + 0.001679879, + 0.001679879, + 0.001679879, + 0.001832596, + 0.001679879, + 0.001527163, + 0.001832596, + 0.001985312, + 0.001527163, + 0.001832596, + 0.001527163, + 0.001985312, + 0.001679879, + 0.001832596, + 0.001679879, + 0.001832596, + 0.001679879, + 0.001679879, + 0.001527163, + 0.001832596, + 0.001985312, + 0.001679879, + 0.001832596, + 0.001527163, + 0.001985312, + 0.001679879, + 0.001832596, + 0.001527163, + 0.001832596, + 0.001985312, + 0.001832596, + 0.001679879, + 0.001679879, + 0.001832596, + 0.001679879, + 0.001832596, + 0.001527163, + 0.001985312, + 0.001374447, + 0.001832596, + 0.001527163, + 0.001679879, + 0.001985312, + 0.001985312, + 0.001832596, + 0.001679879, + 0.002290745, + 0.001832596, + 0.001985312, + 0.001832596, + 0.001679879, + 0.001832596, + 0.001679879, + 0.001679879, + 0.001985312, + 0.001832596, + 0.002138028, + 0.001985312, + 0.001985312, + 0.001985312, + 0.002138028, + 0.001832596, + 0.001985312, + 0.001985312, + 0.001985312, + 0.002443461, + 0.001832596, + 0.001985312, + 0.001832596, + 0.001832596, + 0.002290745, + 0.002290745, + 0.002290745, + 0.001985312, + 0.001985312, + 0.002138028, + 0.002290745, + 0.001985312, + 0.001832596, + 0.002443461, + 0.001985312, + 0.002138028, + 0.001832596, + 0.002443461, + 0.002290745, + 0.001985312, + 0.002138028, + 0.001985312, + 0.002138028, + 0.001679879, + 0.001985312, + 0.001832596, + 0.001985312, + 0.002138028, + 0.002290745, + 0.002138028, + 0.001985312, + 0.002138028, + 0.002138028, + 0.002138028, + 0.002138028, + 0.001985312, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002138028, + 0.002138028, + 0.001985312, + 0.002290745, + 0.002290745, + 0.002138028, + 0.002290745, + 0.002138028, + 0.002290745, + 0.002138028, + 0.001985312, + 0.002290745, + 0.001832596, + 0.002138028, + 0.002290745, + 0.002443461, + 0.002443461, + 0.002138028, + 0.002596177, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002138028, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002138028, + 0.002290745, + 0.002748894, + 0.002443461, + 0.002138028, + 0.002290745, + 0.002138028, + 0.002138028, + 0.002138028, + 0.002443461, + 0.002290745, + 0.002290745, + 0.002596177, + 0.002290745, + 0.002138028, + 0.002596177, + 0.002596177, + 0.002596177, + 0.001985312, + 0.002138028, + 0.002138028, + 0.002443461, + 0.002443461, + 0.002290745, + 0.002443461, + 0.002290745, + 0.002596177, + 0.002596177, + 0.002138028, + 0.002443461, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002290745, + 0.002596177, + 0.002596177, + 0.002290745, + 0.002443461, + 0.002443461, + 0.002290745, + 0.002443461, + 0.002748894, + 0.002443461, + 0.002748894, + 0.002138028, + 0.002748894, + 0.002138028, + 0.002443461, + 0.002596177, + 0.002290745, + 0.002596177, + 0.002596177, + 0.002290745, + 0.002748894, + 0.002748894, + 0.002596177, + 0.002443461, + 0.002748894, + 0.002596177, + 0.002443461, + 0.002290745, + 0.002443461, + 0.002596177, + 0.002748894, + 0.002748894, + 0.002596177, + 0.002596177, + 0.002596177, + 0.002596177, + 0.002290745, + 0.002596177, + 0.002748894, + 0.002596177, + 0.002596177, + 0.002290745, + 0.002748894, + 0.002596177, + 0.00290161, + 0.002748894, + 0.002596177, + 0.002748894, + 0.002596177, + 0.002596177, + 0.00290161, + 0.002748894, + 0.002748894, + 0.003054326, + 0.002748894, + 0.002443461, + 0.002748894, + 0.002596177, + 0.002748894, + 0.003054326, + 0.003054326, + 0.002748894, + 0.002748894, + 0.003054326, + 0.002596177, + 0.00290161, + 0.002596177, + 0.002596177, + 0.00290161, + 0.002596177, + 0.002596177, + 0.002748894, + 0.003207043, + 0.002748894, + 0.002596177, + 0.00290161, + 0.00290161, + 0.002748894, + 0.00290161, + 0.00290161, + 0.002748894, + 0.002596177, + 0.003207043, + 0.00290161, + 0.002748894, + 0.003054326, + 0.003054326, + 0.003054326, + 0.002596177, + 0.002748894, + 0.003207043, + 0.003054326, + 0.00290161, + 0.00290161, + 0.002748894, + 0.003054326, + 0.003054326, + 0.003054326, + 0.003054326, + 0.003054326, + 0.00290161, + 0.00290161, + 0.003054326, + 0.00290161, + 0.002748894, + 0.003207043, + 0.00290161, + 0.00290161, + 0.003054326, + 0.003054326, + 0.003359759, + 0.003512475, + 0.003054326, + 0.003207043, + 0.003054326, + 0.00290161, + 0.003054326, + 0.00290161, + 0.00290161, + 0.003054326, + 0.003359759, + 0.003207043, + 0.00290161, + 0.003207043, + 0.003054326, + 0.003054326, + 0.003359759, + 0.003512475, + 0.00290161, + 0.003054326, + 0.002748894, + 0.003359759, + 0.003359759, + 0.003207043, + 0.003054326, + 0.003054326, + 0.003054326, + 0.00290161, + 0.003054326, + 0.003207043, + 0.00290161, + 0.003207043, + 0.003054326, + 0.00290161, + 0.003512475, + 0.003207043, + 0.003359759, + 0.003359759, + 0.00290161, + 0.003054326, + 0.003207043, + 0.003054326, + 0.003054326, + 0.003054326, + 0.003359759, + 0.00290161, + 0.003207043, + 0.003207043, + 0.003359759, + 0.003207043, + 0.003054326, + 0.003054326, + 0.003207043, + 0.003054326, + 0.003054326, + 0.003512475, + 0.003359759, + 0.003359759, + 0.003054326, + 0.003054326, + 0.003665191, + 0.003207043, + 0.003207043, + 0.003359759, + 0.003207043, + 0.003054326, + 0.003207043, + 0.003359759, + 0.003665191, + 0.003512475, + 0.003207043, + 0.003207043, + 0.003359759, + 0.003207043, + 0.003207043, + 0.003207043, + 0.003359759, + 0.003359759, + 0.003359759, + 0.003512475, + 0.003665191, + 0.003665191, + 0.003207043, + 0.003207043, + 0.003207043, + 0.003512475, + 0.003054326, + 0.003359759, + 0.003665191, + 0.003359759, + 0.003359759, + 0.003359759, + 0.003665191, + 0.003359759, + 0.003665191, + 0.003817908, + 0.003512475, + 0.003512475, + 0.003359759, + 0.003512475, + 0.003665191, + 0.003970624, + 0.003359759, + 0.003359759, + 0.003207043, + 0.003512475, + 0.003817908, + 0.003512475, + 0.003665191, + 0.003207043, + 0.003512475, + 0.003970624, + 0.003207043, + 0.003512475, + 0.003359759, + 0.003359759, + 0.003512475, + 0.003817908, + 0.003970624, + 0.003512475, + 0.003207043, + 0.003970624, + 0.003512475, + 0.003207043, + 0.003512475, + 0.003817908, + 0.003665191, + 0.003512475, + 0.003512475, + 0.003970624, + 0.003512475, + 0.003665191, + 0.003665191, + 0.003665191, + 0.003817908, + 0.003665191, + 0.003665191, + 0.003665191, + 0.003970624, + 0.003665191, + 0.003512475, + 0.003665191, + 0.003665191, + 0.003817908, + 0.003665191, + 0.003665191, + 0.003359759, + 0.003665191, + 0.003665191, + 0.003817908, + 0.003665191, + 0.003970624, + 0.003817908, + 0.003970624, + 0.003665191, + 0.003817908, + 0.003817908, + 0.003665191, + 0.003512475, + 0.003665191, + 0.003512475, + 0.003665191, + 0.003970624, + 0.003817908, + 0.003665191, + 0.003970624, + 0.003817908, + 0.003817908, + 0.003970624, + 0.003817908, + 0.003665191, + 0.00412334, + 0.003665191, + 0.003817908, + 0.003817908, + 0.00412334, + 0.003970624, + 0.003665191, + 0.003665191, + 0.003970624, + 0.003817908, + 0.003512475, + 0.003970624, + 0.003817908, + 0.003817908, + 0.003970624, + 0.003817908, + 0.003970624, + 0.003970624, + 0.003970624, + 0.003817908, + 0.003817908, + 0.003665191, + 0.003970624, + 0.003970624, + 0.003817908, + 0.003817908, + 0.003817908, + 0.003817908, + 0.003665191, + 0.00412334, + 0.003665191, + 0.003817908, + 0.003817908, + 0.00412334, + 0.00412334, + 0.003970624, + 0.003817908, + 0.003970624, + 0.003970624, + 0.004276057, + 0.003970624, + 0.003970624, + 0.003665191, + 0.003970624, + 0.003970624, + 0.003970624, + 0.003970624, + 0.003817908, + 0.00412334, + 0.003970624, + 0.003817908, + 0.003817908, + 0.003970624, + 0.003817908, + 0.003665191, + 0.003665191, + 0.003970624, + 0.003970624, + 0.003817908, + 0.00412334, + 0.003817908, + 0.00412334, + 0.003970624, + 0.00412334, + 0.003970624, + 0.003970624, + 0.003970624, + 0.003817908, + 0.004428773, + 0.003970624, + 0.003970624, + 0.00412334, + 0.003665191, + 0.003817908, + 0.00412334, + 0.004276057, + 0.00412334, + 0.003970624, + 0.00412334, + 0.003817908, + 0.003817908, + 0.003970624, + 0.003817908, + 0.003970624, + 0.003665191, + 0.003970624, + 0.003817908, + 0.003970624, + 0.003970624, + 0.00412334, + 0.003665191, + 0.003817908, + 0.003512475, + 0.004276057, + 0.004276057, + 0.003970624, + 0.003665191, + 0.003970624, + 0.003970624, + 0.003817908, + 0.003970624, + 0.00412334, + 0.003665191, + 0.003970624, + 0.003817908, + 0.003665191, + 0.003665191, + 0.003817908, + 0.003970624, + 0.003817908, + 0.004276057, + 0.004276057, + 0.003970624, + 0.003817908, + 0.00412334, + 0.00412334, + 0.003665191, + 0.003970624, + 0.00412334, + 0.004276057, + 0.004276057, + 0.003970624, + 0.003970624, + 0.004276057, + 0.00412334, + 0.003817908, + 0.003817908}}; + +const float MAGNETOMETER_DATA[MOTION_SENSOR_AXIS_NUM][IMU_DATA_SIZE] = { + {-32.48, -32.306, -32.422, -32.596, -32.306, -32.712, -32.712, -32.596, + -32.828, -32.712, -32.596, -32.48, -32.306, -32.828, -32.306, -32.422, + -32.306, -32.364, -32.248, -32.306, -32.596, -32.538, -32.828, -32.364, + -32.422, -32.422, -32.712, -32.828, -32.538, -32.77, -32.48, -32.596, + -32.422, -32.538, -32.538, -32.422, -32.306, -32.596, -33.002, -32.248, + -32.364, -32.77, -32.48, -32.48, -32.132, -32.306, -32.306, -32.074, + -32.016, -31.842, -32.074, -31.784, -32.132, -32.074, -31.842, -31.958, + -31.726, -31.552, -31.494, -31.436, -31.088, -31.32, -31.32, -31.32, + -30.914, -30.856, -31.03, -30.972, -30.74, -30.508, -30.682, -30.218, + -30.16, -30.16, -29.696, -29.754, -29.696, -29.174, -29.464, -29.348, + -28.826, -28.942, -29.174, -28.942, -28.362, -28.246, -28.304, -28.014, + -28.246, -28.304, -27.898, -27.608, -27.434, -27.434, -26.68, -27.144, + -27.144, -27.086, -27.318, -26.68, -27.434, -26.912, -26.564, -26.912, + -26.216, -26.448, -26.332, -26.158, -25.984, -26.332, -25.81, -26.216, + -26.1, -26.39, -26.158, -25.81, -26.39, -26.274, -26.042, -26.622, + -26.332, -26.506, -26.39, -26.564, -26.39, -26.796, -26.448, -26.796, + -26.854, -26.738, -27.434, -27.028, -27.086, -27.434, -28.014, -27.666, + -27.724, -27.898, -27.782, -27.898, -28.246, -28.42, -27.898, -28.188, + -28.072, -28.594, -28.594, -28.362, -28.652, -28.362, -28.594, -28.42, + -28.652, -28.768, -28.652, -28.304, -28.71, -28.768, -28.652, -28.71, + -28.13, -28.826, -28.478, -28.188, -28.188, -28.478, -27.84, -28.246, + -28.014, -27.898, -27.724, -27.724, -27.608, -27.434, -27.028, -26.97, + -26.97, -27.028, -26.912, -26.738, -26.854, -26.912, -26.622, -27.086, + -26.68, -27.202, -27.086, -26.912, -26.912, -27.202, -27.144, -26.738, + -26.912, -26.796, -26.738, -27.202, -27.086, -27.202, -27.318, -27.492, + -27.666, -27.434, -27.55, -27.492, -27.434, -27.434, -27.608, -27.318, + -27.376, -27.434, -27.666, -27.84, -28.072, -27.724, -27.724, -27.666, + -27.434, -27.55, -27.608, -27.608, -27.434, -27.26, -27.492, -27.434, + -27.144, -27.434, -27.376, -27.26, -26.97, -27.028, -27.028, -26.68, + -27.26, -26.796, -27.086, -26.97, -26.912, -26.68, -26.854, -26.39, + -26.97, -26.796, -27.028, -27.028, -27.376, -27.26, -27.086, -27.028, + -27.434, -27.55, -27.608, -27.724, -27.782, -27.086, -27.086, -27.086, + -27.492, -27.086, -27.202, -27.086, -27.144, -27.26, -27.55, -27.318, + -27.55, -27.028, -27.26, -27.318, -27.028, -27.144, -26.796, -27.028, + -27.028, -27.086, -26.97, -27.318, -27.434, -26.97, -27.028, -26.97, + -26.854, -26.912, -26.912, -26.912, -27.144, -26.912, -27.086, -27.492, + -27.086, -27.028, -27.086, -27.086, -27.318, -27.376, -26.97, -27.202, + -27.318, -27.144, -27.144, -26.796, -27.376, -27.144, -27.26, -27.028, + -27.434, -27.26, -27.26, -27.144, -26.97, -27.144, -27.144, -26.738, + -26.796, -26.68, -26.912, -27.202, -26.912, -26.97, -26.68, -26.854, + -27.55, -27.144, -27.086, -27.202, -27.028, -27.086, -27.26, -26.796, + -26.796, -27.028, -26.854, -26.912, -27.434, -27.086, -26.622, -27.318, + -26.912, -27.144, -26.796, -26.912, -26.796, -27.086, -26.506, -26.796, + -26.448, -27.028, -26.854, -26.738, -26.796, -27.028, -27.144, -26.68, + -26.564, -27.26, -27.144, -27.028, -26.912, -26.912, -26.622, -27.028, + -26.97, -26.39, -27.028, -27.086, -26.854, -26.796, -26.68, -26.506, + -26.68, -26.448, -26.506, -26.506, -26.622, -26.97, -26.854, -26.854, + -27.028, -26.448, -26.738, -26.738, -26.622, -26.274, -26.796, -26.854, + -26.912, -26.68, -26.854, -26.506, -26.68, -26.738, -26.854, -26.506, + -26.738, -26.622, -26.39, -27.144, -26.68, -26.506, -26.564, -26.97, + -26.448, -26.622, -26.332, -26.622, -26.39, -26.564, -26.796, -26.912, + -26.506, -26.448, -26.622, -26.738, -26.39, -26.332, -26.564, -26.854, + -26.274, -26.448, -26.68, -26.506, -26.332, -26.506, -26.332, -26.39, + -26.738, -26.39, -26.332, -26.274, -26.506, -26.216, -26.506, -26.042, + -26.332, -26.564, -26.216, -26.042, -26.216, -26.68, -26.738, -26.39, + -26.564, -26.738, -26.39, -26.274, -26.216, -26.448, -25.81, -26.158, + -25.926, -26.274, -26.216, -26.216, -26.39, -26.332, -26.274, -26.216, + -26.216, -26.1, -26.158, -26.39, -26.39, -26.1, -26.39, -26.274, + -26.216, -26.158, -25.81, -26.216, -26.158, -26.506, -26.274, -26.448, + -26.332, -26.274, -26.1, -26.1, -26.158, -26.042, -26.216, -26.216, + -26.042, -26.216, -26.332, -26.274, -26.042, -26.332, -26.274, -26.216, + -26.1, -26.448, -25.81, -25.984, -26.216, -26.39, -26.158, -26.564, + -25.694, -25.926, -25.868, -26.158, -26.1, -25.984, -25.984, -26.506, + -26.39, -26.042, -26.042, -25.984, -25.81, -26.216, -25.81, -26.39, + -25.984, -25.984, -26.332, -25.868, -26.158, -26.1, -26.042, -25.636, + -26.39, -26.1, -26.1, -25.868, -26.216, -25.984, -26.332, -25.868, + -26.158, -26.506, -26.042, -25.694, -25.926, -26.1, -25.752, -25.868, + -26.216, -25.926, -26.216, -25.984, -25.81, -25.81, -26.332, -25.868, + -25.926, -25.984, -26.1, -25.81, -26.39, -25.636, -26.332, -25.984, + -25.926, -25.868, -25.868, -25.694, -26.042, -26.274, -25.636, -25.81, + -25.984, -25.984, -26.1, -25.868, -26.1, -26.042, -26.042, -25.636, + -25.81, -26.042, -26.042, -25.52, -26.042, -25.694, -25.694, -25.752, + -26.1, -25.868, -26.1, -25.752, -25.984, -25.52, -25.984, -25.52, + -25.636, -25.81, -25.926, -25.288, -25.462, -25.404, -25.52, -25.926, + -25.752, -25.694, -25.462, -25.52, -25.636, -25.81, -25.752, -25.984, + -25.81, -25.636, -25.694, -25.462, -25.752, -25.984, -25.868, -25.462, + -25.636, -25.578, -25.404, -25.346, -25.52, -25.926, -25.694, -25.578, + -25.346, -25.636, -25.462, -25.404, -25.81, -25.52, -25.52, -25.346, + -25.636, -25.636, -25.462, -25.404, -25.172, -25.868, -25.056, -25.578, + -25.288, -25.114, -25.462, -25.404, -25.52, -25.404, -25.52, -25.404, + -25.462, -25.288, -25.868, -25.578, -25.346, -25.636, -25.694, -25.288, + -25.52, -25.346, -25.462, -25.404, -25.52, -25.056, -25.636, -25.23, + -25.578, -25.462, -25.23, -25.694, -25.462, -25.23, -25.462, -25.23, + -25.056, -25.462, -25.23, -25.114, -25.172, -25.346, -25.52, -25.288, + -25.288, -25.578, -25.404, -25.636, -25.288, -25.52, -25.288, -25.462, + -25.346, -25.462, -25.23, -25.636, -25.52, -25.23, -25.288, -25.288, + -24.882, -25.346, -25.056, -25.23, -25.346, -24.998, -25.172, -25.172, + -24.998, -24.708, -24.882, -24.998, -25.114, -24.998, -25.23, -24.766, + -24.882, -24.94, -24.94, -25.056, -25.23, -25.172, -25.404, -24.94, + -24.708, -24.708, -24.94, -24.824, -24.882, -24.65, -24.824, -24.708, + -24.998, -24.998, -24.824, -24.882, -25.056, -24.766, -24.998, -25.056, + -24.65, -25.172, -25.288, -24.882, -25.172, -24.94, -24.766, -24.708, + -25.056, -24.708, -24.708, -24.824, -25.114, -24.708, -24.65, -24.766, + -24.94, -25.172, -25.056, -24.65, -24.882, -24.998, -25.346, -24.824, + -25.114, -24.824, -24.882, -25.056, -24.65, -24.766, -24.534, -24.534, + -24.824, -24.882, -24.824, -25.056, -24.824, -24.766, -24.592, -24.476, + -24.882, -24.94, -24.824, -24.766, -24.94, -24.766, -24.766, -24.94, + -24.65, -24.708, -24.65, -24.766, -24.65, -24.882, -24.592, -24.94, + -24.766, -24.592, -24.534, -24.824, -24.418, -25.114, -24.708, -24.65, + -24.882, -24.882, -24.882, -24.824, -24.94, -24.36, -24.882, -24.824, + -24.824, -24.882, -24.766, -24.418, -24.592, -24.36, -24.418, -24.128, + -24.882, -24.766, -24.882, -24.534, -24.244, -24.186, -24.592, -24.65, + -24.766, -24.708, -24.592, -24.708, -24.766, -24.592, -24.302, -24.186, + -24.36, -24.534, -24.65, -24.592, -24.708, -24.418, -24.186, -24.418, + -24.418, -24.476, -24.244, -24.128, -24.766, -24.36, -24.128, -24.534, + -24.418, -24.244, -24.244, -24.244, -24.36, -24.476, -24.36, -24.36, + -24.244, -24.534, -24.418, -24.244, -23.896, -24.244, -24.07, -23.664, + -23.954, -24.244, -23.896, -24.36, -24.302, -24.36, -23.896, -24.302, + -24.592, -24.418, -24.012, -24.07, -23.838, -24.36, -24.186, -24.708, + -24.186, -23.838, -23.954, -24.07, -24.244, -24.07, -24.244, -24.186, + -24.186, -24.186, -24.186, -24.244, -23.722, -23.896, -23.664, -24.07, + -24.012, -24.012, -23.722, -24.302, -24.07, -23.896, -23.548, -24.186, + -23.49, -24.302, -23.954, -23.838, -23.954, -24.012, -23.78, -23.954, + -23.896, -24.012, -23.896, -23.954, -23.78, -23.722, -24.07, -23.954, + -23.49, -23.954, -23.838, -24.07, -24.07, -23.548, -23.664, -23.78, + -23.722, -23.954, -23.78, -23.432, -24.128, -23.78, -23.722, -23.432, + -23.896, -23.374, -23.316, -23.606, -24.012, -23.722, -23.49, -23.78, + -23.49, -23.78, -23.432, -23.664, -23.78, -23.664, -23.78, -23.374, + -23.258, -23.722, -23.374, -23.49, -23.432, -23.316, -23.316, -23.606, + -23.2, -23.548, -23.78, -23.664, -23.548, -23.606, -23.896, -23.432, + -23.258, -23.432, -23.316, -23.664, -23.432, -23.258, -23.258, -23.258, + -23.258, -23.316, -23.49, -23.258, -23.664, -23.374, -23.374, -23.432, + -23.142, -23.2, -23.084, -23.258, -23.142, -23.026, -22.272, -22.794, + -23.026, -22.91, -23.142, -23.374, -22.91, -22.968, -23.2, -22.794, + -23.316, -22.968, -23.432, -23.2, -22.794, -23.142, -23.026, -23.258, + -22.678, -22.91, -23.316, -22.736, -23.2, -22.62, -23.084, -23.316, + -22.91, -23.026, -22.562, -23.026, -23.084, -23.084, -23.2, -22.794, + -23.142, -22.736, -23.142, -23.026, -22.678, -23.316, -23.258, -22.968, + -22.794, -22.91, -22.62, -22.736, -22.736, -23.026, -22.794, -22.91, + -22.736, -22.736, -22.678, -22.446, -22.678, -23.084, -23.142, -22.562, + -23.084, -22.794, -22.504, -23.142, -22.852, -23.026, -22.388, -22.736, + -22.794, -22.562, -22.446, -22.852, -22.272, -22.91, -22.272, -22.794, + -22.736, -22.562, -22.678, -22.33, -22.736, -22.388, -22.446, -22.33, + -22.736, -22.33, -22.098, -22.388, -22.388, -22.156, -22.446, -22.272, + -22.272, -22.272, -22.214, -22.504, -22.156, -22.156, -22.62, -22.156, + -22.214, -22.214, -22.446, -22.504, -22.446, -22.156, -22.562, -22.446, + -22.736, -22.562, -22.33, -22.388, -22.388, -22.446, -22.388, -22.62, + -22.62, -22.388, -22.446, -22.504, -22.214, -22.272, -22.388, -22.272, + -22.272, -21.982, -22.678, -22.04, -22.214, -22.33, -22.156, -22.156, + -22.388, -22.562, -22.214, -22.33, -22.33, -22.04, -22.388, -22.33, + -22.33, -22.504, -22.156, -22.214, -22.388, -22.214, -21.866, -22.098, + -22.04, -21.924, -22.504, -22.678, -22.62, -22.214, -22.156, -22.04, + -22.562, -22.214, -22.156, -22.156, -21.866, -22.098, -22.388, -21.692, + -22.388, -21.808, -21.75, -22.04, -22.098, -21.866, -21.576, -22.156, + -22.098, -22.272, -21.808, -22.04, -22.04, -21.518, -21.866, -21.866, + -21.982, -21.866, -22.272, -22.04, -21.692, -21.808, -21.576, -22.04, + -21.576, -21.924, -21.692, -21.46, -21.576, -22.098, -21.866, -21.634, + -21.402, -21.518, -21.576, -21.634, -21.402, -21.75, -21.576, -21.576, + -21.75, -21.576, -21.692, -21.75, -21.634, -21.924, -21.46, -21.866, + -21.866, -21.402, -21.344, -21.402, -21.518, -21.634, -21.692, -21.576, + -21.808, -21.402, -21.634, -21.286, -21.46, -21.402, -21.46, -21.344, + -21.344, -21.402, -21.344, -21.75, -21.286, -21.344, -21.112, -21.402, + -21.402, -21.112, -21.17, -21.286, -21.228, -21.344, -21.286, -21.576, + -21.344, -21.17, -21.344, -20.938, -21.46, -21.344, -20.764, -20.938, + -21.344, -20.938, -21.112, -20.996, -21.054, -21.402, -21.17, -20.938, + -21.054, -21.112, -21.054, -20.996, -20.938, -21.054, -21.286, -20.822, + -21.344, -20.938, -20.764, -20.996, -20.996, -20.938, -21.344, -20.706, + -21.112, -20.764, -20.706, -20.764, -20.764, -20.706, -20.764, -20.648, + -20.88, -20.822, -20.648, -20.532, -20.648, -20.996, -20.648, -20.358, + -20.88, -20.822, -20.822, -20.59, -20.88, -20.764, -20.416, -20.706, + -20.764, -20.242, -20.706, -20.532, -20.706, -20.648, -20.416, -20.648, + -20.59, -20.416, -20.416, -20.59, -20.938, -20.706, -20.648, -20.474, + -20.3, -20.88, -20.416, -20.416, -20.474, -20.474, -20.532, -20.242, + -20.474, -20.416, -20.416, -20.532, -20.532, -20.648, -20.532, -20.706, + -20.184, -20.822, -20.416, -20.358, -20.648, -20.184, -20.3, -20.126, + -20.474, -20.822, -20.764, -20.184, -20.3, -20.416, -20.416, -20.358, + -20.3, -20.474, -20.59, -20.532, -20.184, -20.358, -19.836, -20.242, + -20.358, -20.01, -19.894, -20.358, -20.242, -20.358, -20.3, -20.242, + -20.532, -20.474, -20.01, -20.184, -20.242, -20.3, -20.3, -20.358, + -20.01, -20.3, -20.3, -19.72, -20.126, -20.126, -20.126, -20.416, + -20.068, -19.778, -19.894, -20.01, -19.662, -19.894, -19.952, -20.068, + -19.836, -19.546, -19.778, -19.604, -20.068, -20.01, -19.836, -20.126, + -19.72, -19.662, -19.72, -20.242, -19.952, -19.72, -20.184, -19.546, + -20.01, -19.662, -19.082, -19.894, -19.662, -19.604, -19.778, -19.778, + -19.952, -19.778, -19.662, -19.488, -19.894, -19.314, -19.894, -19.43, + -19.256, -19.372, -19.488, -19.662, -19.488, -19.604, -18.966, -19.43, + -19.43, -19.43, -19.314, -19.836, -19.372, -19.256, -19.43, -19.488, + -19.43, -19.43, -19.256, -19.43, -19.604, -19.198, -18.966, -19.198, + -18.966, -19.372, -19.082, -19.198, -19.314, -19.314, -19.024, -19.314, + -19.488, -18.85, -19.256, -19.14, -19.662, -18.966, -18.85, -19.082, + -19.14, -19.024, -19.198, -19.198, -18.85, -19.082, -18.85, -18.792, + -19.256, -19.082, -18.966, -19.256, -18.502, -18.676, -18.212, -19.024, + -18.676, -18.734, -18.27, -18.734, -18.676, -18.676, -18.966, -18.444, + -18.212, -18.792, -18.328, -18.618, -18.734, -18.56, -18.502, -18.444, + -18.56, -18.676, -18.27, -18.676, -18.56, -18.212, -18.444, -18.56, + -18.502, -18.27, -18.502, -18.444, -18.386, -18.328, -18.038, -18.27, + -18.154, -18.212, -18.444, -18.328, -18.27, -18.038, -18.038, -18.444, + -18.27, -18.328, -18.096, -18.038, -18.27, -17.98, -18.444, -17.806, + -18.038, -18.27, -18.038, -17.748, -18.212, -18.154, -17.806, -17.922, + -18.096, -17.98, -17.864, -17.922, -18.038, -17.806, -17.458, -17.864, + -17.632, -17.922, -17.69, -17.69, -17.922, -17.574, -17.98, -17.69, + -17.748, -17.748, -17.516, -17.632, -17.69, -17.69, -17.4, -17.516, + -17.69, -17.4, -17.574, -17.458, -17.226, -17.806, -17.226, -17.284, + -16.762, -17.052, -17.11, -17.458, -17.052, -17.226, -17.284, -17.574, + -17.284, -17.574, -17.226, -17.4, -17.632, -17.226, -17.226, -17.284, + -17.052, -17.748, -16.704, -17.11, -17.4, -17.4, -17.052, -17.168, + -17.284, -17.11, -16.936, -16.704, -16.646, -17.11, -17.284, -16.878, + -16.588, -16.878, -16.994, -16.878, -16.762, -16.704, -17.11, -16.414, + -16.704, -17.226, -16.24, -16.646, -16.762, -16.472, -16.414, -16.82, + -16.588, -16.646, -16.53, -16.414, -16.182, -16.878, -16.356, -16.414, + -16.53, -16.124, -16.182, -16.414, -16.182, -16.472, -16.182, -16.646, + -16.124, -16.182, -15.66, -15.834, -16.356, -16.356, -16.298, -15.892, + -16.066, -16.182, -16.24, -15.892, -15.95, -16.008, -15.66, -15.95, + -15.95, -15.834, -15.95, -15.718, -15.486, -16.356, -15.718, -15.834, + -15.834, -15.602, -15.66, -15.95, -16.008, -15.312, -15.718, -15.892, + -15.37, -15.776, -15.544, -15.544, -15.602, -15.544, -15.428, -15.544, + -15.544, -15.544, -15.428, -15.022, -15.08, -15.254, -14.906, -15.312, + -15.254, -15.312, -15.254, -15.08, -15.254, -15.138, -15.37, -15.022, + -15.254, -14.732, -14.964, -15.196, -15.138, -14.964, -15.196, -15.312, + -15.08, -15.022, -14.79, -14.964, -14.848, -14.906, -14.674, -14.848, + -14.906, -14.616, -14.732, -14.5, -14.964, -15.08, -14.674, -14.268, + -14.906, -14.21, -14.674, -14.384, -14.558, -14.732, -14.616, -14.5, + -14.268, -14.326, -14.152, -14.21, -14.616, -14.616, -14.384, -13.862, + -14.442, -14.094, -14.094, -14.094, -13.862, -13.862, -14.036, -13.978, + -13.688, -13.746, -13.862, -13.978, -13.862, -14.036, -13.746, -13.978, + -13.688, -13.688, -13.63, -13.92, -14.094, -13.804, -13.63, -14.094, + -13.804, -13.34, -13.34, -13.862, -13.398, -13.398, -13.804, -13.398, + -13.804, -13.804, -13.34, -13.572, -13.166, -13.572, -13.108, -13.572, + -13.398, -13.224, -13.398, -13.282, -13.514, -13.05, -12.876, -13.34, + -13.166, -13.688, -13.34, -13.108, -12.992, -12.818, -13.05, -12.644, + -12.992, -13.282, -12.818, -12.528, -12.47, -12.702, -12.238, -12.76, + -12.47, -12.354, -12.934, -12.702, -12.412, -12.702, -12.528, -12.644, + -12.702, -12.76, -12.47, -13.05, -12.47, -12.238, -12.238, -12.818, + -12.354, -12.238, -12.18, -12.238, -12.76, -12.18, -12.064, -12.296, + -12.006, -12.238, -12.064, -12.006, -12.122, -11.89, -12.064, -12.122, + -11.89, -11.774, -12.006, -12.006, -11.542, -11.89, -12.064, -11.774, + -11.948, -11.716, -11.658, -11.6, -11.716, -11.484, -11.89, -11.6, + -11.774, -11.368, -11.426, -11.6, -11.6, -11.774, -11.368, -11.832, + -11.368, -11.31, -11.542, -11.542, -11.31, -11.252, -11.136, -11.6, + -11.194, -11.252, -11.078, -11.426, -11.078, -11.194, -11.02, -10.73, + -11.31, -10.846, -11.02, -10.672, -10.962, -11.02, -10.846, -11.136, + -11.194, -10.788, -10.904, -10.208, -10.44, -10.672, -10.962, -10.788, + -10.208, -10.672, -11.078, -10.962, -10.382, -10.672, -10.498, -10.44, + -11.02, -10.092, -10.44, -10.672, -10.15, -10.324, -10.266, -10.324, + -10.266, -10.614, -10.382, -10.556, -9.976, -10.034, -9.802, -10.15, + -9.802, -9.918, -9.976, -9.744, -9.976, -9.976, -10.15, -10.15, + -9.918, -9.86, -9.454, -9.86, -9.628, -9.86, -9.744, -9.512, + -9.628, -9.512, -9.454, -9.686, -9.57, -9.512, -9.57, -9.106, + -8.99, -9.106, -9.28, -8.932, -8.7, -8.7, -9.048, -8.932, + -8.584, -9.048, -8.99, -9.222, -9.106, -8.816, -8.874, -8.99, + -8.874, -8.99, -8.874, -8.874, -8.816, -8.41, -8.642, -8.352, + -8.178, -8.468, -8.236, -8.004, -8.642, -8.294, -8.236, -8.41, + -8.294, -8.41, -8.468, -7.888, -8.352, -7.772, -7.656, -7.888, + -7.714, -7.83, -7.772, -8.178, -8.178, -7.888, -7.656, -7.366, + -7.946, -7.714, -7.54, -7.83, -7.656, -7.482, -7.482, -7.598, + -7.076, -7.192, -7.424, -7.308, -7.25, -7.018, -7.192, -6.844, + -6.902, -7.076, -6.844, -6.96, -6.554, -6.612, -6.844, -6.844, + -6.438, -6.612, -6.67, -7.134, -6.612, -6.496, -6.612, -6.496, + -6.38, -6.612, -5.974, -6.496, -6.322, -6.148, -6.264, -5.916, + -6.032, -5.8, -6.38, -6.148, -6.09, -6.206, -5.858, -5.568, + -5.974, -5.858, -5.974, -5.916, -5.916, -5.626, -5.858, -5.51, + -5.974, -5.568, -5.22, -5.452, -5.22, -5.51, -5.278, -5.278, + -5.336, -5.22, -5.278, -5.336, -5.046, -5.22, -4.814, -5.104, + -4.988, -4.872, -5.104, -4.756, -4.814, -4.64, -4.756, -4.756, + -4.524, -4.466, -4.756, -4.408, -4.176, -4.118, -4.234, -4.176, + -4.35, -4.35, -4.118, -4.118, -4.234, -4.524, -4.292, -4.118, + -4.06, -4.118, -3.944, -4.234, -3.596, -3.712, -3.828, -3.596, + -3.654, -3.886, -3.654, -3.538, -3.364, -3.48, -3.886, -3.248, + -3.422, -3.074, -3.19, -2.9, -3.248, -2.958, -3.364, -2.958, + -3.132, -2.784, -2.9, -2.494, -2.9, -2.9, -2.9, -2.494, + -2.668, -2.436, -2.436, -2.61, -2.204, -2.494, -2.32, -2.262, + -2.668, -1.798, -2.204, -2.262, -1.682, -1.624, -1.856, -1.624, + -1.45, -1.508, -1.392, -1.334, -1.392, -1.218, -1.218, -1.508, + -1.102, -1.566, -1.392, -1.334, -1.45, -0.986, -1.218, -0.986, + -1.45, -1.334, -0.986, -0.464, -0.638, -0.812, -0.638, -0.406, + -0.87, -0.58, -0.58, -0.638, -0.58, -0.174, -0.406, 0.058, + -0.406, -0.116, -0.174, -0.116, -0.174, -0.232, 0, 0.174, + -0.058, -0.058, -0.058, 0.29, 0.406, 0.29, 0.812, 0.696, + 0.348, 0.522, 0.638, 0.87, 0.696, 0.754, 0.696, 0.812, + 0.522, 0.986, 0.986, 0.812, 0.87, 1.044, 1.276, 1.334, + 1.276, 1.566, 1.392, 1.45, 1.566, 1.45, 1.392, 1.45, + 1.856, 1.74, 1.682, 1.566, 1.566, 2.03, 2.146, 1.914, + 2.03, 2.204, 2.494, 2.436, 2.03, 2.436, 2.842, 2.436, + 2.32, 2.204, 2.552, 2.9, 2.668, 2.958, 2.842, 2.726, + 2.842, 3.074, 2.9, 3.19, 3.074, 2.61, 3.19, 3.596, + 3.422, 3.48, 3.422, 3.48, 3.48, 3.364, 3.886, 4.002, + 3.828, 3.944, 4.002, 4.176, 4.118, 4.002, 4.35, 4.118, + 4.176, 4.756, 4.176, 4.466, 4.698, 4.814, 4.988, 4.988, + 4.698, 4.872, 4.93, 4.872, 5.162, 4.756, 4.988, 4.93, + 5.104, 5.278, 5.22, 5.22, 5.452, 5.51, 5.278, 5.684, + 5.974, 5.626, 5.742, 5.626, 5.8, 5.8, 5.858, 5.858, + 6.09, 6.148, 5.858, 6.206, 6.786, 6.554, 6.496, 6.438, + 6.67, 6.902, 6.96, 6.496, 6.786, 6.96, 7.076, 7.192, + 6.96, 7.134, 6.612, 7.25, 7.192, 7.54, 7.772, 7.25, + 7.598, 7.772, 7.946, 7.54, 8.236, 7.888, 7.772, 8.004, + 8.004, 8.004, 8.062, 8.294, 8.642, 8.41, 8.468, 8.526, + 8.178, 8.352, 8.932, 8.7, 8.7, 8.7, 9.106, 9.106, + 9.106, 9.222, 9.164, 9.396, 9.512, 9.222, 9.454, 9.338, + 9.628, 9.686, 9.628, 9.686, 9.628, 9.57, 10.034, 10.034, + 9.686, 10.034, 9.976, 10.092, 10.324, 10.034, 10.556, 10.556, + 10.672, 10.382, 10.788, 10.962, 10.672, 10.846, 10.904, 10.904, + 11.078, 10.788, 11.31, 11.658, 11.252, 11.426, 11.252, 11.252, + 11.542, 11.368, 11.716, 11.948, 12.006, 11.658, 11.542, 12.064, + 12.238, 11.774, 12.47, 12.122, 12.122, 12.528, 12.122, 12.354, + 12.644, 12.47, 12.818, 12.818, 12.702, 12.934, 12.818, 12.47, + 13.166, 12.876, 13.224, 13.398, 13.34, 13.514, 12.818, 13.224, + 13.92, 13.456, 13.688, 13.862, 13.92, 13.804, 14.152, 14.152, + 14.036, 14.268, 14.268, 14.152, 14.21, 14.442, 14.5, 14.558, + 14.326, 14.558, 14.616, 14.848, 14.616, 14.674, 14.848, 15.08, + 14.79, 15.254, 15.196, 15.312, 15.254, 15.428, 15.602, 15.718, + 15.486, 15.776, 15.776, 15.892, 15.602, 15.66, 16.066, 15.834, + 16.124, 16.298, 16.298, 16.24, 16.82, 16.704, 16.646, 16.356, + 16.588, 16.356, 16.82, 17.11, 16.936, 16.878, 17.4, 17.458, + 17.168, 17.284, 17.458, 17.342, 17.574, 17.516, 17.516, 17.574, + 17.806, 18.386, 18.038, 17.516, 17.922, 18.154, 18.154, 18.328, + 18.444, 18.212, 18.676, 18.56, 18.502, 18.502, 18.85, 18.676, + 18.676, 18.85, 19.198, 19.198, 19.314, 19.024, 19.256, 19.314, + 19.256, 19.256, 19.72, 19.43, 20.126, 20.3, 19.72, 19.488, + 19.836, 20.242, 19.952, 20.126, 20.068, 20.706, 20.358, 20.532, + 20.532, 20.242, 20.59, 20.474, 20.706, 20.764, 21.112, 21.054, + 21.286, 21.402, 21.286, 21.344, 21.17, 21.228, 21.46, 21.402, + 21.518, 21.402, 21.402, 21.402, 22.214, 22.098, 22.156, 22.214, + 22.098, 21.808, 22.156, 22.272, 22.388, 22.214, 22.678, 22.33, + 22.562, 22.736, 22.736, 22.794, 22.736, 22.968, 22.852, 23.084, + 23.084, 23.026, 23.084, 23.49, 23.432, 23.49, 23.722, 23.374, + 23.896, 23.49, 23.432, 23.722, 23.78, 23.838, 23.954, 24.186, + 24.128, 24.476, 24.186, 24.534, 24.186, 24.418, 24.65, 24.418, + 24.824, 24.824, 24.882, 24.94, 24.824, 25.404, 24.708, 25.056, + 25.346, 25.23, 24.882, 25.288, 25.694, 25.23, 25.578, 25.636, + 25.694, 25.462, 25.984, 26.042, 26.216, 26.1, 26.042, 26.274, + 26.1, 26.274, 26.332, 26.39, 26.796, 26.506, 26.68, 26.448, + 26.564, 26.796}, + {-0.406, -0.406, -0.87, -1.044, -0.464, -0.58, -0.348, -0.58, -0.58, + 0.116, -0.986, -0.406, -0.638, -0.754, -0.696, -0.696, -0.464, -0.406, + -0.696, -0.986, -0.638, -0.754, -0.522, -0.696, -0.58, -0.406, -0.812, + -0.87, -0.522, -0.812, -0.232, -0.696, -0.348, -0.812, -0.638, -0.812, + -0.522, -0.754, -0.754, -0.696, -0.464, -0.29, -0.348, -0.58, -0.522, + -0.58, -0.348, -0.174, -0.696, -0.638, -0.464, -0.928, -0.406, -0.58, + -0.348, -0.29, -0.348, -0.522, 0, -0.464, -0.232, -0.232, -0.29, + -0.174, 0, -0.58, -0.058, -0.348, -0.638, -0.116, 0, -0.29, + -0.058, -0.406, 0.058, -0.232, 0, -0.29, -0.232, -0.232, -0.116, + -0.116, -0.29, -0.232, 0.174, 0.174, -0.058, -0.116, 0.522, -0.174, + 0.232, -0.232, -0.058, 0.116, 0.116, 0.174, 0.638, 0.58, 0.464, + 0.522, 0.348, 0.058, 0.058, 0.406, 0, 0.29, 0.116, 0.232, + 0.696, 0, 0.638, 0.348, 0.29, 0.29, 0.406, 0.696, 0.464, + 0.29, 0.29, 0.406, 0.174, 0.406, 0.116, 0.348, 0.58, 0.464, + 0.058, 0.406, 0.174, 0.638, -0.058, 0.522, 0.464, 0.464, 0.29, + 0.348, -0.174, 0.174, 0.29, 0.29, 0.348, 0.058, 0.058, -0.116, + -0.058, 0.29, 0, 0.116, 0.232, 0.116, -0.29, 0.058, 0.116, + 0.406, 0.174, 0.174, -0.116, 0, 0, -0.058, -0.116, 0.058, + 0.174, 0.116, 0.348, 0.232, -0.058, 0.116, 0.174, 0.464, 0.116, + -0.174, -0.232, 0.116, -0.174, 0.116, 0.348, -0.116, -0.116, 0.29, + -0.116, -0.058, 0.232, -0.116, 0.174, 0, 0.406, 0.29, 0.232, + -0.174, -0.29, 0.232, -0.116, -0.116, 0.29, 0.174, 0.116, 0.29, + 0.116, 0, 0.058, 0.058, -0.174, 0.058, 0.29, -0.116, 0.116, + 0.058, -0.058, 0.348, 0.29, 0, 0.058, -0.232, -0.058, -0.232, + -0.174, 0.116, -0.116, -0.174, -0.174, 0.116, -0.058, -0.29, -0.348, + 0.116, 0.116, 0.116, 0.116, 0.116, -0.058, 0.116, -0.058, 0, + -0.174, 0.058, -0.522, -0.638, -0.058, -0.058, -0.174, 0.116, 0, + 0.348, -0.29, -0.406, -0.29, -0.29, -0.058, -0.058, -0.058, -0.29, + -0.232, -0.232, -0.232, 0, -0.406, 0.116, 0.29, 0.058, 0.174, + -0.232, 0, 0.232, -0.232, -0.174, 0.232, 0.174, 0.116, 0.174, + -0.116, 0, -0.058, -0.464, -0.058, 0.232, 0, -0.232, 0, + -0.174, -0.058, 0.058, -0.116, -0.116, -0.232, 0.116, -0.348, 0.464, + 0.058, -0.116, -0.232, -0.058, 0.058, 0.174, 0.174, 0.232, 0.058, + 0.232, -0.116, 0, -0.406, 0, 0.058, -0.116, 0.232, -0.058, + 0.116, -0.058, -0.174, -0.058, -0.232, -0.464, 0.058, -0.232, -0.116, + -0.174, 0.116, -0.232, -0.29, -0.116, -0.058, -0.406, -0.174, 0.058, + -0.058, 0.116, 0.174, 0.174, -0.058, -0.29, -0.29, -0.058, -0.406, + -0.058, -0.116, -0.058, -0.348, 0.232, 0.406, -0.058, 0.058, -0.174, + -0.058, -0.29, -0.116, -0.116, 0.174, 0.116, -0.348, -0.348, -0.058, + -0.58, -0.232, -0.116, 0, 0.058, -0.058, 0, 0.29, -0.232, + -0.174, -0.464, -0.232, -0.232, -0.174, -0.232, -0.754, -0.464, -0.29, + -0.174, -0.29, -0.232, -0.058, -0.232, -0.58, -0.116, -0.174, -0.348, + -0.348, 0, -0.348, -0.29, -0.174, -0.348, -0.116, -0.406, -0.638, + -0.232, -0.174, -0.29, -0.116, -0.29, 0, -0.406, -0.522, -0.116, + -0.406, -0.348, -0.348, -0.464, -0.348, -0.464, -0.638, -0.522, -0.522, + 0.058, -0.174, -0.116, -0.232, -0.464, -0.464, -0.696, -0.348, -0.232, + -0.464, -0.058, -0.29, -0.348, 0, -0.348, -0.116, -0.638, -0.232, + -0.29, -0.29, -0.58, -0.232, -0.696, -0.754, -0.232, -0.406, -0.754, + -0.58, -0.116, -0.464, 0.232, -0.638, -0.522, -0.464, -0.174, -0.522, + -0.116, -0.174, 0.058, -0.464, -0.232, -0.232, -0.406, -0.464, 0.174, + -0.29, -0.406, 0.174, -0.174, -0.29, -0.116, -0.348, -0.348, -0.406, + -0.406, -0.174, 0.116, -0.174, -0.348, -0.522, 0.058, -0.058, -0.58, + -0.348, 0.348, -0.232, -0.29, -0.406, -0.348, -0.116, 0.174, 0.058, + 0, -0.174, -0.058, 0, -0.174, 0.116, 0.406, -0.348, -0.348, + 0.058, 0, 0.116, -0.348, 0.058, -0.174, -0.174, 0.406, 0.174, + 0, 0.116, 0.058, -0.116, -0.116, 0, -0.29, -0.116, 0.058, + 0.058, 0.29, 0.638, 0, -0.232, 0.058, -0.174, -0.232, 0, + -0.116, 0.174, -0.464, -0.522, -0.058, -0.174, 0, 0.29, -0.174, + -0.464, -0.464, -0.348, -0.116, -0.58, -0.116, 0, -0.058, -0.058, + 0, 0.232, -0.406, -0.348, -0.174, 0.174, -0.29, 0.174, 0, + 0, 0.232, -0.058, 0.116, 0.058, -0.058, -0.058, 0.29, 0.29, + 0.116, 0, 0, -0.058, 0.058, 0.116, -0.116, 0.116, 0.232, + 0.29, 0.406, 0.406, 0.348, 0.116, 0.116, -0.058, 0.174, 0.29, + 0, 0.232, 0, 0.174, -0.174, 0.232, -0.058, 0.116, 0.116, + 0.522, 0.174, 0.348, 0.232, 0.348, 0.116, 0.174, -0.058, 0.232, + -0.174, -0.116, 0.058, 0.522, -0.232, 0.232, 0.174, 0.058, -0.058, + 0.116, 0.058, 0.174, -0.058, -0.058, -0.174, 0.116, -0.058, -0.116, + 0.29, 0.174, 0.174, 0, 0.058, 0.058, 0.058, 0.232, 0.29, + -0.116, -0.116, -0.29, 0.116, -0.232, -0.058, -0.29, 0.348, 0.058, + 0.116, 0.29, 0.058, -0.116, 0.29, 0.406, 0.058, 0.348, 0.058, + 0.174, 0.116, -0.174, 0.116, -0.058, -0.29, -0.29, -0.058, 0.406, + -0.29, -0.058, -0.058, 0.29, -0.174, 0.29, 0, 0.58, -0.058, + -0.174, -0.29, 0.29, 0.232, 0.348, 0.058, 0.116, 0.29, 0.348, + -0.29, -0.116, 0.406, 0.522, 0.174, 0, -0.058, -0.116, 0.464, + -0.058, 0.058, 0.464, 0.058, 0.174, 0.29, 0.406, 0, 0.29, + 0.116, 0.232, 0.348, 0.406, 0.464, 0.174, 0.174, 0.232, 0.058, + 0.29, -0.058, 0.406, -0.116, 0.174, 0, 0.058, 0.406, 0.174, + 0.058, 0.522, 0.116, 0.058, 0.29, -0.116, 0.116, 0.116, 0.406, + -0.058, 0.464, -0.058, 0.116, 0, 0.232, -0.058, 0.174, 0.116, + -0.232, 0.058, -0.174, 0.116, -0.058, -0.116, 0.058, -0.29, -0.116, + -0.116, 0.116, 0.116, -0.174, 0.29, 0.116, 0.348, 0.232, -0.174, + 0, 0.058, 0.29, 0.464, 0.174, -0.174, 0, 0.058, 0.058, + -0.232, 0.174, -0.058, 0.116, -0.232, 0.29, 0.29, -0.174, 0.174, + 0.29, 0.174, 0.116, 0, 0.29, 0.232, 0.232, 0.174, 0.116, + 0.58, 0.406, 0.406, -0.116, 0.232, 0.058, -0.174, -0.348, 0.29, + -0.058, -0.116, 0, 0.116, 0.232, 0.116, -0.406, -0.116, -0.29, + -0.29, 0, -0.116, 0.116, -0.174, -0.058, 0.116, -0.464, -0.174, + 0, -0.058, -0.058, 0, 0.058, 0.174, -0.058, 0.116, 0, + 0.232, 0.406, -0.058, -0.29, -0.29, -0.174, -0.232, 0.058, -0.058, + -0.116, -0.58, -0.29, -0.29, -0.174, 0.058, 0.058, 0, -0.348, + 0, -0.406, -0.522, 0, 0, 0, 0.232, 0.058, -0.232, + -0.696, -0.232, -0.232, -0.29, -0.058, -0.174, 0.058, -0.522, -0.116, + -0.464, -0.232, 0, -0.232, -0.348, -0.406, -0.406, -0.174, -0.058, + -0.116, -0.174, -0.116, 0.058, -0.29, -0.174, -0.116, -0.522, -0.174, + -0.232, -0.522, 0.058, -0.232, 0.116, -0.754, -0.116, -0.348, 0.29, + -0.464, -0.116, -0.812, -0.174, 0, -0.406, 0.058, -0.29, -0.348, + -0.29, -0.522, -0.116, -0.464, -0.406, -0.232, -0.58, -0.058, 0.116, + -0.464, 0, -0.29, -0.116, 0.058, -0.406, -0.058, -0.058, -0.696, + -0.522, -0.232, -0.406, -0.232, -0.174, -0.116, 0.116, -0.058, -0.406, + 0.058, -0.174, -0.174, -0.406, -0.116, -0.29, -0.116, -0.116, -0.174, + 0.116, -0.406, -0.116, -0.406, -0.174, -0.464, 0.058, -0.348, 0, + -0.232, -0.174, -0.058, -0.232, -0.348, 0.116, -0.058, 0, 0.058, + -0.116, -0.174, -0.174, -0.116, -0.116, -0.232, -0.058, -0.522, -0.29, + 0.174, -0.29, -0.348, -0.406, -0.348, -0.232, -0.174, -0.058, -0.116, + -0.174, -0.174, 0.058, 0.174, -0.174, -0.058, 0, 0.406, -0.058, + -0.522, -0.58, 0, -0.174, 0.058, 0.116, -0.29, 0.058, 0.116, + 0.232, -0.058, 0.058, 0, 0, -0.116, -0.058, 0.174, 0.116, + 0.058, 0.174, 0.058, 0.406, -0.058, -0.406, 0.116, 0.058, 0.116, + 0.29, -0.29, -0.058, -0.116, -0.116, -0.29, 0, -0.058, -0.406, + -0.232, -0.116, 0.232, -0.232, 0, 0, -0.29, -0.058, -0.058, + 0.522, -0.174, 0.174, 0.058, -0.232, -0.116, 0.29, 0, 0.116, + 0.174, 0.348, 0.116, -0.058, -0.116, 0.116, 0.174, 0.116, 0.232, + -0.29, 0.29, 0.174, 0.174, 0, 0.174, 0.116, 0.174, -0.058, + -0.058, 0.116, 0.174, 0.174, -0.058, -0.058, 0.406, -0.058, -0.29, + -0.348, -0.174, -0.29, -0.29, 0.348, -0.058, -0.348, -0.232, -0.174, + -0.058, -0.522, -0.058, 0.058, 0.116, -0.232, 0.58, -0.29, 0, + 0.058, 0, 0.058, 0.174, 0.174, 0.464, 0, -0.116, -0.058, + -0.058, -0.116, -0.116, -0.058, -0.058, 0.058, -0.058, 0, 0.116, + -0.232, 0.058, -0.232, 0, 0.058, 0.406, 0.116, 0.174, -0.116, + 0.232, 0, -0.116, -0.29, -0.116, 0.232, -0.058, 0.174, 0.174, + -0.058, -0.232, -0.116, 0.116, 0.232, 0.058, 0.174, 0.348, 0.29, + -0.174, 0.058, 0.58, 0.058, 0.464, 0.232, 0, -0.116, 0.29, + 0.116, 0.058, 0.348, 0.29, 0.29, 0, 0.29, 0.348, 0.116, + -0.058, 0.406, -0.174, -0.174, -0.058, 0.058, 0.522, 0.116, 0.232, + 0, 0.058, 0.116, -0.29, 0, -0.174, 0.348, 0.116, 0, + 0, 0.348, 0.406, 0.406, 0.174, 0.232, -0.29, 0.058, 0.464, + 0.058, 0.29, -0.406, 0.29, -0.232, -0.232, 0.058, -0.232, 0.406, + 0.174, 0.058, 0, 0.174, 0.174, 0.29, -0.232, 0.174, 0.174, + 0.058, 0.348, 0.116, 0.29, 0.058, 0, 0.348, -0.058, 0.058, + 0.406, 0.232, 0.058, 0.116, 0.174, 0.174, -0.174, 0.058, -0.232, + 0.29, 0.058, 0.232, 0, -0.348, 0.174, 0.232, 0.174, 0.232, + 0.406, 0.174, -0.174, -0.116, 0.058, 0.058, 0.058, 0.232, -0.116, + 0.464, 0, 0.29, 0, -0.116, -0.058, -0.29, 0.232, 0.174, + 0.058, 0.232, -0.116, -0.058, 0.116, -0.232, -0.174, 0.116, 0.348, + 0.116, 0.116, 0.232, -0.174, 0.116, 0.232, 0, -0.058, -0.232, + 0.232, 0.116, -0.058, 0.174, 0.29, 0.232, 0.058, 0.174, -0.058, + 0.174, 0.174, -0.116, 0.116, -0.174, 0.058, -0.058, 0, 0.464, + -0.29, 0.174, 0, 0.464, -0.058, 0.058, 0.058, -0.116, 0.058, + 0.058, -0.174, -0.174, 0, -0.29, 0.174, 0.348, 0.174, 0, + -0.464, 0.058, 0.348, -0.58, -0.232, -0.174, 0, -0.116, 0.116, + 0.174, -0.29, -0.232, -0.058, 0.058, -0.232, 0.058, 0.058, 0.058, + -0.116, -0.696, -0.348, 0.174, 0.174, 0.116, -0.116, 0.232, -0.464, + -0.29, 0.174, -0.058, -0.116, -0.116, -0.058, -0.058, 0, 0.174, + -0.29, 0.29, -0.174, 0.174, -0.116, 0.174, -0.116, 0.116, -0.058, + 0.348, -0.174, -0.058, -0.174, -0.348, -0.348, 0.116, 0.174, 0.116, + -0.058, 0.116, 0.232, 0.174, -0.464, -0.232, -0.058, 0.174, -0.058, + 0, 0.058, -0.174, -0.116, 0.058, 0, -0.232, -0.058, -0.348, + 0.058, -0.058, -0.174, -0.058, -0.116, 0.116, -0.464, -0.174, -0.232, + -0.058, -0.058, -0.058, -0.116, 0.116, -0.348, -0.58, -0.232, 0.29, + -0.464, 0.058, 0.174, -0.464, -0.406, -0.232, -0.29, -0.058, 0.232, + -0.464, -0.174, -0.058, -0.116, -0.29, 0.348, -0.638, -0.058, -0.29, + -0.058, 0, 0.232, 0.058, 0, 0, -0.29, -0.174, -0.174, + -0.116, -0.116, 0.174, 0.232, -0.116, -0.29, -0.174, -0.116, 0, + 0.348, -0.174, 0, 0.116, -0.29, -0.174, 0.232, 0, 0, + -0.116, -0.116, 0.058, 0.58, 0.348, -0.174, -0.116, -0.116, -0.464, + -0.406, -0.232, -0.174, 0.058, -0.29, -0.174, -0.058, -0.174, -0.174, + 0.174, -0.058, -0.348, 0.116, -0.406, -0.058, -0.116, -0.29, 0.116, + 0.058, -0.058, -0.232, 0.116, -0.58, -0.116, 0.058, -0.116, -0.638, + -0.232, 0.058, 0, -0.232, -0.058, 0.232, 0.058, 0.116, -0.058, + -0.232, -0.116, -0.29, -0.058, 0.058, 0.116, 0.058, -0.29, 0, + 0.058, -0.29, -0.058, 0.058, 0, 0.29, 0, -0.348, -0.058, + 0.348, -0.232, 0.058, 0.116, -0.29, 0, -0.232, -0.058, 0.174, + -0.058, 0.058, -0.348, 0, -0.058, -0.058, 0.116, 0.174, 0.058, + -0.174, -0.116, -0.232, 0, -0.29, -0.174, -0.116, 0, -0.174, + -0.232, 0.058, 0.174, 0.29, 0.232, -0.058, -0.174, 0.116, -0.058, + 0.348, -0.348, -0.116, 0.058, -0.522, 0, -0.116, 0.116, -0.464, + -0.058, 0.058, -0.29, -0.116, 0, -0.058, 0.174, -0.174, -0.174, + -0.174, -0.29, 0.058, -0.174, -0.406, -0.406, 0, -0.29, -0.232, + 0.174, 0, -0.29, -0.232, -0.116, -0.116, -0.29, -0.116, -0.522, + -0.232, -0.29, -0.29, -0.464, -0.174, 0, -0.638, -0.058, 0.058, + 0.174, -0.29, 0.116, -0.174, -0.116, -0.29, -0.116, -0.116, -0.464, + -0.348, -0.29, -0.406, 0.29, 0.232, 0.058, 0, -0.232, -0.406, + -0.116, 0.058, 0.29, 0, -0.058, -0.116, 0.406, 0.058, 0.232, + 0.522, 0.174, -0.174, 0, -0.232, -0.174, 0.116, 0.406, -0.116, + 0.174, 0, 0.116, 0.29, 0.232, -0.116, -0.174, 0, 0.29, + -0.174, 0.174, 0.058, 0, 0, -0.116, -0.348, 0.116, -0.058, + -0.058, 0.232, 0.29, 0.348, 0.348, 0.058, 0.232, -0.058, -0.058, + -0.116, -0.406, 0.29, 0.174, 0.058, 0, -0.116, 0.406, -0.29, + 0.58, -0.058, -0.116, 0.29, 0.116, 0.348, 0, -0.058, -0.232, + 0.116, 0, -0.058, -0.058, 0.174, 0.174, 0.058, 0, -0.232, + 0.058, 0.058, 0.058, -0.058, 0.058, 0, 0.058, -0.058, 0, + 0.058, 0.116, 0, 0.116, -0.058, 0.232, 0, -0.174, 0.406, + -0.29, -0.116, 0.058, -0.058, 0.058, 0.174, 0.116, 0.058, -0.058, + 0.174, 0, 0.116, 0.464, 0.174, 0.174, 0.29, 0.348, 0.29, + 0.232, 0.174, 0.232, 0.232, 0.116, 0.232, 0.348, 0, 0.522, + 0.174, 0.29, 0.29, 0.348, 0.58, 0.464, 0, 0.522, 0.29, + 0.116, 0.406, 0.232, 0.348, 0.29, 0.174, 0.754, 0.232, 0.464, + 0.406, 0.232, 0.29, 0.232, 0.464, 0.696, 0.522, 0.29, 0.638, + 0.232, 0.522, 0.754, 0.406, 0.464, 0.29, 0.29, 0.406, 0.232, + 0.638, 0.58, 0.29, 0.232, 0.232, 0.058, 0.232, 0.058, 0.406, + 0.522, 0.174, 0.058, 0.232, 0.522, 0.174, 0.29, 0.116, 0.348, + 0.696, 0.464, 0.29, 0.29, 0.232, 0.464, 0.29, 0.348, 0.406, + 0.348, 0.058, 0.522, 0.638, 0.464, 0.29, 0.232, 0, -0.058, + 0, 0, -0.232, 0, 0.116, 0.348, 0.116, 0.116, 0.116, + -0.058, -0.116, 0.58, 0.116, 0.232, 0.058, 0.29, 0.232, 0.522, + 0.174, -0.116, 0.058, 0.406, 0.058, 0.116, 0.058, 0.232, 0.58, + 0, 0.348, 0.406, 0.174, 0, 0.29, 0.174, 0.638, 0, + 0.812, 0.29, 0.638, 0.29, 0.058, 0.29, 0.058, 0.29, -0.058, + 0.29, -0.116, 0, 0.522, 0.406, -0.116, 0.174, 0.406, 0.522, + -0.058, 0.058, 0.406, 0.638, 0.174, -0.116, 0.232, 0.29, 0.058, + 0.174, -0.232, 0.348, 0.696, 0.232, 0.348, 0.58, 0.29, 0.406, + 0.232, 0.174, 0.29, 0.464, 0.058, 0.232, 0.232, 0.406, 0.29, + 0.29, 0.464, 0.29, 0.348, 0.348, 0.464, 0.174, 0.696, 0.464, + 0.116, 0.116, 0.058, 0.406, 0.29, 0.058, -0.116, 0.29, 0.464, + 0.174, 0.232, 0, 0.464, 0.174, 0.58, 0.058, 0.116, 0.522, + 0.348, 0.232, 0.638, 0.522, 0.29, 0.348, 0.232, 0.522, 0.174, + 0.638, 0.116, 0.116, 0.638, 0.29, 0.232, 0.29, 0.348, 0.406, + 0.29, 0.232, 0, 0.638, 0.058, 0.174, 0.116, 0.58, 0.58, + 0.812, 0.406, 0, 0.232, 0.29, 0.174, 0.522, 0.232, -0.058, + 0.406, 0.232, 0.232, 0.232, 0, 0.232, 0.174, 0.58, 0.232, + -0.174, 0.232, 0.232, 0.058, 0.29, -0.058, 0.058, -0.29, 0.174, + 0.116, 0.232, 0.406, 0.174, -0.058, 0.58, 0.58, 0.174, 0.406, + -0.116, 0.348, 0.58, 0, 0.29, 0.174, 0.29, 0.348, 0.348, + 0.464, 0.58, 0.58, 0.058, 0.464, 0, 0.522, 0.406, 0.29, + 0.232, 0.058, 0.406, 0.464, 0.232, 0.058, 0.174, 0.116, 0.232, + 0.174, 0.232, 0.406, 0.406, 0.058, 0.058, 0.232, 0.058, 0.29, + -0.058, -0.174, 0.348, 0.638, 0.174, 0.232, 0.29, 0.232, 0.522, + 0.406, 0.116, 0.29, 0.174, 0.348, 0.29, 0, 0.522, 0.348, + 0.406, 0.174, 0.464, 0.232, 0.464, 0.232, 0.058, 0.058, 0.058, + 0.29, 0, 0.464, -0.116, 0.116, 0.58, 0.058, 0.348, 0.29, + 0.638, 0.058, 0.058, 0, 0.406, 0.29, 0.58, 0.232, 0.348, + 0.174, 0.116, -0.058, -0.174, 0.232, 0.174, 0.232, 0.406, 0.348, + -0.058, 0.232, 0.638, 0.29, 0.406, 0.232, 0.29, 0, -0.232, + 0.348, 0, 0.522, 0.174, 0.464, 0.232, 0.116, 0.406, 0.406, + 0.638, 0.29, 0.232, 0.116, 0.406, 0.464, 0.116, 0.348, 0.406, + 0.29, 0.232, -0.232, 0.058, 0.116, -0.174, 0, 0.29, 0.29, + 0.232, -0.232, -0.116, -0.29, 0.116, 0, 0.174, 0.232, 0.116, + 0.232, -0.058, -0.116, 0.058, 0, 0.638, 0.174, 0.116, 0.406, + 0.522, 0.348, 0.348, 0.116, 0.464, 0.232, 0.116, 0.174, 0, + 0.174, 0.464, 0.406, -0.058, 0.406, 0.29, 0.348, 0.058, 0.174, + 0.232, 0.348, 0.116, 0.174, 0.406, 0, 0.464, 0.522, 0.174, + 0.29, 0.174, 0.58, -0.232, 0.232, 0.058, 0.058, 0.406, 0.348, + 0.348, 0.174, 0.058, 0.29, 0.406, 0.174, -0.116, 0.464, 0.58, + 0.522, 0.58, 0.464, 0.174, 0.522, 0.464, 0.58, 0.232, 0.638, + 0.232, 0.348, 0.232, 0.696, 0.58, 0.232, 0.406, 0.116, 0.406, + 0.348, 0.174, 0.464, 0.348, 0.232, 0.58, 0.522, 0.406, 0.29, + 0.174, 0.812, 0.696, 0.406, 0.406, 0.232, 0.406, 0.406, 0.348, + -0.058, 0.406, 0.696, -0.174, 0.058, 0, 0.406, 0.464, 0.406, + 0.638, 0.058, 0.232, 0.638, 0.058, 0.116, 0.232, 0.29, 0.522, + 0.29, 0.29, 0.406, 0.232, 0.638, 0.406, 0.464, 0.348, 0.232, + 0.058, -0.116, 0.58, 0.58, 0.638, 0.348, 0.174, 0.174, 0.174, + 0.058, 0.348, 0.232, 0.754, 0.464, 0.174, 0.348, 0.58, 0.638, + 0.406, 0.29, 0.174, 0.232, 0.232, 0.522, 0.348, 0.464, 0.348, + 0.522, 0.696, 0.058, 0.29, -0.058, 0.464, 0.29, 0.58, 0.406, + -0.174, -0.116, 0.058, 0.058, 0.348, 0, 0.232, 0.232, 0.29, + 0.754, 0.58, 0.116, 0.29, 0.348, 0.464, 0.232, 0.522, 0.58, + 0.058, 0.348, 0.58, 0.174, 0.058, 0.174, 0.29, 0.232, 0.174, + 0.522, 0.232, 0.348, 0.116, 0.348, -0.174, 0.29, 0.348, 0.348, + 0.116, -0.29, 0.058, 0.058, 0.29, 0, 0.406, 0.232, 0.29, + -0.348, 0.464, 0.522, 0.174, 0.116, 0.348, 0.406, 0.116, 0.348, + 0.522, 0.638, 0.406, 0.464, 0.348, 0.348, 0.29, 0.116, 0.522, + 0.174, 0.348, 0.116, 0.232, 0.406, 0, 0.116, -0.058, 0, + -0.116, 0, 0.406, 0.116, 0.29, 0.58, 0.464, 0.232, 0.406, + 0.29, 0.638, 0.638, 0.29, 0.464, -0.116, 0.696, 0.406, 0.406, + 0.174, 0.232, 0.754, 0.464, 0.232, 0.58, 0.058, 0.522, 0.522, + 0.406, 0.232, 0.406, 0, 0.174, 0.174, 0.406, 0.464, 0.464, + 0.464, 0.406, 0.174, 0.174, 0.464, -0.058, 0.29, 0.464, 0.812, + 0.464, 0.638, 0.406, 0.232, 0.232, 0.522, 0.29, 0.696, 0.348, + 0.058, 0.812, 0.174, 0.58, 0.058, 0.174, 0.696, 0.232, 0.348, + 0.464, 0.116, 0.174, 0.58, 0.406, 0.232, 0.174, 0.406, 0.116, + 0.638, 0.406, 0.348, 0.406, 0.29, 0.58, 0.348, 0.522, 0.522, + 0.29, 0.464, 0.058, 0.58, 0.232, 0.174, 0.174, 0.58, 0.406, + 0.058, 0.232, 0.638, 0.522, 0.58, 0.696, 0.29, 0.348, 0.348, + 0.348, 0.348, 0.754, 0.348, 0.232, 0.29, 0.174, 0.754, 0.232, + 0.812, 0.58, 0.58, 0.174, 0.116, 0.174, 0.464, 0.464, 0.232, + 0, 0.638, 0.116, 0.406, 0.522, 0.29, 0.116, 0.174, 0, + 0.174, 0.232, 0.058, 0.348, 0.29, 0.232, 0, 0.522, 0.406, + 0.232, 0.232, 0.116, 0.522, 0.232, 0.174, 0, 0, -0.348, + 0.406, 0.464, 0.58, 0.174, 0.174, 0.406, -0.116, 0.232, 0.348, + 0.464, 0.174, 0.348, 0.58, 0.696, 0.464, 0.29, 0.232, 0.058, + 0.058, 0.116, 0.638, 0.058, 0.232, 0.174, -0.232, 0.638, 0.522, + -0.174, 0.058, 0, 0.058, 0.174, 0.29, 0.29, 0, 0.232, + -0.29, 0.116, -0.058, 0.174, -0.058, 0.058, -0.232, -0.116, -0.058, + 0.058, -0.174, 0.116, 0.232, -0.058, 0.29, 0.232, 0.638, 0.232, + -0.174, 0.232, 0, 0.058, 0.232, 0.116, -0.232, 0, 0.348, + 0, 0, 0.174, -0.058, 0.29, 0.406, 0.116, 0.174, 0.174, + 0.174, 0, -0.232, 0.522, 0.348, 0, 0.116, 0.348, 0.174, + 0.174, 0.29, 0.232, 0.348, -0.406, 0.174, 0.464, -0.116, 0.29, + 0.232, -0.058, -0.116, 0.174, 0.174, 0.116, -0.348, 0.174, 0.116, + 0.348, -0.29, -0.058, 0.406, 0.116, 0.232, 0.58, 0.058, -0.116, + 0.116, 0.058, 0.116, -0.406, 0.174, 0.058, 0.406, 0.174, 0.174, + 0.058, 0.406, -0.058, -0.174, 0.232, -0.058, 0, 0.058, 0.116, + -0.116, -0.116, 0.29, -0.116, 0.232, 0.174, 0.232, 0.522, 0.116, + 0.116, -0.464, 0.116, 0.522, -0.058, 0.174, -0.232, 0, -0.058, + 0, 0.232, 0, 0.58, 0.174, -0.232, 0.29, 0.058, 0.464, + 0.116, 0, 0.174, 0.406, 0.348}, + {29.87, 30.16, 30.044, 29.986, 30.16, 30.044, 29.928, 30.044, 29.812, + 29.928, 30.044, 30.044, 30.16, 29.928, 30.044, 30.276, 30.218, 30.16, + 29.986, 29.928, 30.102, 29.87, 30.276, 30.334, 29.928, 30.102, 29.928, + 29.928, 30.102, 29.87, 29.87, 30.16, 29.754, 30.102, 30.16, 30.276, + 30.276, 30.334, 29.87, 30.218, 30.16, 30.276, 30.16, 30.218, 30.102, + 30.276, 30.392, 30.624, 30.392, 30.218, 30.856, 30.566, 30.74, 31.03, + 30.856, 31.146, 31.204, 31.204, 31.378, 31.088, 31.378, 31.436, 31.494, + 31.552, 31.61, 32.132, 31.726, 32.132, 32.654, 32.248, 32.48, 32.712, + 32.712, 32.77, 32.77, 32.944, 33.234, 33.118, 33.118, 33.524, 33.814, + 33.988, 34.22, 33.466, 34.046, 33.988, 33.988, 34.568, 34.394, 35.032, + 34.568, 34.684, 35.264, 34.568, 35.09, 35.032, 35.322, 35.206, 35.612, + 35.728, 35.67, 35.264, 35.612, 35.554, 35.612, 35.728, 36.018, 35.554, + 35.786, 35.844, 35.902, 36.076, 35.786, 35.902, 35.96, 36.018, 35.902, + 35.902, 36.018, 35.438, 35.612, 35.902, 35.67, 35.554, 35.728, 35.67, + 35.612, 35.38, 35.438, 35.09, 35.438, 35.322, 34.8, 34.684, 34.916, + 34.974, 34.8, 34.684, 34.626, 34.684, 34.452, 33.988, 34.742, 34.336, + 34.104, 34.104, 33.872, 34.278, 34.104, 33.988, 33.93, 33.93, 33.93, + 34.51, 33.698, 33.756, 34.104, 34.046, 34.22, 34.046, 34.394, 34.22, + 34.336, 34.278, 33.988, 34.336, 34.452, 34.8, 34.916, 34.8, 34.8, + 34.684, 34.858, 35.032, 35.032, 35.09, 35.032, 35.322, 35.264, 35.206, + 35.38, 35.554, 35.67, 35.38, 35.148, 35.438, 35.148, 35.554, 35.032, + 35.438, 35.09, 35.264, 35.206, 34.974, 35.09, 35.032, 35.09, 35.09, + 35.38, 35.148, 35.148, 35.09, 34.626, 34.858, 34.858, 35.322, 35.032, + 34.8, 34.858, 34.858, 34.626, 34.626, 34.626, 34.51, 34.626, 34.568, + 34.974, 34.626, 34.452, 34.974, 35.09, 34.8, 35.09, 34.8, 35.438, + 35.148, 35.09, 35.206, 35.612, 35.496, 35.032, 35.148, 35.206, 35.264, + 35.206, 35.206, 35.438, 35.496, 35.264, 35.09, 35.206, 35.206, 35.728, + 34.452, 35.09, 35.148, 35.206, 35.206, 34.974, 34.974, 35.206, 34.858, + 35.148, 34.8, 34.684, 34.742, 34.742, 34.8, 35.322, 34.974, 35.148, + 35.322, 34.8, 35.38, 35.438, 35.032, 35.148, 35.264, 35.148, 35.264, + 35.264, 35.032, 35.148, 35.496, 35.148, 34.974, 34.974, 35.09, 35.496, + 35.554, 35.264, 35.09, 35.496, 35.206, 34.916, 34.742, 35.148, 35.09, + 34.8, 35.09, 34.8, 35.148, 34.858, 34.974, 35.264, 35.09, 35.264, + 35.148, 35.38, 34.8, 35.032, 35.148, 34.974, 34.974, 34.974, 34.974, + 34.916, 34.858, 35.322, 35.438, 35.38, 35.206, 34.858, 35.322, 35.09, + 35.032, 35.264, 35.496, 35.206, 35.554, 35.032, 35.09, 35.38, 35.09, + 35.148, 35.438, 35.148, 35.206, 35.032, 35.322, 35.438, 35.554, 35.206, + 35.264, 35.264, 35.264, 34.8, 35.496, 35.554, 35.322, 35.09, 35.496, + 35.206, 35.032, 35.322, 35.09, 35.38, 35.264, 35.322, 35.09, 35.554, + 35.322, 35.264, 35.38, 35.032, 35.322, 35.554, 35.032, 35.612, 35.496, + 35.206, 35.264, 35.67, 35.206, 35.38, 35.264, 35.322, 35.322, 35.206, + 35.264, 35.032, 35.264, 35.438, 35.728, 34.8, 35.206, 35.554, 35.264, + 35.148, 35.264, 35.206, 35.264, 35.322, 35.38, 34.858, 35.322, 35.032, + 35.148, 35.496, 35.206, 35.032, 35.09, 35.38, 35.322, 35.612, 35.264, + 34.974, 34.916, 35.206, 34.916, 35.206, 35.496, 35.09, 35.322, 35.322, + 35.09, 35.206, 35.438, 34.916, 35.148, 35.148, 35.438, 35.38, 35.148, + 35.38, 35.322, 35.438, 35.496, 35.032, 35.496, 35.206, 35.438, 35.264, + 34.974, 35.322, 35.206, 35.554, 35.322, 35.496, 35.264, 35.322, 35.148, + 35.438, 35.322, 35.438, 34.916, 35.032, 35.38, 35.264, 35.322, 35.496, + 35.322, 35.438, 35.612, 35.206, 35.322, 35.554, 35.438, 35.438, 35.38, + 35.264, 35.206, 35.38, 35.496, 35.496, 35.496, 35.264, 35.496, 35.496, + 35.612, 35.09, 35.554, 35.38, 35.206, 35.496, 35.554, 35.67, 35.206, + 35.206, 35.032, 35.206, 35.438, 35.264, 35.438, 35.38, 35.148, 35.206, + 35.206, 35.206, 35.148, 34.974, 35.264, 35.206, 34.916, 35.554, 35.264, + 35.612, 35.206, 35.438, 35.612, 35.206, 35.496, 35.554, 35.496, 35.554, + 34.916, 35.496, 35.38, 35.728, 35.264, 35.032, 35.612, 35.322, 35.38, + 35.206, 35.902, 35.67, 35.38, 35.322, 35.438, 35.264, 35.032, 35.322, + 35.38, 35.322, 35.554, 35.786, 35.264, 35.496, 35.322, 35.612, 35.67, + 35.148, 35.844, 35.38, 35.786, 35.554, 35.322, 35.554, 35.438, 35.844, + 35.38, 35.728, 35.38, 35.728, 35.612, 36.134, 35.786, 35.438, 35.38, + 35.496, 35.612, 35.322, 35.728, 35.438, 35.38, 35.554, 35.38, 35.496, + 35.728, 36.018, 35.67, 35.322, 35.612, 35.67, 35.67, 35.496, 35.786, + 35.612, 35.902, 35.612, 35.728, 35.728, 35.844, 35.554, 35.786, 35.496, + 35.902, 36.018, 35.554, 35.786, 35.67, 35.728, 35.67, 35.67, 35.728, + 35.902, 35.902, 35.67, 35.67, 35.612, 35.786, 35.496, 35.67, 35.902, + 35.728, 35.96, 35.67, 35.67, 35.554, 36.134, 35.96, 35.496, 35.786, + 35.67, 35.96, 35.902, 35.496, 35.612, 35.844, 35.728, 35.786, 35.554, + 35.554, 35.902, 35.902, 35.38, 35.322, 35.438, 35.554, 35.902, 35.554, + 35.844, 36.018, 36.018, 35.612, 35.96, 36.134, 35.728, 35.554, 35.844, + 35.67, 35.902, 35.496, 36.192, 35.902, 35.438, 35.496, 35.496, 35.844, + 36.018, 35.96, 36.192, 35.786, 35.902, 35.786, 35.844, 35.612, 35.902, + 36.192, 35.96, 35.96, 35.554, 35.902, 36.018, 36.076, 36.134, 35.902, + 35.844, 36.018, 36.25, 35.786, 36.076, 35.902, 35.96, 35.902, 35.554, + 35.902, 35.728, 35.554, 36.192, 35.612, 35.844, 35.96, 36.018, 35.67, + 35.67, 35.844, 35.844, 35.67, 35.728, 35.438, 35.728, 35.728, 35.844, + 35.844, 35.844, 36.134, 36.076, 36.018, 35.728, 36.018, 35.554, 36.018, + 36.018, 36.134, 36.308, 36.366, 35.612, 35.902, 35.902, 35.844, 36.134, + 36.018, 36.018, 35.902, 35.96, 35.67, 35.844, 36.192, 36.366, 35.96, + 35.96, 35.902, 36.54, 36.076, 36.076, 36.018, 36.076, 36.308, 35.844, + 36.308, 36.25, 36.076, 36.134, 36.25, 36.076, 36.134, 36.308, 36.366, + 36.018, 36.076, 36.25, 36.25, 36.076, 36.018, 36.192, 36.076, 36.54, + 36.25, 36.134, 36.134, 36.134, 36.076, 36.192, 36.25, 36.25, 36.424, + 36.076, 35.96, 36.54, 36.482, 35.844, 36.482, 36.308, 36.598, 35.96, + 35.96, 36.018, 36.598, 36.308, 36.772, 36.424, 36.54, 36.366, 36.308, + 36.424, 36.656, 36.308, 36.772, 36.598, 36.366, 36.424, 36.25, 36.482, + 36.656, 36.54, 36.25, 36.424, 36.25, 36.424, 36.192, 36.192, 36.482, + 36.482, 36.192, 36.366, 36.424, 36.366, 36.598, 36.192, 36.424, 36.308, + 36.25, 36.54, 36.482, 36.656, 36.482, 36.308, 36.54, 36.598, 36.25, + 36.482, 36.308, 36.424, 36.598, 36.714, 36.366, 36.54, 36.54, 36.424, + 36.308, 36.656, 36.25, 36.54, 36.482, 36.772, 36.772, 36.54, 36.54, + 36.714, 36.54, 36.424, 36.366, 36.598, 36.134, 36.656, 36.888, 36.366, + 36.598, 36.888, 36.366, 36.134, 36.424, 36.482, 36.888, 36.656, 36.482, + 36.598, 36.25, 36.888, 36.656, 36.54, 36.656, 36.598, 36.54, 36.424, + 36.366, 36.134, 36.366, 36.482, 36.714, 36.424, 36.308, 36.83, 36.424, + 37.004, 36.714, 36.946, 36.424, 36.714, 37.12, 36.946, 36.888, 36.946, + 36.482, 36.656, 36.366, 36.482, 36.192, 36.598, 36.946, 36.54, 36.54, + 36.888, 36.482, 36.656, 37.004, 36.134, 36.424, 36.83, 36.366, 36.772, + 36.656, 37.12, 36.424, 36.772, 36.714, 37.12, 36.482, 36.656, 37.004, + 37.004, 36.83, 36.772, 36.656, 37.004, 36.888, 37.062, 36.656, 36.598, + 36.772, 36.54, 36.598, 36.54, 36.83, 36.714, 36.714, 36.54, 36.83, + 36.714, 37.12, 36.656, 36.83, 36.714, 36.656, 36.714, 36.656, 36.714, + 36.54, 36.54, 36.714, 36.598, 37.062, 36.598, 36.424, 36.83, 36.482, + 36.83, 36.25, 36.598, 36.888, 36.714, 36.192, 36.656, 36.888, 36.714, + 36.714, 37.062, 36.598, 36.888, 36.656, 37.178, 37.12, 36.83, 36.54, + 36.888, 36.714, 36.83, 36.83, 37.004, 36.714, 36.83, 36.888, 36.598, + 37.062, 36.482, 36.888, 36.656, 36.424, 36.888, 37.004, 36.83, 36.772, + 36.888, 37.062, 37.062, 37.294, 36.714, 36.888, 37.004, 37.062, 37.178, + 37.12, 37.004, 36.83, 36.83, 37.178, 37.178, 36.714, 36.714, 36.946, + 37.236, 37.294, 37.236, 37.352, 36.888, 37.12, 37.12, 37.004, 36.946, + 36.83, 37.062, 36.946, 37.294, 37.12, 37.004, 36.946, 37.294, 37.178, + 37.294, 37.062, 37.062, 37.062, 37.468, 36.888, 36.946, 36.888, 36.888, + 37.178, 36.946, 37.352, 37.12, 36.83, 37.236, 36.888, 37.12, 36.598, + 37.004, 36.946, 37.004, 37.062, 37.294, 37.12, 37.178, 37.236, 37.062, + 36.888, 37.12, 37.12, 37.352, 37.41, 37.004, 37.178, 37.236, 37.062, + 36.946, 36.946, 37.062, 37.178, 36.772, 36.946, 36.772, 37.352, 37.062, + 36.772, 37.178, 36.946, 37.236, 37.12, 37.294, 37.236, 37.468, 36.83, + 36.83, 37.468, 37.236, 37.584, 37.352, 37.468, 37.12, 37.12, 37.236, + 37.236, 36.714, 37.236, 37.062, 36.83, 37.178, 37.236, 37.468, 37.178, + 37.236, 37.12, 37.12, 37.12, 37.062, 37.178, 37.004, 37.178, 37.236, + 37.352, 37.352, 37.236, 37.352, 37.062, 36.946, 37.004, 37.352, 37.062, + 37.004, 37.062, 37.12, 37.004, 37.062, 37.236, 37.178, 37.178, 37.642, + 37.12, 37.41, 37.12, 37.178, 37.468, 37.352, 37.236, 37.352, 37.352, + 37.236, 37.352, 37.584, 37.294, 37.294, 37.062, 37.12, 37.236, 37.004, + 37.062, 36.714, 37.294, 37.12, 37.758, 37.41, 37.584, 37.294, 37.236, + 37.062, 37.294, 37.062, 37.178, 37.41, 37.352, 37.41, 37.526, 37.352, + 37.526, 37.874, 37.004, 37.526, 37.178, 37.294, 37.584, 37.642, 37.41, + 37.178, 37.352, 36.946, 37.41, 37.584, 37.584, 37.236, 37.41, 37.41, + 37.352, 37.41, 37.41, 37.236, 37.41, 37.294, 37.236, 37.352, 37.642, + 37.352, 37.874, 37.178, 37.468, 37.352, 37.468, 37.41, 37.468, 37.468, + 37.41, 37.468, 37.584, 37.352, 38.106, 37.584, 37.584, 37.642, 37.236, + 37.874, 37.932, 37.178, 37.526, 37.468, 37.468, 37.584, 37.99, 37.526, + 37.584, 37.642, 37.874, 37.816, 37.41, 37.526, 37.7, 37.526, 37.236, + 37.12, 37.932, 37.468, 37.7, 37.352, 37.7, 37.584, 37.932, 37.584, + 37.816, 37.99, 37.932, 37.758, 37.758, 37.352, 37.642, 37.816, 37.816, + 37.99, 37.758, 38.048, 37.932, 37.874, 37.7, 38.106, 37.584, 37.7, + 37.758, 37.7, 37.584, 37.758, 37.642, 37.468, 37.584, 37.932, 37.99, + 37.526, 37.584, 37.642, 37.816, 37.758, 37.758, 37.816, 37.642, 37.99, + 37.526, 38.222, 37.7, 37.758, 37.816, 37.584, 37.758, 37.874, 37.816, + 37.816, 37.816, 37.99, 37.7, 37.41, 37.816, 37.758, 37.7, 37.758, + 37.468, 37.526, 37.932, 37.874, 37.526, 37.584, 37.41, 37.874, 37.7, + 37.7, 37.584, 37.468, 37.758, 37.7, 37.874, 37.758, 37.642, 37.584, + 38.048, 37.874, 37.758, 37.584, 38.048, 37.642, 37.932, 37.99, 38.338, + 37.874, 37.874, 37.874, 37.99, 37.99, 38.222, 38.048, 38.048, 38.222, + 37.932, 38.106, 37.99, 37.816, 37.816, 38.048, 38.454, 37.932, 38.048, + 38.164, 37.758, 38.396, 38.164, 38.222, 38.164, 37.932, 37.874, 38.106, + 38.164, 38.106, 38.57, 38.28, 38.106, 37.99, 37.932, 37.99, 38.222, + 37.99, 38.164, 38.28, 38.396, 38.28, 38.338, 37.932, 37.874, 38.222, + 38.512, 38.164, 38.28, 38.28, 38.628, 38.28, 38.164, 38.512, 38.512, + 38.512, 37.874, 38.686, 38.57, 38.86, 38.106, 38.222, 38.396, 38.28, + 38.222, 38.628, 38.338, 38.164, 38.454, 38.512, 38.628, 38.512, 38.512, + 38.802, 38.86, 38.628, 38.57, 38.628, 38.048, 38.918, 38.686, 38.57, + 38.802, 38.512, 38.57, 39.034, 38.744, 38.454, 38.802, 38.802, 39.092, + 38.976, 38.976, 38.802, 38.628, 38.802, 38.396, 38.744, 38.802, 38.686, + 38.918, 38.744, 38.222, 38.86, 38.628, 38.744, 38.686, 38.918, 38.628, + 39.034, 38.57, 38.802, 38.686, 38.86, 38.744, 38.86, 38.86, 38.396, + 38.628, 38.918, 38.57, 38.976, 38.57, 38.976, 38.628, 38.86, 39.092, + 38.744, 38.918, 38.86, 38.918, 38.976, 39.034, 38.976, 39.382, 38.976, + 39.382, 39.034, 38.454, 39.266, 39.324, 38.918, 39.266, 38.918, 38.976, + 38.512, 39.324, 38.976, 39.034, 38.918, 38.86, 39.382, 38.918, 38.686, + 39.092, 38.802, 39.092, 39.208, 38.918, 38.86, 39.034, 39.034, 39.034, + 39.034, 38.976, 39.15, 39.092, 38.918, 39.208, 39.034, 39.208, 39.092, + 39.034, 39.324, 39.208, 39.266, 39.498, 39.034, 39.266, 39.208, 39.208, + 39.092, 39.208, 38.976, 38.976, 39.266, 39.208, 38.976, 39.034, 39.15, + 39.092, 38.802, 39.034, 39.034, 39.15, 39.092, 39.672, 39.324, 39.092, + 39.44, 39.15, 39.44, 39.556, 39.44, 39.324, 39.614, 39.382, 38.86, + 39.498, 39.208, 39.092, 38.976, 38.976, 38.976, 39.266, 39.498, 38.976, + 39.034, 39.382, 39.382, 39.382, 39.498, 39.324, 39.266, 39.266, 39.324, + 39.034, 39.208, 39.382, 39.73, 39.266, 39.498, 39.208, 39.556, 39.208, + 39.498, 39.672, 39.44, 39.324, 39.324, 39.614, 39.44, 39.324, 39.44, + 39.556, 39.092, 39.382, 39.324, 39.208, 39.556, 39.44, 39.324, 39.266, + 39.324, 39.44, 39.672, 39.382, 39.382, 39.15, 39.324, 39.556, 39.672, + 39.556, 39.44, 39.498, 39.498, 39.44, 39.382, 39.498, 39.44, 39.44, + 39.788, 39.382, 39.266, 39.266, 39.44, 39.324, 39.788, 39.962, 39.614, + 39.556, 39.614, 39.556, 39.382, 39.614, 39.672, 39.672, 39.73, 39.672, + 39.672, 39.962, 39.44, 39.44, 39.788, 39.672, 39.44, 39.382, 39.556, + 39.382, 39.614, 39.556, 39.556, 39.498, 39.556, 39.614, 39.73, 40.02, + 39.672, 39.44, 39.614, 39.788, 39.614, 40.02, 39.788, 39.498, 39.672, + 40.02, 39.382, 39.904, 39.44, 39.788, 39.846, 39.962, 40.078, 39.556, + 39.672, 39.556, 40.02, 39.614, 39.788, 40.02, 39.614, 39.73, 39.73, + 39.846, 40.078, 39.846, 39.614, 39.498, 39.788, 39.73, 39.904, 39.846, + 40.02, 39.672, 39.962, 39.788, 40.078, 40.078, 39.788, 39.846, 39.846, + 40.136, 40.078, 40.078, 40.02, 39.788, 40.02, 40.194, 40.136, 40.426, + 39.962, 40.252, 40.252, 40.368, 39.904, 40.02, 40.426, 40.194, 40.02, + 40.194, 40.02, 39.904, 40.484, 40.02, 40.136, 40.542, 40.194, 40.484, + 40.194, 40.078, 40.31, 40.31, 40.426, 40.02, 40.368, 40.368, 40.89, + 40.542, 40.716, 40.368, 40.6, 40.136, 40.6, 40.89, 40.194, 40.832, + 40.774, 40.426, 40.194, 40.426, 40.658, 40.716, 40.6, 40.6, 40.484, + 40.426, 40.542, 40.658, 40.774, 40.484, 40.6, 40.6, 40.774, 40.948, + 40.89, 40.832, 40.716, 40.774, 40.484, 40.89, 40.774, 41.006, 40.658, + 41.064, 40.31, 40.542, 40.774, 40.716, 40.832, 40.658, 40.484, 40.832, + 40.89, 40.89, 41.006, 40.832, 40.716, 40.948, 40.774, 40.774, 40.716, + 41.354, 41.006, 40.832, 41.412, 41.18, 40.716, 41.18, 41.18, 40.948, + 41.006, 41.122, 41.122, 41.18, 40.716, 40.948, 41.18, 41.122, 41.064, + 41.238, 41.238, 40.774, 41.354, 40.89, 40.832, 41.47, 41.18, 41.354, + 41.412, 41.354, 41.122, 41.238, 41.064, 41.122, 41.238, 41.238, 40.948, + 41.586, 41.238, 41.064, 41.296, 41.354, 41.586, 41.47, 41.354, 41.296, + 41.296, 41.122, 41.122, 41.354, 41.876, 41.528, 41.586, 41.528, 41.354, + 41.644, 41.296, 41.47, 41.354, 41.354, 41.18, 41.644, 41.354, 41.528, + 41.412, 41.702, 41.296, 41.876, 41.354, 41.528, 41.818, 41.47, 41.876, + 41.644, 41.528, 42.108, 41.76, 41.818, 41.818, 41.76, 41.702, 41.412, + 41.644, 41.354, 41.818, 41.47, 41.76, 41.876, 42.108, 41.354, 41.992, + 41.586, 41.876, 41.586, 41.76, 41.644, 42.224, 42.05, 41.818, 41.76, + 42.108, 41.876, 41.992, 41.528, 41.992, 41.644, 41.876, 41.876, 41.644, + 42.166, 41.76, 41.992, 41.818, 41.76, 42.108, 41.876, 41.876, 42.63, + 41.76, 41.702, 41.876, 42.108, 41.586, 42.224, 41.876, 41.876, 42.108, + 42.108, 41.76, 41.76, 41.76, 42.05, 42.05, 42.224, 41.934, 42.224, + 41.934, 41.992, 42.05, 41.876, 41.934, 41.992, 42.34, 41.876, 42.34, + 42.05, 42.108, 42.224, 42.224, 42.05, 42.05, 42.224, 42.108, 42.34, + 42.166, 42.05, 42.282, 42.108, 42.282, 42.108, 42.282, 41.992, 42.108, + 42.108, 42.224, 42.108, 42.456, 41.934, 42.456, 42.514, 41.992, 42.398, + 42.572, 42.166, 42.108, 42.05, 42.05, 42.282, 42.282, 42.34, 42.166, + 42.166, 42.05, 42.572, 42.05, 42.108, 42.224, 42.166, 42.224, 42.514, + 42.166, 42.282, 42.746, 42.63, 42.34, 42.63, 42.108, 42.63, 42.456, + 42.34, 42.34, 42.572, 42.282, 42.456, 42.63, 42.63, 42.282, 42.166, + 42.514, 42.572, 42.34, 42.862, 42.398, 42.572, 42.746, 42.688, 42.746, + 42.688, 42.862, 42.862, 43.094, 42.456, 42.456, 42.63, 42.63, 42.746, + 42.34, 42.456, 42.688, 42.514, 42.978, 42.63, 42.804, 42.572, 42.514, + 42.688, 42.978, 42.862, 42.804, 42.398, 42.63, 43.442, 42.456, 42.688, + 42.63, 42.746, 43.094, 42.92, 42.63, 42.63, 42.688, 42.63, 42.398, + 42.746, 42.978, 42.63, 42.63, 42.92, 42.978, 43.094, 43.384, 43.036, + 43.21, 42.746, 42.978, 42.804, 42.804, 43.384, 42.978, 42.92, 42.92, + 43.036, 42.862, 43.442, 42.688, 43.094, 43.21, 42.978, 43.036, 42.978, + 42.978, 43.036, 42.456, 43.036, 43.036, 43.036, 43.21, 43.094, 42.862, + 42.804, 43.384, 43.21, 43.268, 43.094, 43.036, 43.442, 42.978, 43.152, + 42.688, 43.442, 43.036, 43.036, 43.094, 42.978, 43.094, 42.746, 42.978, + 42.862, 43.094, 42.978, 43.094, 43.5, 43.442, 43.384, 43.21, 43.036, + 43.268, 43.094, 43.268, 43.5, 43.094, 43.268, 42.92, 42.978, 43.094, + 43.268, 43.674, 42.978, 42.978, 43.21, 43.442, 43.5, 43.268, 43.326, + 43.268, 43.5, 43.442, 43.384, 43.384, 43.21, 43.21, 43.558, 43.848, + 42.978, 43.442, 43.268, 43.21, 43.5, 43.268, 43.732, 43.384, 43.384, + 43.21, 43.5, 43.674, 43.326, 43.79, 43.674, 43.558, 43.21, 43.442, + 43.268, 43.268, 43.384, 43.616, 43.268, 43.442, 43.152, 43.848, 43.558, + 43.21, 43.5, 43.442, 43.558, 43.674, 43.558, 43.326, 44.022, 43.268, + 43.79, 43.732, 43.384, 43.848, 43.384, 43.79, 43.152, 43.384, 43.732, + 43.558, 43.21, 43.558, 43.558, 43.5, 43.442, 43.442, 43.5, 43.906, + 43.732, 43.558, 43.674, 43.442, 43.848, 43.732, 43.732, 43.326, 43.964, + 43.674, 43.558, 43.79, 43.268, 43.616, 43.848, 43.732, 43.326, 43.384, + 43.848, 43.848, 43.384, 43.442, 43.442, 43.732, 43.442, 43.5, 43.616, + 43.674, 43.384, 43.442, 43.674, 43.384, 43.616, 43.616, 43.616, 43.268, + 43.5, 43.5, 43.326, 43.384, 43.906, 43.558, 43.5, 43.21, 43.442, + 43.558, 44.022, 43.906, 43.268, 43.558, 43.616, 43.732, 43.674, 43.442, + 43.384, 43.558, 43.5, 43.442, 43.674, 43.442, 43.79, 43.442, 43.268, + 43.732, 43.906, 43.326, 43.326, 43.616, 43.616, 43.79, 43.5, 43.79, + 43.384, 43.674, 43.616, 43.558, 43.616, 43.674, 43.326, 43.906, 43.558, + 43.79, 43.558, 43.558, 43.21, 43.674, 43.152, 43.442, 43.094, 43.616, + 43.674, 43.616, 43.442, 43.674, 43.732, 43.558, 43.5, 42.688, 43.616, + 43.5, 43.442, 43.616, 43.326, 43.558, 43.906, 43.152, 43.384, 43.384, + 43.5, 43.384, 43.094, 43.442, 43.616, 43.152, 43.152, 43.5, 43.558, + 43.616, 43.152, 43.326, 43.732, 43.326, 43.152, 43.152, 43.5, 43.326, + 43.036, 43.5, 43.268, 43.326, 43.616, 43.326, 43.326, 43.036, 43.036, + 43.152, 43.094, 43.152, 42.572, 42.92, 43.152, 42.978, 43.094, 43.036, + 42.804, 43.21, 42.92, 42.92, 42.688, 42.92, 42.688, 43.036, 43.384, + 42.804, 43.036, 42.862, 43.094, 43.268, 42.804, 43.268, 42.92, 42.746, + 42.572, 42.746, 42.92, 43.094, 42.978, 42.456, 42.862, 42.978, 42.92, + 42.862, 42.688, 42.862, 42.63, 42.746, 42.804, 42.572, 42.92, 42.862, + 42.572, 42.92, 42.746, 42.746, 42.746, 42.746, 42.804, 42.978, 42.92, + 42.746, 42.572, 42.746, 42.63, 42.804, 42.804, 42.456, 42.224, 42.688, + 42.63, 42.63, 42.862, 42.572, 42.514, 42.572, 42.572, 42.514, 42.224, + 42.34, 42.108, 42.456, 42.34, 42.224, 42.456, 42.746, 42.398, 42.398, + 42.398, 42.166, 42.514, 42.456, 41.876, 42.108, 42.514, 41.934, 42.282, + 42.166, 42.398, 42.108, 42.282, 42.108, 41.702, 42.108, 42.05, 42.108, + 42.166, 41.818, 41.876, 41.76, 41.992, 41.818, 42.05, 41.992, 41.76, + 42.34, 41.876, 42.166, 41.934, 41.702, 41.76, 41.586, 41.76, 41.76, + 41.644, 41.528, 41.702, 41.818, 41.354, 41.934, 41.18, 41.586, 41.586, + 41.586, 41.18, 41.412, 41.644, 41.528, 41.412, 41.18, 41.238, 41.354, + 41.296, 41.354, 41.18, 41.412, 41.528, 41.122, 41.18, 41.122, 41.238, + 41.122, 41.238, 41.064, 40.948, 40.832, 41.122, 40.832, 41.006, 40.89, + 40.89, 40.484, 40.658, 40.716, 40.89, 40.89, 40.658, 40.89, 41.18, + 41.064, 40.542, 40.658, 40.832, 40.89, 40.484, 40.6, 40.368, 40.658, + 40.774, 40.542, 40.194, 40.078, 39.73, 40.31, 40.02, 40.252, 40.484, + 40.252, 40.136, 40.252, 39.73, 39.846, 40.02, 39.788, 39.73, 39.788, + 39.962, 39.324, 39.672, 39.904, 39.846, 39.788, 39.15, 39.556, 39.846, + 39.266, 39.382, 39.614, 39.44, 39.904, 39.556, 39.324, 39.324, 39.44, + 39.208, 38.976, 39.324, 38.976, 39.034, 39.15, 39.092, 39.208, 38.86, + 39.324, 39.034, 38.628, 38.454, 38.802, 38.86, 39.208, 38.338, 38.744, + 38.628, 38.57, 38.396, 38.164, 38.802, 37.99, 38.338, 38.396, 38.222, + 38.106, 38.57, 38.28, 38.454, 38.164, 38.222, 38.222, 37.7, 38.106, + 37.7, 37.99, 37.932, 37.758, 37.7, 37.758, 37.584, 37.816, 37.352, + 37.352, 37.7, 37.584, 37.41, 37.584, 37.236, 37.642, 37.004, 37.526, + 37.12, 37.236, 37.294, 36.772, 37.004, 36.598, 36.714, 36.714, 36.656, + 36.482, 36.598, 36.946, 36.482, 36.946, 36.482, 36.54, 36.366, 36.308, + 36.424, 36.308, 36.308, 35.902, 36.424, 35.96, 36.192, 36.018, 36.076, + 35.902, 35.902, 35.786, 35.67, 35.728, 35.612, 35.902, 35.96, 35.554, + 35.786, 35.612, 35.612, 35.496, 35.322, 35.496, 35.264, 35.148, 35.438, + 34.742, 35.496, 35.322, 34.916, 35.38, 34.974, 34.858, 34.626, 34.916, + 34.858, 34.742, 34.626, 34.8, 34.162}}; diff --git a/src/tests/mock_sensors/test-mock-data.h b/src/tests/mock_sensors/test-mock-data.h index 13c3d35ef1f467617ac6d7d445f7a254855507ab..191c01884722e9859a05f0edb45a51f27f3e9d7a 100644 --- a/src/tests/mock_sensors/test-mock-data.h +++ b/src/tests/mock_sensors/test-mock-data.h @@ -1,20 +1,19 @@ -/* - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Mozzarelli - * +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -23,9 +22,17 @@ #pragma once -static const unsigned PRESSURE_DATA_SIZE = 665; -static const unsigned GPS_DATA_SIZE = 333; - +static const unsigned PRESSURE_DATA_SIZE = 2570; extern const float SIMULATED_PRESSURE[PRESSURE_DATA_SIZE]; + +static const unsigned GPS_DATA_SIZE = 2570; extern const float SIMULATED_LAT[GPS_DATA_SIZE]; -extern const float SIMULATED_LON[GPS_DATA_SIZE]; \ No newline at end of file +extern const float SIMULATED_LON[GPS_DATA_SIZE]; +extern const float SIMULATED_VNORD[GPS_DATA_SIZE]; +extern const float SIMULATED_VEAST[GPS_DATA_SIZE]; + +static const unsigned IMU_DATA_SIZE = 2570; +static const unsigned MOTION_SENSOR_AXIS_NUM = 3; +extern const float ACCELEROMETER_DATA[MOTION_SENSOR_AXIS_NUM][IMU_DATA_SIZE]; +extern const float GYROSCOPE_DATA[MOTION_SENSOR_AXIS_NUM][IMU_DATA_SIZE]; +extern const float MAGNETOMETER_DATA[MOTION_SENSOR_AXIS_NUM][IMU_DATA_SIZE]; \ No newline at end of file diff --git a/src/tests/mock_sensors/test-mock-sensors.cpp b/src/tests/mock_sensors/test-mock-sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d21fa7b4e901f8da59ecb5d5ef64c691478162be --- /dev/null +++ b/src/tests/mock_sensors/test-mock-sensors.cpp @@ -0,0 +1,82 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Common.h> +#include <mocksensors/MockSensors.h> + +using namespace DeathStackBoard; + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + MockGPS gps; + MockPressureSensor p_sensor; + MockIMU imu; + TestSensor test_sensor; + + int i = 0; + + while (true) + { + + if (i == 10) + { + gps.signalLiftoff(); + p_sensor.signalLiftoff(); + TRACE("---------- LIFTOFF !!! ----------\n\n"); + } + + gps.sample(); + GPSData gps_data = gps.getLastSample(); + TRACE("GPS SAMPLE: \n"); + TRACE("lat = %f \n", gps_data.latitude); + TRACE("lon = %f \n\n", gps_data.longitude); + + p_sensor.sample(); + PressureData pressure = p_sensor.getLastSample(); + TRACE("PRESSURE SAMPLE: \n"); + TRACE("pressure = %f \n\n", pressure.press); + + imu.sample(); + MockIMUData imu_data = imu.getLastSample(); + TRACE("IMU SAMPLE: \n"); + TRACE("accel x = %f \n", imu_data.accel_x); + TRACE("accel y = %f \n", imu_data.accel_y); + TRACE("accel z = %f \n", imu_data.accel_z); + TRACE("gyro x = %f \n", imu_data.gyro_x); + TRACE("gyro y = %f \n", imu_data.gyro_y); + TRACE("gyro z = %f \n", imu_data.gyro_z); + TRACE("magneto x = %f \n", imu_data.mag_x); + TRACE("magneto y = %f \n", imu_data.mag_y); + TRACE("magneto z = %f \n\n", imu_data.mag_z); + + test_sensor.sample(); + TestData test_data = test_sensor.getLastSample(); + TRACE("TEST SAMPLE: \n"); + TRACE("value = %f \n\n", test_data.value); + + miosix::Thread::sleep(1000); + + i++; + } +} \ No newline at end of file diff --git a/src/tests/ram_test/README.md b/src/tests/ram_test/README.md new file mode 100644 index 0000000000000000000000000000000000000000..c57f1871281a713372c4db180dfb9777346fd16c --- /dev/null +++ b/src/tests/ram_test/README.md @@ -0,0 +1,13 @@ +# RAM Test + +This entrypoint is useful to test the external RAM of a board. + +It writes into the external RAM and then reads back the content to check its correctness and also the memory retention capability. + +### Usage + +* In `ramtest.cpp` : + - Set the `ramBase` value to the starting address of the external memory, as defined in the linker script (e.g. `0xd0000000`). + - Set the `ramSize` value to the size of the external memory in bytes, as defined in the linker script (e.g. for 8 MB : `8*1024*1024`). +* `__ENABLE_XRAM` must be defined, in Miosix's `config/Makefile.inc` (in the section corresponding to your board) or in `sbs.conf` (in the `ramtest` entrypoint section). +* In `miosix-kernel/miosix/config/Makefile.inc`, in the section of your board, select the correct linker script. _The correct linker is not the one that defines the external RAM_. **You must select (uncomment) the linker script for your board that ends with "rom"**. This is a must, because the linker script could place some executable sections (such as the `bss`) in the external ram : the `ramtest` will then crash when trying to write to those same addresses. \ No newline at end of file diff --git a/src/tests/ram_test/sha1.cpp b/src/tests/ram_test/sha1.cpp index 19564c706a875a5bca792a920b49d878795196c9..83d735562c14e662f5b0c27742370a620a523465 100644 --- a/src/tests/ram_test/sha1.cpp +++ b/src/tests/ram_test/sha1.cpp @@ -1,10 +1,32 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + /* * sha1.cpp * * Copyright (C) 1998, 2009 * Paul E. Jones <paulej@packetizer.com> * All Rights Reserved. - * + * * Freeware Public License (FPL) * * This software is licensed as "freeware." Permission to distribute @@ -49,10 +71,9 @@ * */ - #include "sha1.h" -/* +/* * SHA1 * * Description: @@ -67,12 +88,9 @@ * Comments: * */ -SHA1::SHA1() -{ - Reset(); -} +SHA1::SHA1() { Reset(); } -/* +/* * ~SHA1 * * Description: @@ -92,7 +110,7 @@ SHA1::~SHA1() // The destructor does nothing } -/* +/* * Reset * * Description: @@ -114,17 +132,17 @@ void SHA1::Reset() Length_High = 0; Message_Block_Index = 0; - H[0] = 0x67452301; - H[1] = 0xEFCDAB89; - H[2] = 0x98BADCFE; - H[3] = 0x10325476; - H[4] = 0xC3D2E1F0; + H[0] = 0x67452301; + H[1] = 0xEFCDAB89; + H[2] = 0x98BADCFE; + H[3] = 0x10325476; + H[4] = 0xC3D2E1F0; - Computed = false; - Corrupted = false; + Computed = false; + Corrupted = false; } -/* +/* * Result * * Description: @@ -144,7 +162,7 @@ void SHA1::Reset() */ bool SHA1::Result(unsigned *message_digest_array) { - int i; // Counter + int i; // Counter if (Corrupted) { @@ -157,7 +175,7 @@ bool SHA1::Result(unsigned *message_digest_array) Computed = true; } - for(i = 0; i < 5; i++) + for (i = 0; i < 5; i++) { message_digest_array[i] = H[i]; } @@ -165,7 +183,7 @@ bool SHA1::Result(unsigned *message_digest_array) return true; } -/* +/* * Input * * Description: @@ -183,8 +201,7 @@ bool SHA1::Result(unsigned *message_digest_array) * Comments: * */ -void SHA1::Input( const unsigned char *message_array, - unsigned length) +void SHA1::Input(const unsigned char *message_array, unsigned length) { if (!length) { @@ -197,19 +214,19 @@ void SHA1::Input( const unsigned char *message_array, return; } - while(length-- && !Corrupted) + while (length-- && !Corrupted) { Message_Block[Message_Block_Index++] = (*message_array & 0xFF); Length_Low += 8; - Length_Low &= 0xFFFFFFFF; // Force it to 32 bits + Length_Low &= 0xFFFFFFFF; // Force it to 32 bits if (Length_Low == 0) { Length_High++; - Length_High &= 0xFFFFFFFF; // Force it to 32 bits + Length_High &= 0xFFFFFFFF; // Force it to 32 bits if (Length_High == 0) { - Corrupted = true; // Message is too long + Corrupted = true; // Message is too long } } @@ -222,7 +239,7 @@ void SHA1::Input( const unsigned char *message_array, } } -/* +/* * Input * * Description: @@ -242,13 +259,12 @@ void SHA1::Input( const unsigned char *message_array, * Comments: * */ -void SHA1::Input( const char *message_array, - unsigned length) +void SHA1::Input(const char *message_array, unsigned length) { - Input((unsigned char *) message_array, length); + Input((unsigned char *)message_array, length); } -/* +/* * Input * * Description: @@ -264,12 +280,9 @@ void SHA1::Input( const char *message_array, * Comments: * */ -void SHA1::Input(unsigned char message_element) -{ - Input(&message_element, 1); -} +void SHA1::Input(unsigned char message_element) { Input(&message_element, 1); } -/* +/* * Input * * Description: @@ -287,10 +300,10 @@ void SHA1::Input(unsigned char message_element) */ void SHA1::Input(char message_element) { - Input((unsigned char *) &message_element, 1); + Input((unsigned char *)&message_element, 1); } -/* +/* * operator<< * * Description: @@ -308,11 +321,11 @@ void SHA1::Input(char message_element) * Each character is assumed to hold 8 bits of information. * */ -SHA1& SHA1::operator<<(const char *message_array) +SHA1 &SHA1::operator<<(const char *message_array) { const char *p = message_array; - while(*p) + while (*p) { Input(*p); p++; @@ -321,7 +334,7 @@ SHA1& SHA1::operator<<(const char *message_array) return *this; } -/* +/* * operator<< * * Description: @@ -339,11 +352,11 @@ SHA1& SHA1::operator<<(const char *message_array) * Each character is assumed to hold 8 bits of information. * */ -SHA1& SHA1::operator<<(const unsigned char *message_array) +SHA1 &SHA1::operator<<(const unsigned char *message_array) { const unsigned char *p = message_array; - while(*p) + while (*p) { Input(*p); p++; @@ -352,7 +365,7 @@ SHA1& SHA1::operator<<(const unsigned char *message_array) return *this; } -/* +/* * operator<< * * Description: @@ -369,14 +382,14 @@ SHA1& SHA1::operator<<(const unsigned char *message_array) * The character is assumed to hold 8 bits of information. * */ -SHA1& SHA1::operator<<(const char message_element) +SHA1 &SHA1::operator<<(const char message_element) { - Input((unsigned char *) &message_element, 1); + Input((unsigned char *)&message_element, 1); return *this; } -/* +/* * operator<< * * Description: @@ -393,14 +406,14 @@ SHA1& SHA1::operator<<(const char message_element) * The character is assumed to hold 8 bits of information. * */ -SHA1& SHA1::operator<<(const unsigned char message_element) +SHA1 &SHA1::operator<<(const unsigned char message_element) { Input(&message_element, 1); return *this; } -/* +/* * ProcessMessageBlock * * Description: @@ -421,31 +434,27 @@ SHA1& SHA1::operator<<(const unsigned char message_element) */ void SHA1::ProcessMessageBlock() { - const unsigned K[] = { // Constants defined for SHA-1 - 0x5A827999, - 0x6ED9EBA1, - 0x8F1BBCDC, - 0xCA62C1D6 - }; - int t; // Loop counter - unsigned temp; // Temporary word value - unsigned W[80]; // Word sequence - unsigned A, B, C, D, E; // Word buffers + const unsigned K[] = {// Constants defined for SHA-1 + 0x5A827999, 0x6ED9EBA1, 0x8F1BBCDC, 0xCA62C1D6}; + int t; // Loop counter + unsigned temp; // Temporary word value + unsigned W[80]; // Word sequence + unsigned A, B, C, D, E; // Word buffers /* * Initialize the first 16 words in the array W */ - for(t = 0; t < 16; t++) + for (t = 0; t < 16; t++) { - W[t] = ((unsigned) Message_Block[t * 4]) << 24; - W[t] |= ((unsigned) Message_Block[t * 4 + 1]) << 16; - W[t] |= ((unsigned) Message_Block[t * 4 + 2]) << 8; - W[t] |= ((unsigned) Message_Block[t * 4 + 3]); + W[t] = ((unsigned)Message_Block[t * 4]) << 24; + W[t] |= ((unsigned)Message_Block[t * 4 + 1]) << 16; + W[t] |= ((unsigned)Message_Block[t * 4 + 2]) << 8; + W[t] |= ((unsigned)Message_Block[t * 4 + 3]); } - for(t = 16; t < 80; t++) + for (t = 16; t < 80; t++) { - W[t] = CircularShift(1,W[t-3] ^ W[t-8] ^ W[t-14] ^ W[t-16]); + W[t] = CircularShift(1, W[t - 3] ^ W[t - 8] ^ W[t - 14] ^ W[t - 16]); } A = H[0]; @@ -454,47 +463,47 @@ void SHA1::ProcessMessageBlock() D = H[3]; E = H[4]; - for(t = 0; t < 20; t++) + for (t = 0; t < 20; t++) { - temp = CircularShift(5,A) + ((B & C) | ((~B) & D)) + E + W[t] + K[0]; + temp = CircularShift(5, A) + ((B & C) | ((~B) & D)) + E + W[t] + K[0]; temp &= 0xFFFFFFFF; E = D; D = C; - C = CircularShift(30,B); + C = CircularShift(30, B); B = A; A = temp; } - for(t = 20; t < 40; t++) + for (t = 20; t < 40; t++) { - temp = CircularShift(5,A) + (B ^ C ^ D) + E + W[t] + K[1]; + temp = CircularShift(5, A) + (B ^ C ^ D) + E + W[t] + K[1]; temp &= 0xFFFFFFFF; E = D; D = C; - C = CircularShift(30,B); + C = CircularShift(30, B); B = A; A = temp; } - for(t = 40; t < 60; t++) + for (t = 40; t < 60; t++) { - temp = CircularShift(5,A) + - ((B & C) | (B & D) | (C & D)) + E + W[t] + K[2]; + temp = CircularShift(5, A) + ((B & C) | (B & D) | (C & D)) + E + W[t] + + K[2]; temp &= 0xFFFFFFFF; E = D; D = C; - C = CircularShift(30,B); + C = CircularShift(30, B); B = A; A = temp; } - for(t = 60; t < 80; t++) + for (t = 60; t < 80; t++) { - temp = CircularShift(5,A) + (B ^ C ^ D) + E + W[t] + K[3]; + temp = CircularShift(5, A) + (B ^ C ^ D) + E + W[t] + K[3]; temp &= 0xFFFFFFFF; E = D; D = C; - C = CircularShift(30,B); + C = CircularShift(30, B); B = A; A = temp; } @@ -508,7 +517,7 @@ void SHA1::ProcessMessageBlock() Message_Block_Index = 0; } -/* +/* * PadMessage * * Description: @@ -539,14 +548,14 @@ void SHA1::PadMessage() if (Message_Block_Index > 55) { Message_Block[Message_Block_Index++] = 0x80; - while(Message_Block_Index < 64) + while (Message_Block_Index < 64) { Message_Block[Message_Block_Index++] = 0; } ProcessMessageBlock(); - while(Message_Block_Index < 56) + while (Message_Block_Index < 56) { Message_Block[Message_Block_Index++] = 0; } @@ -554,11 +563,10 @@ void SHA1::PadMessage() else { Message_Block[Message_Block_Index++] = 0x80; - while(Message_Block_Index < 56) + while (Message_Block_Index < 56) { Message_Block[Message_Block_Index++] = 0; } - } /* @@ -567,17 +575,16 @@ void SHA1::PadMessage() Message_Block[56] = (Length_High >> 24) & 0xFF; Message_Block[57] = (Length_High >> 16) & 0xFF; Message_Block[58] = (Length_High >> 8) & 0xFF; - Message_Block[59] = (Length_High) & 0xFF; + Message_Block[59] = (Length_High)&0xFF; Message_Block[60] = (Length_Low >> 24) & 0xFF; Message_Block[61] = (Length_Low >> 16) & 0xFF; Message_Block[62] = (Length_Low >> 8) & 0xFF; - Message_Block[63] = (Length_Low) & 0xFF; + Message_Block[63] = (Length_Low)&0xFF; ProcessMessageBlock(); } - -/* +/* * CircularShift * * Description: @@ -597,5 +604,5 @@ void SHA1::PadMessage() */ unsigned SHA1::CircularShift(int bits, unsigned word) { - return ((word << bits) & 0xFFFFFFFF) | ((word & 0xFFFFFFFF) >> (32-bits)); + return ((word << bits) & 0xFFFFFFFF) | ((word & 0xFFFFFFFF) >> (32 - bits)); } diff --git a/src/tests/ram_test/sha1.h b/src/tests/ram_test/sha1.h index 130e7c4a9bf5d19ace38980cd5ef2b943e1a7ad5..5e8280b57782b6c3c8b3497be05fbdc79524e25e 100644 --- a/src/tests/ram_test/sha1.h +++ b/src/tests/ram_test/sha1.h @@ -1,3 +1,25 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + /* * sha1.h * @@ -39,89 +61,87 @@ class SHA1 { - public: - - SHA1(); - virtual ~SHA1(); - - /* - * Re-initialize the class - */ - void Reset(); - - /* - * Returns the message digest - */ - bool Result(unsigned *message_digest_array); - - /* - * Provide input to SHA1 - */ - void Input( const unsigned char *message_array, - unsigned length); - void Input( const char *message_array, - unsigned length); - void Input(unsigned char message_element); - void Input(char message_element); - SHA1& operator<<(const char *message_array); - SHA1& operator<<(const unsigned char *message_array); - SHA1& operator<<(const char message_element); - SHA1& operator<<(const unsigned char message_element); - - private: - - /* - * Process the next 512 bits of the message - */ - void ProcessMessageBlock(); - - /* - * Pads the current message block to 512 bits - */ - void PadMessage(); - - /* - * Performs a circular left shift operation - */ - inline unsigned CircularShift(int bits, unsigned word); - - unsigned H[5]; // Message digest buffers - - unsigned Length_Low; // Message length in bits - unsigned Length_High; // Message length in bits - - unsigned char Message_Block[64]; // 512-bit message blocks - int Message_Block_Index; // Index into message block array - - bool Computed; // Is the digest computed? - bool Corrupted; // Is the message digest corruped? - +public: + SHA1(); + virtual ~SHA1(); + + /* + * Re-initialize the class + */ + void Reset(); + + /* + * Returns the message digest + */ + bool Result(unsigned* message_digest_array); + + /* + * Provide input to SHA1 + */ + void Input(const unsigned char* message_array, unsigned length); + void Input(const char* message_array, unsigned length); + void Input(unsigned char message_element); + void Input(char message_element); + SHA1& operator<<(const char* message_array); + SHA1& operator<<(const unsigned char* message_array); + SHA1& operator<<(const char message_element); + SHA1& operator<<(const unsigned char message_element); + +private: + /* + * Process the next 512 bits of the message + */ + void ProcessMessageBlock(); + + /* + * Pads the current message block to 512 bits + */ + void PadMessage(); + + /* + * Performs a circular left shift operation + */ + inline unsigned CircularShift(int bits, unsigned word); + + unsigned H[5]; // Message digest buffers + + unsigned Length_Low; // Message length in bits + unsigned Length_High; // Message length in bits + + unsigned char Message_Block[64]; // 512-bit message blocks + int Message_Block_Index; // Index into message block array + + bool Computed; // Is the digest computed? + bool Corrupted; // Is the message digest corruped? }; // Added by TFT -- begin -#include <string> #include <cstdio> +#include <string> -const int sha1size=20; +const int sha1size = 20; inline std::string sha1(const std::string& message) { - SHA1 hash; - hash.Input(message.c_str(),message.length()); - unsigned int r[sha1size/4]; - hash.Result(r); - char resultstr[2*sha1size+1]; - std::sprintf(resultstr,"%04x%04x%04x%04x%04x",r[0],r[1],r[2],r[3],r[4]); - return std::string(resultstr); + SHA1 hash; + hash.Input(message.c_str(), message.length()); + unsigned int r[sha1size / 4]; + hash.Result(r); + char resultstr[2 * sha1size + 1]; + std::sprintf(resultstr, "%04x%04x%04x%04x%04x", r[0], r[1], r[2], r[3], + r[4]); + return std::string(resultstr); } -inline void sha1binary(const std::string& message, unsigned char digest[sha1size]) +inline void sha1binary(const std::string& message, + unsigned char digest[sha1size]) { - SHA1 hash; - hash.Input(message.c_str(),message.length()); - unsigned int r[sha1size/4]; - hash.Result(r); - for(int i=0;i<sha1size;i++) digest[i]=(r[i/4] >> 8*(3-i%4)) & 0xff; + SHA1 hash; + hash.Input(message.c_str(), message.length()); + unsigned int r[sha1size / 4]; + hash.Result(r); + for (int i = 0; i < sha1size; i++) + digest[i] = (r[i / 4] >> 8 * (3 - i % 4)) & 0xff; } // Added by TFT -- end diff --git a/src/tests/test-ada-dpl-simulation.cpp b/src/tests/test-ada-dpl-simulation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fb03424586d6a98669fb4b4936f4c30b340ece15 --- /dev/null +++ b/src/tests/test-ada-dpl-simulation.cpp @@ -0,0 +1,210 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> +#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <Deployment/DeploymentController.h> + +#include <random> + +#include "PinHandler/PinHandler.h" +#include "catch/ada/ada_kalman_p/test-ada-data.h" +#include "events/EventBroker.h" +#include "events/Events.h" +#include "events/utils/EventCounter.h" + +using namespace DeathStackBoard; + +constexpr float NOISE_STD_DEV = 5; // Noise varaince +constexpr float LSB = 28; + +class MockPressureSensor : public Sensor<PressureData> +{ +public: + MockPressureSensor() {} + + bool init() override { return true; } + + bool selfTest() override { return true; } + + PressureData sampleImpl() override + { + float press = 0.0; + + if (before_liftoff) + { + press = addNoise(ADA_SIMULATED_PRESSURE[0]); + } + else + { + if (i < DATA_SIZE) + { + press = addNoise(ADA_SIMULATED_PRESSURE[i++]); + } + else + { + press = addNoise(ADA_SIMULATED_PRESSURE[DATA_SIZE - 1]); + } + } + + return PressureData{TimestampTimer::getTimestamp(), press}; + } + + void signalLiftoff() { before_liftoff = false; } + +private: + volatile bool before_liftoff = true; + volatile unsigned int i = 0; // Last index + std::default_random_engine generator{1234567}; + std::normal_distribution<float> distribution{0.0f, NOISE_STD_DEV}; + + float addNoise(float sample) + { + return quantization(sample + distribution(generator)); + } + + float quantization(float sample) { return round(sample / LSB) * LSB; } +}; + +class MockGPSSensor : public Sensor<GPSData> +{ +public: + bool init() { return true; } + bool selfTest() { return true; } + GPSData sampleImpl() { return GPSData{}; } +}; + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + MockPressureSensor baro; + MockGPSSensor gps; + + if (!baro.init()) + { + TRACE("Barometer init failed ... \n"); + for (;;) + ; + } + + ADAController<PressureData, GPSData> ada_controller(baro, gps); + DeploymentController dpl_controller; + PinHandler pin_handler; + + sEventBroker->start(); + EventCounter counter{*sEventBroker}; + counter.subscribe(TOPIC_ADA); + counter.subscribe(TOPIC_FLIGHT_EVENTS); + + Thread::sleep(1000); + + if (!pin_handler.start()) + { + TRACE("[PinHnalder] Failed to start \n"); + } + + ada_controller.start(); + dpl_controller.start(); + + Thread::sleep(1000); + + // Enter ADA calibration state + sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_ADA); + Thread::sleep(100); + + // Send baro calibration samples for ADA + for (unsigned i = 0; i < CALIBRATION_BARO_N_SAMPLES + 5; i++) + { + baro.sample(); + Thread::sleep(50); + ada_controller.update(); + } + + // Send set deployment altitude + ada_controller.setDeploymentAltitude(200); + baro.sample(); + Thread::sleep(50); + ada_controller.update(); + // Send set altitude ref + ada_controller.setReferenceAltitude(1300); + baro.sample(); + Thread::sleep(50); + ada_controller.update(); + // Send set temperature ref + ada_controller.setReferenceTemperature(15); + baro.sample(); + Thread::sleep(50); + ada_controller.update(); + + // wait for launch pin detach + while (counter.getCount({EV_UMBILICAL_DETACHED}) == 0) + { + Thread::sleep(100); + } + + TRACE("Sending EV_LIFTOFF ... \n"); + // Send liftoff event + sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + baro.signalLiftoff(); + + bool first_apogee_detection = true; + bool first_dpl_altitude_detection = true; + + for (unsigned int i = 0; i < DATA_SIZE; i++) + { + baro.sample(); + Thread::sleep(50); + ada_controller.update(); + + TRACE("%u, %.2f, %.2f \n", i, ada_controller.getADAData().vert_speed, + ada_controller.getADAData().msl_altitude); + + if (ada_controller.getStatus().apogee_reached && first_apogee_detection) + { + TRACE("\n\n******* APOGEE DETECTED! ******* \n\n"); + first_apogee_detection = false; + + TRACE("Triggering nosecone ejection ... \n\n"); + sEventBroker->post({EV_NC_OPEN}, TOPIC_DPL); + } + + if (ada_controller.getStatus().dpl_altitude_reached && + first_dpl_altitude_detection) + { + TRACE("\n\n******* DPL ALTITUDE DETECTED! ******* \n\n"); + first_dpl_altitude_detection = false; + + TRACE("Triggering cutting sequence ... \n\n"); + sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DPL); + } + } + + TRACE("\nLanded ... \n\n"); + + for (;;) + { + Thread::sleep(1000); + } + + return 0; +} diff --git a/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp b/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5bc1014821bfc329a6fa5eb8bb87db82890402ff --- /dev/null +++ b/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp @@ -0,0 +1,149 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <miosix.h> + +#include <cmath> +#include <cstdio> +#include <iostream> +#include <sstream> +#include <string> + +#define private public + +#include "AirBrakesController/AirBrakesControllerAlgorithm.h" +#include "AirBrakesController/AirBrakesServo.h" +#include "drivers/HardwareTimer.h" +#include "sensors/Sensor.h" +#include "testdata.h" + +using namespace DeathStackBoard; +using namespace std; +class MockActuator : public AirBrakesServo +{ +public: + MockActuator() {} + void sendData(float alpha) { lastAlpha = round(alpha * 180 / PI); } + int error(int alphaTest) { return abs(alphaTest - lastAlpha); } + +private: + int lastAlpha; +}; + +template <typename T> +class MockSensor : public Sensor<T> +{ +private: + int ts = 0; + +protected: + T sampleImpl() override + { + input_t input = DATA[ts].input; + T sampled; + + sampled.z = input.z; + sampled.vz = input.vz; + sampled.vMod = input.vMod; + sampled.timestamp = ts; + + ts++; + + return sampled; + } + + bool init() override { return true; } + + bool selfTest() override { return true; } +}; + +class InputClass +{ +public: + float z; + float vz; + float vMod; + uint64_t timestamp; +}; + +MockActuator mockActuator; +MockSensor<InputClass> mockSensor; +AirBrakesControllerAlgorithm<InputClass> algorithm{mockSensor, mockActuator}; + +int main() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable TIM5 peripheral clocks + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + } + + input_t input; + + input = DATA[0].input; + + HardwareTimer<uint32_t> hrclock( + TIM5, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)); + hrclock.setPrescaler(127); + hrclock.start(); + + mockSensor.sample(); + + uint32_t t1 = hrclock.tick(); + algorithm.begin(); + uint32_t t2 = hrclock.tick(); + + float startTime = hrclock.toMilliSeconds(t2 - t1); + float stepTime = 0; + int maxAlphaError = 0; + + for (int i = 1; i < LEN_TEST; i++) + { + mockSensor.sample(); + t1 = hrclock.tick(); + algorithm.update(); + t2 = hrclock.tick(); + + int error = mockActuator.error(DATA[i].output.alpha); + + if (error > maxAlphaError) + { + maxAlphaError = error; + } + + stepTime += hrclock.toMilliSeconds(t2 - t1); + + cout << "z: " << mockSensor.getLastSample().z + << "\tvz: " << mockSensor.getLastSample().vz + << "\tvMod: " << mockSensor.getLastSample().vMod + << "\tservo: " << mockActuator.currentPosition << "\n"; + } + + printf("Max alpha error: %d\n", maxAlphaError); + + printf( + "Init time: %f ms\nAvg iter time: %f ms\nTotal computation time: %f " + "ms\n", + startTime, stepTime / (LEN_TEST - 1), stepTime + startTime); +} \ No newline at end of file diff --git a/src/tests/test-airbrakes-algorithm/testdata.h b/src/tests/test-airbrakes-algorithm/testdata.h new file mode 100644 index 0000000000000000000000000000000000000000..97636dc2bedce84e1828e05b3bf9c69cd7a2a685 --- /dev/null +++ b/src/tests/test-airbrakes-algorithm/testdata.h @@ -0,0 +1,389 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +static const int LEN_TEST = 169; + +struct input_t +{ + float z; + float vz; + float vMod; + float sampleTime; +}; + +struct output_t +{ + float z_setpoint; + float vz_setpoint; + float u; + float deltas; + int alpha; +}; + +struct test_t +{ + input_t input; + output_t output; +}; + +const test_t DATA[LEN_TEST] = { + {{1407.523534, 223.918458, 232.831273, 0.1}, + {1410.698478, 224.370516, 0.000000, 0.000000, 0}}, + {{1429.826222, 222.137686, 231.046739, 0.1}, + {1433.007837, 221.822602, 22.001227, 0.000000, 0}}, + {{1451.951540, 220.370997, 229.276914, 0.1}, + {1455.064742, 219.321178, 83.824886, 0.000000, 0}}, + {{1473.900883, 218.618135, 227.521532, 0.1}, + {1476.873741, 216.864257, 146.806876, 0.000000, 0}}, + {{1495.675620, 216.878841, 225.780334, 0.1}, + {1498.439192, 214.449971, 210.925533, 0.002500, 8}}, + {{1517.277018, 215.150907, 224.051216, 0.1}, + {1519.765268, 212.076566, 275.998544, 0.004500, 15}}, + {{1538.692467, 213.164813, 222.050419, 0.1}, + {1540.855976, 209.742391, 319.912919, 0.006000, 21}}, + {{1559.896941, 210.929504, 219.786565, 0.1}, + {1561.715159, 207.445889, 342.042809, 0.007000, 26}}, + {{1580.868317, 208.503942, 217.321830, 0.1}, + {1582.346512, 205.185595, 345.908908, 0.007000, 26}}, + {{1601.589631, 205.929325, 214.699801, 0.1}, + {1602.753584, 202.960124, 333.870736, 0.007000, 26}}, + {{1622.055564, 203.396178, 212.121054, 0.1}, + {1622.939793, 200.768167, 320.739091, 0.007000, 26}}, + {{1642.270215, 200.903468, 209.584307, 0.1}, + {1642.908428, 198.608488, 306.570638, 0.007000, 26}}, + {{1662.237560, 198.449839, 207.088281, 0.1}, + {1662.662657, 196.479916, 291.390833, 0.007000, 26}}, + {{1681.961982, 196.058211, 204.657393, 0.1}, + {1682.205536, 194.381342, 277.209978, 0.006500, 23}}, + {{1701.451202, 193.731747, 202.294942, 0.1}, + {1701.540011, 192.311716, 264.533577, 0.006500, 23}}, + {{1720.712684, 191.503026, 200.035297, 0.1}, + {1720.668927, 190.270041, 256.296072, 0.006500, 23}}, + {{1739.752816, 189.304576, 197.807258, 0.1}, + {1739.595032, 188.255368, 247.391254, 0.006500, 23}}, + {{1758.574577, 187.135466, 195.609965, 0.1}, + {1758.320980, 186.266798, 237.832929, 0.006000, 21}}, + {{1777.180861, 184.994918, 193.442602, 0.1}, + {1776.849338, 184.303477, 227.643718, 0.006000, 21}}, + {{1795.578146, 182.954926, 191.381404, 0.1}, + {1795.182591, 182.364588, 222.810466, 0.006000, 21}}, + {{1813.772666, 180.939526, 189.345943, 0.1}, + {1813.323143, 180.449357, 217.548324, 0.006000, 21}}, + {{1831.767240, 178.960146, 187.348411, 0.1}, + {1831.273322, 178.557045, 212.859568, 0.006000, 21}}, + {{1849.565570, 177.010170, 185.381850, 0.1}, + {1849.035385, 176.686946, 208.325170, 0.006000, 21}}, + {{1867.170012, 175.082317, 183.438539, 0.1}, + {1866.611519, 174.838389, 203.439009, 0.006000, 21}}, + {{1884.582753, 173.176068, 181.517929, 0.1}, + {1884.003846, 173.010730, 198.214336, 0.006000, 21}}, + {{1901.805928, 171.290886, 179.619473, 0.1}, + {1901.214426, 171.203355, 192.660780, 0.006000, 21}}, + {{1918.841611, 169.426147, 177.742548, 0.1}, + {1918.245256, 169.415678, 186.779343, 0.006000, 21}}, + {{1935.691826, 167.581464, 175.886774, 0.1}, + {1935.098279, 167.647137, 180.588101, 0.006000, 21}}, + {{1952.358556, 165.756368, 174.051669, 0.1}, + {1951.775381, 165.897193, 174.097177, 0.006000, 21}}, + {{1968.843737, 163.950404, 172.236762, 0.1}, + {1968.278396, 164.165332, 167.316651, 0.006000, 21}}, + {{1985.149439, 162.169295, 170.448186, 0.1}, + {1984.609107, 162.451059, 160.761439, 0.005500, 19}}, + {{2001.278287, 160.410614, 168.683383, 0.1}, + {2000.769249, 160.753900, 154.307788, 0.005500, 19}}, + {{2017.233572, 158.697809, 166.967443, 0.1}, + {2016.760511, 159.073401, 149.942314, 0.005500, 19}}, + {{2033.018389, 157.001195, 165.268700, 0.1}, + {2032.584538, 157.409124, 145.412723, 0.005500, 19}}, + {{2048.634338, 155.320412, 163.586791, 0.1}, + {2048.242929, 155.760649, 140.723784, 0.005500, 19}}, + {{2064.082987, 153.655113, 161.921368, 0.1}, + {2063.737245, 154.127573, 135.880349, 0.005000, 17}}, + {{2079.365866, 152.004960, 160.272087, 0.1}, + {2079.069006, 152.509506, 130.886994, 0.005000, 17}}, + {{2094.486432, 150.408612, 158.680445, 0.1}, + {2094.239695, 150.906075, 128.945069, 0.005000, 17}}, + {{2109.448027, 148.825441, 157.102924, 0.1}, + {2109.250756, 149.316919, 126.948556, 0.005000, 17}}, + {{2124.251954, 147.255236, 155.539324, 0.1}, + {2124.103601, 147.741691, 124.903075, 0.005000, 17}}, + {{2138.899517, 145.698979, 153.990723, 0.1}, + {2138.799604, 146.180055, 122.911826, 0.005500, 19}}, + {{2153.392272, 144.158165, 152.458755, 0.1}, + {2153.340110, 144.631690, 121.125681, 0.005500, 19}}, + {{2167.729859, 142.595759, 150.903540, 0.1}, + {2167.726428, 143.096281, 116.544249, 0.005000, 17}}, + {{2181.911853, 141.046238, 149.362216, 0.1}, + {2181.959840, 141.573528, 111.846697, 0.005000, 17}}, + {{2195.941149, 139.541589, 147.869341, 0.1}, + {2196.041597, 140.063140, 109.680869, 0.005000, 17}}, + {{2209.820551, 138.048329, 146.388841, 0.1}, + {2209.972921, 138.564834, 107.486848, 0.005000, 17}}, + {{2223.551186, 136.566227, 144.920493, 0.1}, + {2223.755007, 137.078337, 105.264693, 0.005000, 17}}, + {{2237.134161, 135.095065, 143.464081, 0.1}, + {2237.389022, 135.603386, 103.014797, 0.005000, 17}}, + {{2250.570557, 133.634627, 142.019394, 0.1}, + {2250.876108, 134.139726, 100.737494, 0.005000, 17}}, + {{2263.861436, 132.184701, 140.586228, 0.1}, + {2264.217381, 132.687107, 98.432779, 0.005000, 17}}, + {{2277.007840, 130.745074, 139.164381, 0.1}, + {2277.413934, 131.245290, 96.100334, 0.005000, 17}}, + {{2290.010788, 129.315539, 137.753660, 0.1}, + {2290.466835, 129.814042, 93.739752, 0.005000, 17}}, + {{2302.871343, 127.897638, 136.355777, 0.1}, + {2303.377130, 128.393136, 91.493595, 0.005000, 17}}, + {{2315.590644, 126.489974, 134.969234, 0.1}, + {2316.145842, 126.982353, 89.271851, 0.005000, 17}}, + {{2328.169653, 125.091761, 133.593199, 0.1}, + {2328.773972, 125.581479, 87.028189, 0.005000, 17}}, + {{2340.609305, 123.702814, 132.227500, 0.1}, + {2341.262502, 124.190307, 84.762076, 0.005000, 17}}, + {{2352.910518, 122.322948, 130.871970, 0.1}, + {2353.612390, 122.808635, 82.472658, 0.005000, 17}}, + {{2365.074191, 120.951981, 129.526447, 0.1}, + {2365.824578, 121.436267, 80.159115, 0.005000, 17}}, + {{2377.101205, 119.589741, 128.190773, 0.1}, + {2377.899985, 120.073012, 77.820896, 0.005000, 17}}, + {{2388.992425, 118.236061, 126.864795, 0.1}, + {2389.839515, 118.718683, 75.457721, 0.004500, 15}}, + {{2400.748697, 116.890780, 125.548366, 0.1}, + {2401.644050, 117.373101, 73.069412, 0.004500, 15}}, + {{2412.371612, 115.568806, 124.257936, 0.1}, + {2413.314456, 116.036087, 71.891017, 0.004500, 15}}, + {{2423.862714, 114.254488, 122.976329, 0.1}, + {2424.851582, 114.707470, 70.727132, 0.005000, 17}}, + {{2435.222760, 112.947681, 121.703422, 0.1}, + {2436.256259, 113.387082, 69.575894, 0.005000, 17}}, + {{2446.451788, 111.634162, 120.423512, 0.1}, + {2447.529301, 112.074759, 67.280809, 0.004500, 15}}, + {{2457.549877, 110.328894, 119.153172, 0.1}, + {2458.671507, 110.770342, 65.008026, 0.004500, 15}}, + {{2468.518481, 109.044366, 117.906294, 0.1}, + {2469.683660, 109.473673, 63.796332, 0.004500, 15}}, + {{2479.358982, 107.766798, 116.667643, 0.1}, + {2480.566527, 108.184601, 62.593195, 0.004500, 15}}, + {{2490.072068, 106.496060, 115.437113, 0.1}, + {2491.320859, 106.902976, 61.396891, 0.005000, 17}}, + {{2500.658417, 105.231983, 114.214575, 0.1}, + {2501.947396, 105.628652, 60.202504, 0.005000, 17}}, + {{2511.118055, 103.961930, 112.985948, 0.1}, + {2512.446858, 104.361488, 57.982258, 0.004500, 15}}, + {{2521.451029, 102.698677, 111.765518, 0.1}, + {2522.819957, 103.101343, 55.729628, 0.004500, 15}}, + {{2531.658591, 101.453613, 110.566106, 0.1}, + {2533.067385, 101.848081, 54.388563, 0.004500, 15}}, + {{2541.741959, 100.214775, 109.374353, 0.1}, + {2543.189826, 100.601569, 53.045474, 0.004500, 15}}, + {{2551.701750, 98.982052, 108.190179, 0.1}, + {2553.187947, 99.361675, 51.699514, 0.004500, 15}}, + {{2561.538570, 97.755336, 107.013501, 0.1}, + {2563.062404, 98.128272, 50.349771, 0.004500, 15}}, + {{2571.253014, 96.534517, 105.844243, 0.1}, + {2572.813840, 96.901233, 48.995142, 0.004500, 15}}, + {{2580.845666, 95.319487, 104.682332, 0.1}, + {2582.442885, 95.680437, 47.634353, 0.004500, 15}}, + {{2590.317100, 94.110134, 103.527695, 0.1}, + {2591.950157, 94.465761, 46.266054, 0.004500, 15}}, + {{2599.667867, 92.906047, 102.379915, 0.1}, + {2601.336262, 93.257089, 44.863988, 0.004500, 15}}, + {{2608.898492, 91.707370, 101.239199, 0.1}, + {2610.601795, 92.054303, 43.445653, 0.004000, 13}}, + {{2618.009521, 90.514092, 100.105582, 0.1}, + {2619.747339, 90.857290, 42.017225, 0.004000, 13}}, + {{2627.002040, 89.337126, 98.991641, 0.1}, + {2628.773466, 89.665938, 41.480883, 0.004000, 13}}, + {{2635.877108, 88.165040, 97.884362, 0.1}, + {2637.680735, 88.480138, 40.961420, 0.004500, 15}}, + {{2644.635208, 86.997737, 96.783700, 0.1}, + {2646.469697, 87.299782, 40.456275, 0.004500, 15}}, + {{2653.276298, 85.824893, 95.677786, 0.1}, + {2655.140892, 86.124763, 39.124360, 0.004500, 15}}, + {{2661.800350, 84.656946, 94.578723, 0.1}, + {2663.694846, 84.954979, 37.775662, 0.004500, 15}}, + {{2670.207848, 83.493807, 93.486471, 0.1}, + {2672.132080, 83.790326, 36.409646, 0.004000, 13}}, + {{2678.499269, 82.335394, 92.400999, 0.1}, + {2680.453100, 82.630705, 35.026106, 0.004000, 13}}, + {{2686.675545, 81.190866, 91.333090, 0.1}, + {2688.658406, 81.476016, 34.382681, 0.004000, 13}}, + {{2694.737585, 80.050628, 90.271622, 0.1}, + {2696.748485, 80.326164, 33.745383, 0.004000, 13}}, + {{2702.685811, 78.914571, 89.216537, 0.1}, + {2700.750465, 79.753021, 0.000000, 0.000000, 0}}, + {{2710.520635, 77.782587, 88.167786, 0.1}, + {2708.668599, 78.610244, 0.000000, 0.000000, 0}}, + {{2718.244674, 76.698652, 87.177631, 0.1}, + {2716.472686, 77.472068, 0.000000, 0.000000, 0}}, + {{2725.860456, 75.617439, 86.192585, 0.1}, + {2724.163181, 76.338400, 0.000000, 0.000000, 0}}, + {{2733.368175, 74.537122, 85.210554, 0.1}, + {2731.740532, 75.209151, 0.000000, 0.000000, 0}}, + {{2740.767967, 73.459160, 84.233359, 0.1}, + {2739.205174, 74.084232, 2.638818, 0.000000, 0}}, + {{2748.060095, 72.383840, 83.261426, 0.1}, + {2746.557537, 72.963557, 3.232653, 0.000000, 0}}, + {{2755.244822, 71.311123, 82.294793, 0.1}, + {2753.798041, 71.847038, 3.925782, 0.000000, 0}}, + {{2762.322406, 70.240971, 81.333504, 0.1}, + {2760.927097, 70.734592, 4.714279, 0.000000, 0}}, + {{2769.293100, 69.173345, 80.377603, 0.1}, + {2767.945109, 69.626135, 5.594345, 0.000000, 0}}, + {{2776.157158, 68.108208, 79.427143, 0.1}, + {2774.852471, 68.521586, 6.562208, 0.000000, 0}}, + {{2782.914824, 67.045519, 78.482177, 0.1}, + {2781.649570, 67.420863, 7.614075, 0.000000, 0}}, + {{2789.566341, 65.985236, 77.542764, 0.1}, + {2788.336784, 66.323887, 8.746139, 0.000000, 0}}, + {{2796.111950, 64.927316, 76.608969, 0.1}, + {2794.914485, 65.230580, 9.954627, 0.000000, 0}}, + {{2802.551882, 63.871717, 75.680859, 0.1}, + {2801.383035, 64.140865, 11.235865, 0.000000, 0}}, + {{2808.886369, 62.818398, 74.758508, 0.1}, + {2807.742790, 63.054665, 12.586344, 0.000000, 0}}, + {{2815.115636, 61.767318, 73.841996, 0.1}, + {2813.994097, 61.971905, 14.002770, 0.000000, 0}}, + {{2821.239906, 60.718440, 72.931408, 0.1}, + {2820.137297, 60.892512, 15.482087, 0.001000, 3}}, + {{2827.259396, 59.671727, 72.026834, 0.1}, + {2826.172723, 59.816412, 17.021470, 0.002500, 8}}, + {{2833.174044, 58.621604, 71.121232, 0.1}, + {2832.100700, 58.743534, 18.163957, 0.003500, 11}}, + {{2838.983308, 57.564111, 70.209511, 0.1}, + {2837.921548, 57.673807, 18.557488, 0.004000, 13}}, + {{2844.686607, 56.502319, 69.295655, 0.1}, + {2843.635577, 56.607161, 18.407003, 0.004500, 15}}, + {{2850.283663, 55.439136, 68.383536, 0.1}, + {2849.243093, 55.543527, 17.919713, 0.004500, 15}}, + {{2855.774270, 54.373463, 67.471945, 0.1}, + {2854.744393, 54.482838, 16.989111, 0.004000, 13}}, + {{2861.158442, 53.310395, 66.567992, 0.1}, + {2860.139768, 53.425025, 16.011311, 0.004000, 13}}, + {{2866.436640, 52.253928, 65.677398, 0.1}, + {2865.429503, 52.370024, 15.317990, 0.004000, 13}}, + {{2871.609301, 51.199672, 64.794534, 0.1}, + {2870.613876, 51.317768, 14.573491, 0.003500, 11}}, + {{2876.676648, 50.147640, 63.919552, 0.1}, + {2875.693158, 50.268194, 13.781458, 0.003500, 11}}, + {{2881.639061, 49.100977, 63.056873, 0.1}, + {2880.667613, 49.221238, 13.202782, 0.003000, 9}}, + {{2886.496920, 48.056611, 62.202318, 0.1}, + {2885.537501, 48.176836, 12.604339, 0.003000, 9}}, + {{2891.250636, 47.018088, 61.360933, 0.1}, + {2890.303074, 47.134928, 12.280825, 0.003000, 9}}, + {{2895.900617, 45.981928, 60.527937, 0.1}, + {2894.964578, 46.095452, 11.968529, 0.003500, 11}}, + {{2900.447101, 44.948158, 59.703518, 0.1}, + {2899.522253, 45.058347, 11.674383, 0.003500, 11}}, + {{2904.890167, 43.913570, 58.883223, 0.1}, + {2903.976334, 44.023555, 11.140163, 0.003500, 11}}, + {{2909.229897, 42.881424, 58.072049, 0.1}, + {2908.327049, 42.991016, 10.622481, 0.003000, 9}}, + {{2913.466530, 41.851616, 57.270212, 0.1}, + {2912.574619, 41.960672, 10.118410, 0.003000, 9}}, + {{2917.600434, 40.826803, 56.482155, 0.1}, + {2916.719263, 40.932466, 9.851340, 0.003000, 9}}, + {{2921.631953, 39.803918, 55.703792, 0.1}, + {2920.761191, 39.906341, 9.588696, 0.003500, 11}}, + {{2925.561287, 38.783108, 54.935412, 0.1}, + {2924.700607, 38.882242, 9.346359, 0.003500, 11}}, + {{2929.388524, 37.762009, 54.173486, 0.1}, + {2928.537713, 37.860112, 8.935239, 0.003500, 11}}, + {{2933.113764, 36.743153, 53.422213, 0.1}, + {2932.272702, 36.839897, 8.556173, 0.003500, 11}}, + {{2936.737228, 35.726506, 52.681892, 0.1}, + {2935.905762, 35.821543, 8.212388, 0.003500, 11}}, + {{2940.259136, 34.711992, 51.952833, 0.1}, + {2939.437078, 34.804997, 7.903881, 0.003500, 11}}, + {{2943.679695, 33.699509, 51.235358, 0.1}, + {2942.866827, 33.790206, 7.628086, 0.003500, 11}}, + {{2946.999101, 32.688846, 50.529650, 0.1}, + {2946.195183, 32.777117, 7.373489, 0.003500, 11}}, + {{2950.217493, 31.679292, 49.835035, 0.1}, + {2950.997969, 31.260565, 12.867386, 0.010000, 41}}, + {{2953.335016, 30.671441, 49.153096, 0.1}, + {2954.073563, 30.251509, 12.283864, 0.010000, 48}}, + {{2956.350647, 29.641635, 48.441400, 0.1}, + {2957.048328, 29.243979, 11.696148, 0.010000, 48}}, + {{2959.263303, 28.611932, 47.739490, 0.1}, + {2959.922414, 28.237924, 11.123138, 0.010000, 48}}, + {{2962.073123, 27.584913, 47.052590, 0.1}, + {2962.695966, 27.233296, 10.566719, 0.010000, 48}}, + {{2964.780373, 26.560521, 46.381113, 0.1}, + {2965.369125, 26.230046, 10.026509, 0.010000, 48}}, + {{2967.385314, 25.538714, 45.725492, 0.1}, + {2967.942025, 25.228128, 9.502122, 0.010000, 48}}, + {{2969.888201, 24.519460, 45.086174, 0.1}, + {2970.414798, 24.227493, 8.993173, 0.010000, 48}}, + {{2972.289290, 23.502736, 44.463621, 0.1}, + {2972.787570, 23.228095, 8.499271, 0.010000, 48}}, + {{2974.588832, 22.488518, 43.858309, 0.1}, + {2975.060462, 22.229890, 8.020013, 0.010000, 48}}, + {{2976.787076, 21.476777, 43.270725, 0.1}, + {2977.233591, 21.232831, 7.554990, 0.010000, 48}}, + {{2978.884269, 20.467481, 42.701369, 0.1}, + {2979.307070, 20.236874, 7.103775, 0.010000, 48}}, + {{2980.880652, 19.460587, 42.150748, 0.1}, + {2981.281006, 19.241976, 6.665932, 0.010000, 48}}, + {{2982.776464, 18.456044, 41.619378, 0.1}, + {2983.155503, 18.248095, 6.241007, 0.010000, 48}}, + {{2984.571937, 17.453792, 41.107781, 0.1}, + {2984.930661, 17.255187, 5.828534, 0.010000, 48}}, + {{2986.267297, 16.453765, 40.616482, 0.1}, + {2986.606575, 16.263212, 5.428031, 0.010000, 48}}, + {{2987.862762, 15.455888, 40.146011, 0.1}, + {2988.183337, 15.272130, 5.039001, 0.010000, 48}}, + {{2989.358540, 14.460001, 39.696907, 0.1}, + {2989.661033, 14.281903, 4.660909, 0.010000, 48}}, + {{2990.754830, 13.466130, 39.269688, 0.1}, + {2991.039748, 13.292492, 4.293264, 0.010000, 48}}, + {{2992.051832, 12.474233, 38.864869, 0.1}, + {2992.319561, 12.303862, 3.935539, 0.010000, 48}}, + {{2993.249740, 11.484250, 38.482958, 0.1}, + {2993.500549, 11.315977, 3.587188, 0.010000, 48}}, + {{2994.348743, 10.496116, 38.124445, 0.1}, + {2994.582783, 10.328804, 3.247651, 0.010000, 48}}, + {{2995.349023, 9.509767, 37.789806, 0.1}, + {2995.566335, 9.342312, 2.916352, 0.010000, 48}}, + {{2996.250754, 8.525139, 37.479491, 0.1}, + {2996.451270, 8.356472, 2.592704, 0.010000, 48}}, + {{2997.054106, 7.542171, 37.193927, 0.1}, + {2997.237653, 7.371257, 2.276107, 0.010000, 48}}, + {{2997.759242, 6.560805, 36.933508, 0.1}, + {2997.593906, 6.878876, 0.000000, 0.000000, 18}}, + {{2998.366319, 5.580987, 36.698594, 0.1}, + {2998.232574, 5.894551, 0.000000, 0.000000, 0}}, + {{2998.875647, 4.605733, 36.513507, 0.1}, + {2998.772838, 4.910794, 0.000000, 0.000000, 0}}, + {{2999.287586, 3.633139, 36.370710, 0.1}, + {2999.214753, 3.927587, 0.000000, 0.000000, 0}}, + {{2999.602292, 2.661052, 36.253879, 0.1}, + {2999.558375, 2.944913, 0.000000, 0.000000, 0}}, + {{2999.819812, 1.689438, 36.163201, 0.1}, + {2999.803756, 1.962763, 0.000000, 0.000000, 0}}, + {{2999.940194, 0.718271, 36.098808, 0.1}, + {2999.950947, 0.981127, 0.000000, 0.000000, 0}}, + {{2999.963481, -0.252449, 36.060779, 0.1}, + {2999.950947, 0.981127, 0.000000, 0.000000, 0}}, +}; diff --git a/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp b/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4a4a716d58af3b52f9cffc830dafa1168005f17e --- /dev/null +++ b/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp @@ -0,0 +1,142 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <miosix.h> + +#include <cmath> +#include <cstdio> + +#define private public + +#include "boards/DeathStack/AirBrakesController/AirBrakesControllerAlgorithm.h" +#include "boards/DeathStack/AirBrakesController/AirbrakesServoInterface.h" +#include "drivers/HardwareTimer.h" +#include "sensors/Sensor.h" +#include "testdata.h" + +using namespace DeathStackBoard; + +class MockActuator : public AirbrakesServoInterface +{ +public: + MockActuator() {} + void sendData(float alpha) { lastAlpha = round(alpha * 180 / PI); } + int error(int alphaTest) { return abs(alphaTest - lastAlpha); } + +private: + int lastAlpha; +}; + +template <typename T> +class MockSensor : public Sensor<T> +{ +private: + int ts = 0; + +protected: + T sampleImpl() override + { + input_t input = DATA[ts].input; + T sampled; + + sampled.z = input.z; + sampled.vz = input.vz; + sampled.vMod = input.vMod; + sampled.timestamp = ts; + + ts++; + + return sampled; + } + + bool init() override { return true; } + + bool selfTest() override { return true; } +}; + +class InputClass +{ +public: + float z; + float vz; + float vMod; + uint64_t timestamp; +}; + +MockActuator* ma = new MockActuator; +MockSensor<InputClass>* ms = new MockSensor<InputClass>; +AirBrakesControllerAlgorithm<InputClass> ca = + AirBrakesControllerAlgorithm<InputClass>(ms, ma); + +int main() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Enable TIM5 peripheral clocks + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + } + + input_t input; + + input = DATA[0].input; + + HardwareTimer<uint32_t> hrclock( + TIM5, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)); + hrclock.setPrescaler(127); + hrclock.start(); + + ms->sample(); + + uint32_t t1 = hrclock.tick(); + ca.begin(); + uint32_t t2 = hrclock.tick(); + + float startTime = hrclock.toMilliSeconds(t2 - t1); + float stepTime = 0; + int maxAlphaError = 0; + + for (int i = 1; i < LEN_TEST; i++) + { + ms->sample(); + t1 = hrclock.tick(); + ca.update(); + t2 = hrclock.tick(); + + int error = ma->error(DATA[i].output.alpha); + + if (error > maxAlphaError) + { + maxAlphaError = error; + } + + stepTime += hrclock.toMilliSeconds(t2 - t1); + } + + printf("Max alpha error: %d\n", maxAlphaError); + + printf( + "Init time: %f ms\nAvg iter time: %f ms\nTotal computation time: %f " + "ms\n", + startTime, stepTime / (LEN_TEST - 1), stepTime + startTime); +} \ No newline at end of file diff --git a/src/tests/test-airbrakescontroller/testdata.cpp b/src/tests/test-airbrakescontroller/testdata.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7f44502081145862cdc67f9d51c8099199e42d46 --- /dev/null +++ b/src/tests/test-airbrakescontroller/testdata.cpp @@ -0,0 +1,364 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "testdata.h" + +const test_t DATA[LEN_TEST] = { + {{1407.523534, 223.918458, 232.831273, 0.1}, + {1410.698478, 224.370516, 0.000000, 0.000000, 0}}, + {{1429.826222, 222.137686, 231.046739, 0.1}, + {1433.007837, 221.822602, 22.001227, 0.000000, 0}}, + {{1451.951540, 220.370997, 229.276914, 0.1}, + {1455.064742, 219.321178, 83.824886, 0.000000, 0}}, + {{1473.900883, 218.618135, 227.521532, 0.1}, + {1476.873741, 216.864257, 146.806876, 0.000000, 0}}, + {{1495.675620, 216.878841, 225.780334, 0.1}, + {1498.439192, 214.449971, 210.925533, 0.002500, 8}}, + {{1517.277018, 215.150907, 224.051216, 0.1}, + {1519.765268, 212.076566, 275.998544, 0.004500, 15}}, + {{1538.692467, 213.164813, 222.050419, 0.1}, + {1540.855976, 209.742391, 319.912919, 0.006000, 21}}, + {{1559.896941, 210.929504, 219.786565, 0.1}, + {1561.715159, 207.445889, 342.042809, 0.007000, 26}}, + {{1580.868317, 208.503942, 217.321830, 0.1}, + {1582.346512, 205.185595, 345.908908, 0.007000, 26}}, + {{1601.589631, 205.929325, 214.699801, 0.1}, + {1602.753584, 202.960124, 333.870736, 0.007000, 26}}, + {{1622.055564, 203.396178, 212.121054, 0.1}, + {1622.939793, 200.768167, 320.739091, 0.007000, 26}}, + {{1642.270215, 200.903468, 209.584307, 0.1}, + {1642.908428, 198.608488, 306.570638, 0.007000, 26}}, + {{1662.237560, 198.449839, 207.088281, 0.1}, + {1662.662657, 196.479916, 291.390833, 0.007000, 26}}, + {{1681.961982, 196.058211, 204.657393, 0.1}, + {1682.205536, 194.381342, 277.209978, 0.006500, 23}}, + {{1701.451202, 193.731747, 202.294942, 0.1}, + {1701.540011, 192.311716, 264.533577, 0.006500, 23}}, + {{1720.712684, 191.503026, 200.035297, 0.1}, + {1720.668927, 190.270041, 256.296072, 0.006500, 23}}, + {{1739.752816, 189.304576, 197.807258, 0.1}, + {1739.595032, 188.255368, 247.391254, 0.006500, 23}}, + {{1758.574577, 187.135466, 195.609965, 0.1}, + {1758.320980, 186.266798, 237.832929, 0.006000, 21}}, + {{1777.180861, 184.994918, 193.442602, 0.1}, + {1776.849338, 184.303477, 227.643718, 0.006000, 21}}, + {{1795.578146, 182.954926, 191.381404, 0.1}, + {1795.182591, 182.364588, 222.810466, 0.006000, 21}}, + {{1813.772666, 180.939526, 189.345943, 0.1}, + {1813.323143, 180.449357, 217.548324, 0.006000, 21}}, + {{1831.767240, 178.960146, 187.348411, 0.1}, + {1831.273322, 178.557045, 212.859568, 0.006000, 21}}, + {{1849.565570, 177.010170, 185.381850, 0.1}, + {1849.035385, 176.686946, 208.325170, 0.006000, 21}}, + {{1867.170012, 175.082317, 183.438539, 0.1}, + {1866.611519, 174.838389, 203.439009, 0.006000, 21}}, + {{1884.582753, 173.176068, 181.517929, 0.1}, + {1884.003846, 173.010730, 198.214336, 0.006000, 21}}, + {{1901.805928, 171.290886, 179.619473, 0.1}, + {1901.214426, 171.203355, 192.660780, 0.006000, 21}}, + {{1918.841611, 169.426147, 177.742548, 0.1}, + {1918.245256, 169.415678, 186.779343, 0.006000, 21}}, + {{1935.691826, 167.581464, 175.886774, 0.1}, + {1935.098279, 167.647137, 180.588101, 0.006000, 21}}, + {{1952.358556, 165.756368, 174.051669, 0.1}, + {1951.775381, 165.897193, 174.097177, 0.006000, 21}}, + {{1968.843737, 163.950404, 172.236762, 0.1}, + {1968.278396, 164.165332, 167.316651, 0.006000, 21}}, + {{1985.149439, 162.169295, 170.448186, 0.1}, + {1984.609107, 162.451059, 160.761439, 0.005500, 19}}, + {{2001.278287, 160.410614, 168.683383, 0.1}, + {2000.769249, 160.753900, 154.307788, 0.005500, 19}}, + {{2017.233572, 158.697809, 166.967443, 0.1}, + {2016.760511, 159.073401, 149.942314, 0.005500, 19}}, + {{2033.018389, 157.001195, 165.268700, 0.1}, + {2032.584538, 157.409124, 145.412723, 0.005500, 19}}, + {{2048.634338, 155.320412, 163.586791, 0.1}, + {2048.242929, 155.760649, 140.723784, 0.005500, 19}}, + {{2064.082987, 153.655113, 161.921368, 0.1}, + {2063.737245, 154.127573, 135.880349, 0.005000, 17}}, + {{2079.365866, 152.004960, 160.272087, 0.1}, + {2079.069006, 152.509506, 130.886994, 0.005000, 17}}, + {{2094.486432, 150.408612, 158.680445, 0.1}, + {2094.239695, 150.906075, 128.945069, 0.005000, 17}}, + {{2109.448027, 148.825441, 157.102924, 0.1}, + {2109.250756, 149.316919, 126.948556, 0.005000, 17}}, + {{2124.251954, 147.255236, 155.539324, 0.1}, + {2124.103601, 147.741691, 124.903075, 0.005000, 17}}, + {{2138.899517, 145.698979, 153.990723, 0.1}, + {2138.799604, 146.180055, 122.911826, 0.005500, 19}}, + {{2153.392272, 144.158165, 152.458755, 0.1}, + {2153.340110, 144.631690, 121.125681, 0.005500, 19}}, + {{2167.729859, 142.595759, 150.903540, 0.1}, + {2167.726428, 143.096281, 116.544249, 0.005000, 17}}, + {{2181.911853, 141.046238, 149.362216, 0.1}, + {2181.959840, 141.573528, 111.846697, 0.005000, 17}}, + {{2195.941149, 139.541589, 147.869341, 0.1}, + {2196.041597, 140.063140, 109.680869, 0.005000, 17}}, + {{2209.820551, 138.048329, 146.388841, 0.1}, + {2209.972921, 138.564834, 107.486848, 0.005000, 17}}, + {{2223.551186, 136.566227, 144.920493, 0.1}, + {2223.755007, 137.078337, 105.264693, 0.005000, 17}}, + {{2237.134161, 135.095065, 143.464081, 0.1}, + {2237.389022, 135.603386, 103.014797, 0.005000, 17}}, + {{2250.570557, 133.634627, 142.019394, 0.1}, + {2250.876108, 134.139726, 100.737494, 0.005000, 17}}, + {{2263.861436, 132.184701, 140.586228, 0.1}, + {2264.217381, 132.687107, 98.432779, 0.005000, 17}}, + {{2277.007840, 130.745074, 139.164381, 0.1}, + {2277.413934, 131.245290, 96.100334, 0.005000, 17}}, + {{2290.010788, 129.315539, 137.753660, 0.1}, + {2290.466835, 129.814042, 93.739752, 0.005000, 17}}, + {{2302.871343, 127.897638, 136.355777, 0.1}, + {2303.377130, 128.393136, 91.493595, 0.005000, 17}}, + {{2315.590644, 126.489974, 134.969234, 0.1}, + {2316.145842, 126.982353, 89.271851, 0.005000, 17}}, + {{2328.169653, 125.091761, 133.593199, 0.1}, + {2328.773972, 125.581479, 87.028189, 0.005000, 17}}, + {{2340.609305, 123.702814, 132.227500, 0.1}, + {2341.262502, 124.190307, 84.762076, 0.005000, 17}}, + {{2352.910518, 122.322948, 130.871970, 0.1}, + {2353.612390, 122.808635, 82.472658, 0.005000, 17}}, + {{2365.074191, 120.951981, 129.526447, 0.1}, + {2365.824578, 121.436267, 80.159115, 0.005000, 17}}, + {{2377.101205, 119.589741, 128.190773, 0.1}, + {2377.899985, 120.073012, 77.820896, 0.005000, 17}}, + {{2388.992425, 118.236061, 126.864795, 0.1}, + {2389.839515, 118.718683, 75.457721, 0.004500, 15}}, + {{2400.748697, 116.890780, 125.548366, 0.1}, + {2401.644050, 117.373101, 73.069412, 0.004500, 15}}, + {{2412.371612, 115.568806, 124.257936, 0.1}, + {2413.314456, 116.036087, 71.891017, 0.004500, 15}}, + {{2423.862714, 114.254488, 122.976329, 0.1}, + {2424.851582, 114.707470, 70.727132, 0.005000, 17}}, + {{2435.222760, 112.947681, 121.703422, 0.1}, + {2436.256259, 113.387082, 69.575894, 0.005000, 17}}, + {{2446.451788, 111.634162, 120.423512, 0.1}, + {2447.529301, 112.074759, 67.280809, 0.004500, 15}}, + {{2457.549877, 110.328894, 119.153172, 0.1}, + {2458.671507, 110.770342, 65.008026, 0.004500, 15}}, + {{2468.518481, 109.044366, 117.906294, 0.1}, + {2469.683660, 109.473673, 63.796332, 0.004500, 15}}, + {{2479.358982, 107.766798, 116.667643, 0.1}, + {2480.566527, 108.184601, 62.593195, 0.004500, 15}}, + {{2490.072068, 106.496060, 115.437113, 0.1}, + {2491.320859, 106.902976, 61.396891, 0.005000, 17}}, + {{2500.658417, 105.231983, 114.214575, 0.1}, + {2501.947396, 105.628652, 60.202504, 0.005000, 17}}, + {{2511.118055, 103.961930, 112.985948, 0.1}, + {2512.446858, 104.361488, 57.982258, 0.004500, 15}}, + {{2521.451029, 102.698677, 111.765518, 0.1}, + {2522.819957, 103.101343, 55.729628, 0.004500, 15}}, + {{2531.658591, 101.453613, 110.566106, 0.1}, + {2533.067385, 101.848081, 54.388563, 0.004500, 15}}, + {{2541.741959, 100.214775, 109.374353, 0.1}, + {2543.189826, 100.601569, 53.045474, 0.004500, 15}}, + {{2551.701750, 98.982052, 108.190179, 0.1}, + {2553.187947, 99.361675, 51.699514, 0.004500, 15}}, + {{2561.538570, 97.755336, 107.013501, 0.1}, + {2563.062404, 98.128272, 50.349771, 0.004500, 15}}, + {{2571.253014, 96.534517, 105.844243, 0.1}, + {2572.813840, 96.901233, 48.995142, 0.004500, 15}}, + {{2580.845666, 95.319487, 104.682332, 0.1}, + {2582.442885, 95.680437, 47.634353, 0.004500, 15}}, + {{2590.317100, 94.110134, 103.527695, 0.1}, + {2591.950157, 94.465761, 46.266054, 0.004500, 15}}, + {{2599.667867, 92.906047, 102.379915, 0.1}, + {2601.336262, 93.257089, 44.863988, 0.004500, 15}}, + {{2608.898492, 91.707370, 101.239199, 0.1}, + {2610.601795, 92.054303, 43.445653, 0.004000, 13}}, + {{2618.009521, 90.514092, 100.105582, 0.1}, + {2619.747339, 90.857290, 42.017225, 0.004000, 13}}, + {{2627.002040, 89.337126, 98.991641, 0.1}, + {2628.773466, 89.665938, 41.480883, 0.004000, 13}}, + {{2635.877108, 88.165040, 97.884362, 0.1}, + {2637.680735, 88.480138, 40.961420, 0.004500, 15}}, + {{2644.635208, 86.997737, 96.783700, 0.1}, + {2646.469697, 87.299782, 40.456275, 0.004500, 15}}, + {{2653.276298, 85.824893, 95.677786, 0.1}, + {2655.140892, 86.124763, 39.124360, 0.004500, 15}}, + {{2661.800350, 84.656946, 94.578723, 0.1}, + {2663.694846, 84.954979, 37.775662, 0.004500, 15}}, + {{2670.207848, 83.493807, 93.486471, 0.1}, + {2672.132080, 83.790326, 36.409646, 0.004000, 13}}, + {{2678.499269, 82.335394, 92.400999, 0.1}, + {2680.453100, 82.630705, 35.026106, 0.004000, 13}}, + {{2686.675545, 81.190866, 91.333090, 0.1}, + {2688.658406, 81.476016, 34.382681, 0.004000, 13}}, + {{2694.737585, 80.050628, 90.271622, 0.1}, + {2696.748485, 80.326164, 33.745383, 0.004000, 13}}, + {{2702.685811, 78.914571, 89.216537, 0.1}, + {2700.750465, 79.753021, 0.000000, 0.000000, 0}}, + {{2710.520635, 77.782587, 88.167786, 0.1}, + {2708.668599, 78.610244, 0.000000, 0.000000, 0}}, + {{2718.244674, 76.698652, 87.177631, 0.1}, + {2716.472686, 77.472068, 0.000000, 0.000000, 0}}, + {{2725.860456, 75.617439, 86.192585, 0.1}, + {2724.163181, 76.338400, 0.000000, 0.000000, 0}}, + {{2733.368175, 74.537122, 85.210554, 0.1}, + {2731.740532, 75.209151, 0.000000, 0.000000, 0}}, + {{2740.767967, 73.459160, 84.233359, 0.1}, + {2739.205174, 74.084232, 2.638818, 0.000000, 0}}, + {{2748.060095, 72.383840, 83.261426, 0.1}, + {2746.557537, 72.963557, 3.232653, 0.000000, 0}}, + {{2755.244822, 71.311123, 82.294793, 0.1}, + {2753.798041, 71.847038, 3.925782, 0.000000, 0}}, + {{2762.322406, 70.240971, 81.333504, 0.1}, + {2760.927097, 70.734592, 4.714279, 0.000000, 0}}, + {{2769.293100, 69.173345, 80.377603, 0.1}, + {2767.945109, 69.626135, 5.594345, 0.000000, 0}}, + {{2776.157158, 68.108208, 79.427143, 0.1}, + {2774.852471, 68.521586, 6.562208, 0.000000, 0}}, + {{2782.914824, 67.045519, 78.482177, 0.1}, + {2781.649570, 67.420863, 7.614075, 0.000000, 0}}, + {{2789.566341, 65.985236, 77.542764, 0.1}, + {2788.336784, 66.323887, 8.746139, 0.000000, 0}}, + {{2796.111950, 64.927316, 76.608969, 0.1}, + {2794.914485, 65.230580, 9.954627, 0.000000, 0}}, + {{2802.551882, 63.871717, 75.680859, 0.1}, + {2801.383035, 64.140865, 11.235865, 0.000000, 0}}, + {{2808.886369, 62.818398, 74.758508, 0.1}, + {2807.742790, 63.054665, 12.586344, 0.000000, 0}}, + {{2815.115636, 61.767318, 73.841996, 0.1}, + {2813.994097, 61.971905, 14.002770, 0.000000, 0}}, + {{2821.239906, 60.718440, 72.931408, 0.1}, + {2820.137297, 60.892512, 15.482087, 0.001000, 3}}, + {{2827.259396, 59.671727, 72.026834, 0.1}, + {2826.172723, 59.816412, 17.021470, 0.002500, 8}}, + {{2833.174044, 58.621604, 71.121232, 0.1}, + {2832.100700, 58.743534, 18.163957, 0.003500, 11}}, + {{2838.983308, 57.564111, 70.209511, 0.1}, + {2837.921548, 57.673807, 18.557488, 0.004000, 13}}, + {{2844.686607, 56.502319, 69.295655, 0.1}, + {2843.635577, 56.607161, 18.407003, 0.004500, 15}}, + {{2850.283663, 55.439136, 68.383536, 0.1}, + {2849.243093, 55.543527, 17.919713, 0.004500, 15}}, + {{2855.774270, 54.373463, 67.471945, 0.1}, + {2854.744393, 54.482838, 16.989111, 0.004000, 13}}, + {{2861.158442, 53.310395, 66.567992, 0.1}, + {2860.139768, 53.425025, 16.011311, 0.004000, 13}}, + {{2866.436640, 52.253928, 65.677398, 0.1}, + {2865.429503, 52.370024, 15.317990, 0.004000, 13}}, + {{2871.609301, 51.199672, 64.794534, 0.1}, + {2870.613876, 51.317768, 14.573491, 0.003500, 11}}, + {{2876.676648, 50.147640, 63.919552, 0.1}, + {2875.693158, 50.268194, 13.781458, 0.003500, 11}}, + {{2881.639061, 49.100977, 63.056873, 0.1}, + {2880.667613, 49.221238, 13.202782, 0.003000, 9}}, + {{2886.496920, 48.056611, 62.202318, 0.1}, + {2885.537501, 48.176836, 12.604339, 0.003000, 9}}, + {{2891.250636, 47.018088, 61.360933, 0.1}, + {2890.303074, 47.134928, 12.280825, 0.003000, 9}}, + {{2895.900617, 45.981928, 60.527937, 0.1}, + {2894.964578, 46.095452, 11.968529, 0.003500, 11}}, + {{2900.447101, 44.948158, 59.703518, 0.1}, + {2899.522253, 45.058347, 11.674383, 0.003500, 11}}, + {{2904.890167, 43.913570, 58.883223, 0.1}, + {2903.976334, 44.023555, 11.140163, 0.003500, 11}}, + {{2909.229897, 42.881424, 58.072049, 0.1}, + {2908.327049, 42.991016, 10.622481, 0.003000, 9}}, + {{2913.466530, 41.851616, 57.270212, 0.1}, + {2912.574619, 41.960672, 10.118410, 0.003000, 9}}, + {{2917.600434, 40.826803, 56.482155, 0.1}, + {2916.719263, 40.932466, 9.851340, 0.003000, 9}}, + {{2921.631953, 39.803918, 55.703792, 0.1}, + {2920.761191, 39.906341, 9.588696, 0.003500, 11}}, + {{2925.561287, 38.783108, 54.935412, 0.1}, + {2924.700607, 38.882242, 9.346359, 0.003500, 11}}, + {{2929.388524, 37.762009, 54.173486, 0.1}, + {2928.537713, 37.860112, 8.935239, 0.003500, 11}}, + {{2933.113764, 36.743153, 53.422213, 0.1}, + {2932.272702, 36.839897, 8.556173, 0.003500, 11}}, + {{2936.737228, 35.726506, 52.681892, 0.1}, + {2935.905762, 35.821543, 8.212388, 0.003500, 11}}, + {{2940.259136, 34.711992, 51.952833, 0.1}, + {2939.437078, 34.804997, 7.903881, 0.003500, 11}}, + {{2943.679695, 33.699509, 51.235358, 0.1}, + {2942.866827, 33.790206, 7.628086, 0.003500, 11}}, + {{2946.999101, 32.688846, 50.529650, 0.1}, + {2946.195183, 32.777117, 7.373489, 0.003500, 11}}, + {{2950.217493, 31.679292, 49.835035, 0.1}, + {2950.997969, 31.260565, 12.867386, 0.010000, 41}}, + {{2953.335016, 30.671441, 49.153096, 0.1}, + {2954.073563, 30.251509, 12.283864, 0.010000, 48}}, + {{2956.350647, 29.641635, 48.441400, 0.1}, + {2957.048328, 29.243979, 11.696148, 0.010000, 48}}, + {{2959.263303, 28.611932, 47.739490, 0.1}, + {2959.922414, 28.237924, 11.123138, 0.010000, 48}}, + {{2962.073123, 27.584913, 47.052590, 0.1}, + {2962.695966, 27.233296, 10.566719, 0.010000, 48}}, + {{2964.780373, 26.560521, 46.381113, 0.1}, + {2965.369125, 26.230046, 10.026509, 0.010000, 48}}, + {{2967.385314, 25.538714, 45.725492, 0.1}, + {2967.942025, 25.228128, 9.502122, 0.010000, 48}}, + {{2969.888201, 24.519460, 45.086174, 0.1}, + {2970.414798, 24.227493, 8.993173, 0.010000, 48}}, + {{2972.289290, 23.502736, 44.463621, 0.1}, + {2972.787570, 23.228095, 8.499271, 0.010000, 48}}, + {{2974.588832, 22.488518, 43.858309, 0.1}, + {2975.060462, 22.229890, 8.020013, 0.010000, 48}}, + {{2976.787076, 21.476777, 43.270725, 0.1}, + {2977.233591, 21.232831, 7.554990, 0.010000, 48}}, + {{2978.884269, 20.467481, 42.701369, 0.1}, + {2979.307070, 20.236874, 7.103775, 0.010000, 48}}, + {{2980.880652, 19.460587, 42.150748, 0.1}, + {2981.281006, 19.241976, 6.665932, 0.010000, 48}}, + {{2982.776464, 18.456044, 41.619378, 0.1}, + {2983.155503, 18.248095, 6.241007, 0.010000, 48}}, + {{2984.571937, 17.453792, 41.107781, 0.1}, + {2984.930661, 17.255187, 5.828534, 0.010000, 48}}, + {{2986.267297, 16.453765, 40.616482, 0.1}, + {2986.606575, 16.263212, 5.428031, 0.010000, 48}}, + {{2987.862762, 15.455888, 40.146011, 0.1}, + {2988.183337, 15.272130, 5.039001, 0.010000, 48}}, + {{2989.358540, 14.460001, 39.696907, 0.1}, + {2989.661033, 14.281903, 4.660909, 0.010000, 48}}, + {{2990.754830, 13.466130, 39.269688, 0.1}, + {2991.039748, 13.292492, 4.293264, 0.010000, 48}}, + {{2992.051832, 12.474233, 38.864869, 0.1}, + {2992.319561, 12.303862, 3.935539, 0.010000, 48}}, + {{2993.249740, 11.484250, 38.482958, 0.1}, + {2993.500549, 11.315977, 3.587188, 0.010000, 48}}, + {{2994.348743, 10.496116, 38.124445, 0.1}, + {2994.582783, 10.328804, 3.247651, 0.010000, 48}}, + {{2995.349023, 9.509767, 37.789806, 0.1}, + {2995.566335, 9.342312, 2.916352, 0.010000, 48}}, + {{2996.250754, 8.525139, 37.479491, 0.1}, + {2996.451270, 8.356472, 2.592704, 0.010000, 48}}, + {{2997.054106, 7.542171, 37.193927, 0.1}, + {2997.237653, 7.371257, 2.276107, 0.010000, 48}}, + {{2997.759242, 6.560805, 36.933508, 0.1}, + {2997.593906, 6.878876, 0.000000, 0.000000, 18}}, + {{2998.366319, 5.580987, 36.698594, 0.1}, + {2998.232574, 5.894551, 0.000000, 0.000000, 0}}, + {{2998.875647, 4.605733, 36.513507, 0.1}, + {2998.772838, 4.910794, 0.000000, 0.000000, 0}}, + {{2999.287586, 3.633139, 36.370710, 0.1}, + {2999.214753, 3.927587, 0.000000, 0.000000, 0}}, + {{2999.602292, 2.661052, 36.253879, 0.1}, + {2999.558375, 2.944913, 0.000000, 0.000000, 0}}, + {{2999.819812, 1.689438, 36.163201, 0.1}, + {2999.803756, 1.962763, 0.000000, 0.000000, 0}}, + {{2999.940194, 0.718271, 36.098808, 0.1}, + {2999.950947, 0.981127, 0.000000, 0.000000, 0}}, + {{2999.963481, -0.252449, 36.060779, 0.1}, + {2999.950947, 0.981127, 0.000000, 0.000000, 0}}, +}; diff --git a/src/tests/test-airbrakescontroller/testdata.h b/src/tests/test-airbrakescontroller/testdata.h new file mode 100644 index 0000000000000000000000000000000000000000..059266e40c85212bbce88a5dd80848112794d70d --- /dev/null +++ b/src/tests/test-airbrakescontroller/testdata.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +static const int LEN_TEST = 169; + +struct input_t +{ + float z; + float vz; + float vMod; + float sampleTime; +}; + +struct output_t +{ + float z_setpoint; + float vz_setpoint; + float u; + float deltas; + int alpha; +}; + +struct test_t +{ + input_t input; + output_t output; +}; + +extern const test_t DATA[LEN_TEST]; diff --git a/src/tests/test-dpl.cpp b/src/tests/test-dpl.cpp deleted file mode 100644 index a92926e6857f7cbbd957824fbcea6c7427c563fb..0000000000000000000000000000000000000000 --- a/src/tests/test-dpl.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <logger/Logger.h> -#include <cstdio> -#include <iostream> -#include <sstream> -#include <string> - -#include "DeathStack/DeploymentController/DeploymentController.h" -#include "DeathStack/DeploymentController/ThermalCutter/Cutter.h" -#include "DeathStack/SensorManager/Sensors/ADCWrapper.h" -#include "DeathStack/configs/CutterConfig.h" -#include "DeathStack/events/EventInjector.h" -#include "DeathStack/events/Events.h" -#include "utils/testutils/TestHelper.h" - -using namespace DeathStackBoard; - -using namespace std; - -void cuttingSequence(); -void cutterContinuity(); -void noseconeEjection(); -void manualEvent(); - -float adcToCurrent(uint16_t adc_in) { return (adc_in - 107) / 32.4f; } - -int main() -{ - sEventBroker->start(); - - string temp; - for (;;) - { - int choice; - cout << "\n\nWhat do you want to do?: \n"; - cout << "1. Test cutting sequence\n"; - cout << "2. Non-destructive cutter test\n"; - cout << "3. Nosecone ejection\n"; - cout << "4. Manual event input\n"; - - getline(cin, temp); - stringstream(temp) >> choice; - - switch (choice) - { - case 1: - cuttingSequence(); - break; - case 2: - cutterContinuity(); - break; - case 3: - noseconeEjection(); - break; - case 4: - manualEvent(); - break; - default: - break; - } - } -} - -bool print = false; - -void csense(void*) -{ - ADCWrapper adc; - adc.getCurrentSensorPtr()->init(); - - for (;;) - { - adc.getCurrentSensorPtr()->onSimpleUpdate(); - uint16_t raw1 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_1; - uint16_t raw2 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_2; - - float current1 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_1; - float current2 = - adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_2; - if (print) - { - printf("%d,%d,%d,%f,%f\n", (int)miosix::getTick(), (int)raw1, - (int)raw2, current1, current2); - } - Thread::sleep(100); - } -} - -void cuttingSequence() -{ - string temp; - char yn; - - cout << "\n\n** CUTTER SEQUENCE TEST **\n\n"; - cout << "Use predefined cutter parameters? (y/n)\n"; - getline(cin, temp); - stringstream(temp) >> yn; - unsigned int freq = CUTTER_PWM_FREQUENCY; - float duty = CUTTER_PWM_DUTY_CYCLE * 100; - - if (yn == 'n') - { - do - { - cout << "Insert frequency (Hz): \n"; - getline(cin, temp); - stringstream(temp) >> freq; - - cout << "Insert duty cycle(%): \n"; - getline(cin, temp); - stringstream(temp) >> duty; - } while (freq < 100 || freq > 30000 || duty < 0 || duty > 100); - - printf("Using custom parameters: Freq: %d, Duty: %.2f\n", freq, duty); - } - else - { - printf("Using default parameters: Freq: %d, Duty: %.2f\n", freq, duty); - } - - do - { - cout << "Write 'start' to begin:\n"; - getline(cin, temp); - } while (temp != "start"); - - { - Cutter c{freq, duty / 100.0f, CUTTER_TEST_PWM_DUTY_CYCLE}; - Servo s{DeploymentConfigs::SERVO_TIMER}; - - DeploymentController dpl{c, s}; - dpl.start(); - print = true; - Thread::sleep(1000); - cout << "Activating primary cutter...\n"; - sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DEPLOYMENT); - - waitForEvent(EV_TIMEOUT_CUTTING, TOPIC_DEPLOYMENT); - cout << "Primary cutter is done.\n"; - cout << "Activating secondary cutter\n"; - - waitForEvent(EV_TIMEOUT_CUTTING, TOPIC_DEPLOYMENT); - cout << "Backup cutter is done.\n"; - cout << "\n Test finished!\n\n"; - Thread::sleep(1000); - print = false; - } -} - -void cutterContinuity() -{ - string temp; - int cutter; - cout << "\n\n** NON-DESTRUCTIVE CUTTER TEST **\n\n"; - do - { - cout << "Which cutter to test? (1-primary / 2-backup)\n"; - - string temp; - getline(cin, temp); - stringstream(temp) >> cutter; - } while (cutter != 1 && cutter != 2); - - cout << "Use predefined cutter parameters? (y/n)\n"; - - char yn; - getline(cin, temp); - stringstream(temp) >> yn; - unsigned int freq = CUTTER_PWM_FREQUENCY; - float duty = CUTTER_TEST_PWM_DUTY_CYCLE * 100; - - if (yn == 'n') - { - do - { - cout << "Insert frequency (Hz): \n"; - getline(cin, temp); - stringstream(temp) >> freq; - - cout << "Insert test duty cycle(%): \n"; - getline(cin, temp); - stringstream(temp) >> duty; - } while (freq < 100 || freq > 30000 || duty < 0 || duty > 100); - - printf("Using custom parameters: Freq: %d, Test Duty: %.2f\n", freq, - duty); - } - else - { - printf("Using default parameters: Freq: %d, Test Duty: %.2f\n", freq, - duty); - } - - do - { - cout << "Write 'start' to begin:\n"; - getline(cin, temp); - } while (temp != "start"); - - { - Cutter c{freq, CUTTER_PWM_DUTY_CYCLE, duty}; - Servo s{DeploymentConfigs::SERVO_TIMER}; - - DeploymentController dpl{c, s}; - dpl.start(); - Thread::sleep(1000); - print = true; - if (cutter == 1) - { - cout << "Activating primary cutter...\n"; - sEventBroker->post({EV_TEST_CUTTER_PRIMARY}, TOPIC_DEPLOYMENT); - } - else - { - cout << "Activating backup cutter...\n"; - sEventBroker->post({EV_TEST_CUTTER_BACKUP}, TOPIC_DEPLOYMENT); - } - - waitForEvent(EV_TIMEOUT_CUTTING, TOPIC_DEPLOYMENT); - - cout << "Cutter test is done.\n"; - cout << "\n Test finished!\n\n"; - - Thread::sleep(1000); - print = false; - - dpl.stop(); - } -} - -void noseconeEjection() -{ - Servo s{DeploymentConfigs::SERVO_TIMER}; - s.setMinPulseWidth(800); - s.setMaxPulseWidth(2200); - s.enable(DeploymentConfigs::SERVO_CHANNEL); - - s.setPosition(DeploymentConfigs::SERVO_CHANNEL, - DeploymentConfigs::SERVO_RESET_POS); - - s.start(); - - cout << "\n\n** NOSECONE EJECTION TEST **\n\n"; - string temp; - do - { - cout << "Write 'yeet' to yeet out the nosecone:\n"; - getline(cin, temp); - } while (temp != "yeet"); - - { - EventCounter counter{*sEventBroker}; - - Cutter c{CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE, - CUTTER_TEST_PWM_DUTY_CYCLE}; - - DeploymentController dpl{c, s}; - dpl.start(); - counter.subscribe(TOPIC_DEPLOYMENT); - counter.start(); - - cout << "Activating primary cutter...\n"; - sEventBroker->post({EV_NC_OPEN}, TOPIC_DEPLOYMENT); - - for (;;) - { - if (counter.getCount(EV_TIMEOUT_NC_OPEN) == - DeploymentConfigs::MAX_EJECTION_ATTEMPTS) - break; - if (counter.getCount(EV_NC_DETACHED) >= 1) - break; - Thread::sleep(10); - } - - cout << "Cutter test is done.\n"; - cout << "\n Test finished!\n\n"; - - dpl.stop(); - counter.stop(); - } -} - -void manualEvent() -{ - cout << "\n\n** MANUAL MODE **\n\n"; - cout << "Hope you know what you are doing\n"; - - Cutter c{CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE, - CUTTER_TEST_PWM_DUTY_CYCLE}; - - Servo s{DeploymentConfigs::SERVO_TIMER}; - s.setMinPulseWidth(800); - s.setMaxPulseWidth(2200); - - DeploymentController dpl{c, s}; - EventInjector ei; - - ei.start(); - dpl.start(); -} diff --git a/src/tests/test-fmm.cpp b/src/tests/test-fmm-interactive.cpp similarity index 65% rename from src/tests/test-fmm.cpp rename to src/tests/test-fmm-interactive.cpp index 2da64eee3d05be5463f166ec71e5fb487e3a1f00..7ed81fb54b8a79bf836b5fb113d7eaaaf96bce80 100644 --- a/src/tests/test-fmm.cpp +++ b/src/tests/test-fmm-interactive.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2018-2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron + * Author: Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,19 +13,17 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISIN\G FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISIN\G + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * IN THE SOFTWARE. */ -#include <boards/DeathStack/FlightModeManager/FlightModeManager.h> +#include <FlightModeManager/FMMController.h> +#include <events/Events.h> +#include <events/Topics.h> #include <events/utils/EventSniffer.h> - -#include <boards/DeathStack/events/Events.h> -#include <boards/DeathStack/events/Topics.h> - #include <inttypes.h> using namespace miosix; @@ -33,27 +31,29 @@ using namespace DeathStackBoard; void printEvent(uint8_t event, uint8_t topic) { - TRACE("%s on %s\n", getEventString(event).c_str(), getTopicString(topic).c_str()); + TRACE("%s on %s\n", getEventString(event).c_str(), + getTopicString(topic).c_str()); } int main() { - FlightModeManager* fmm = new FlightModeManager(); - fmm->start(); sEventBroker->start(); + FMMController* fmm = new FMMController(); + fmm->start(); - EventSniffer* sniffer = new EventSniffer(*sEventBroker, TOPIC_LIST, printEvent); - UNUSED(sniffer); // The sniffer's handler will be called by evBroker + EventSniffer* sniffer = + new EventSniffer(*sEventBroker, TOPIC_LIST, printEvent); + UNUSED(sniffer); // The sniffer's handler will be called by evBroker printf("\nOk\n"); Event ev; int topic; - while(true) + while (true) { printf("Choose an event\n"); printf("ID:\n"); - scanf("%hu", &(ev.sig)); + scanf("%hhu", &(ev.sig)); printf("TOPIC:\n"); scanf("%d", &topic); diff --git a/src/tests/test-hse.cpp b/src/tests/test-hse.cpp new file mode 100644 index 0000000000000000000000000000000000000000..92100e508cfa10c00727a1ebb054f601e7802dca --- /dev/null +++ b/src/tests/test-hse.cpp @@ -0,0 +1,75 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <miosix.h> +#include <stdio.h> + +int main() +{ + printf("\nClock configuration : 0x%x \n\n", (unsigned int)RCC->CR); + + // check if HSE is enabled + if (RCC->CR & RCC_CR_HSEON) + { + printf("HSE is on ... ok \n"); + } + else + { + printf("HSE is off ... error \n"); + } + + // check if HSE is ready + if (RCC->CR & RCC_CR_HSERDY) + { + printf("HSE is ready ... ok \n"); + } + else + { + printf("HSE not ready ... error \n"); + } + + // check if HSE is bypassed + if (RCC->CR & RCC_CR_HSEBYP) + { + printf("HSE is bypassed ... error \n"); + } + else + { + printf("HSE is not bypassed ... ok \n"); + } + + // check if Clock Security System is enabled + if (RCC->CR & RCC_CR_CSSON) + { + printf("CSS is on \n"); + } + else + { + printf("CSS is off \n"); + } + + // Wait for the user to press ENTER or the timer to elapse + printf("Press any key to exit the external oscillator test\n"); + (void)getchar(); + + return 0; +} diff --git a/src/tests/test-kalman-hitl.cpp b/src/tests/test-kalman-hitl.cpp index 2893bc3fa5cc0d6ade8cc5ac64f2e35ea1183e97..f534a48f42b54b6f280ca2153c644b7b0b8a3dd5 100644 --- a/src/tests/test-kalman-hitl.cpp +++ b/src/tests/test-kalman-hitl.cpp @@ -1,13 +1,36 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Mozzarelli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ -#include <iostream> -#include "DeathStack/ADA/ADAController.h" -#include <events/FSM.h> -#include <DeathStack/events/Events.h> -#include <events/EventBroker.h> -#include <DeathStack/EventClasses.h> #include <Common.h> +#include <EventClasses.h> +#include <events/EventBroker.h> +#include <events/Events.h> +#include <events/FSM.h> + +#include <iostream> #include <string> +#include "ADA/ADAController.h" + typedef miosix::Gpio<GPIOG_BASE, 13> greenLed; typedef miosix::Gpio<GPIOG_BASE, 14> redLed; using namespace DeathStackBoard; @@ -31,7 +54,7 @@ int main() // Read serial std::string line; - while ( std::getline(std::cin, line) ) + while (std::getline(std::cin, line)) { if (line[0] == '#') { @@ -44,24 +67,24 @@ void handleCommands(std::string line) { // Extract command name unsigned delimiterIndex = line.find_last_of("#"); - std::string command = line.substr(1, delimiterIndex-1); + std::string command = line.substr(1, delimiterIndex - 1); // TRACE("ADA HITL Test command: %s \n", command.c_str()); - + // Blink green led: command received - greenLed::high(); + greenLed::high(); miosix::Thread::sleep(50); greenLed::low(); - if ( command == "INPUT_SAMPLE" ) + if (command == "INPUT_SAMPLE") { // char* end; - // std::string height = line.substr(delimiterIndex+1, std::string::npos); - // float y = strtof(height.c_str(), &end); + // std::string height = line.substr(delimiterIndex+1, + // std::string::npos); float y = strtof(height.c_str(), &end); // TODO: Change to accept temperature // ada.updateAltitude(y); } - else if ( command == "GET_STATE" ) + else if (command == "GET_STATE") { std::cout << "#X0#" << ada.getKalmanState().x0 << "\n"; std::cout << "#X1#" << ada.getKalmanState().x1 << "\n"; @@ -70,30 +93,31 @@ void handleCommands(std::string line) // TODO: Change to new version // else if ( command == "GET_CALIB" ) // { - // std::cout << "#N_SAMPLES#" << ada.getCalibrationData().stats.nSamples << "\n"; - // std::cout << "#AVG#" << ada.getCalibrationData().stats.mean << "\n"; + // std::cout << "#N_SAMPLES#" << ada.getCalibrationData().stats.nSamples + // << "\n"; std::cout << "#AVG#" << ada.getCalibrationData().stats.mean + // << "\n"; // } - else if ( command == "EV_TC_RESET_CALIBRATION") + else if (command == "EV_TC_RESET_CALIBRATION") { sEventBroker->post({EV_TC_RESET_CALIBRATION}, TOPIC_ADA); } - else if ( command == "EV_LIFTOFF") + else if (command == "EV_LIFTOFF") { sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); } - else if ( command == "EV_APOGEE") + else if (command == "EV_APOGEE") { sEventBroker->post({EV_APOGEE}, TOPIC_FLIGHT_EVENTS); } - else if ( command == "EV_SET_DPL_ALTITUDE") + else if (command == "EV_SET_DPL_ALTITUDE") { int alt = std::atoi(line.substr(20, std::string::npos).c_str()); DeploymentAltitudeEvent ev; ev.dplAltitude = alt; - ev.sig = EV_TC_SET_DPL_ALTITUDE; + ev.sig = EV_TC_SET_DPL_ALTITUDE; sEventBroker->post(ev, TOPIC_TC); } - else if ( command == "EV_DPL_ALTITUDE") + else if (command == "EV_DPL_ALTITUDE") { sEventBroker->post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS); } @@ -104,7 +128,4 @@ void handleCommands(std::string line) miosix::Thread::sleep(50); redLed::low(); } - - - } \ No newline at end of file diff --git a/src/tests/test-logproxy.cpp b/src/tests/test-logproxy.cpp index 2a5030d23d4a4b77c67f80614780e4b9cc8e51ab..754109d694fd262b3286cfe3935a64bd5dfe160e 100644 --- a/src/tests/test-logproxy.cpp +++ b/src/tests/test-logproxy.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Alessio Galluccio + * Author: Alessio Galluccio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,14 +13,14 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #include <Common.h> -#include <boards/DeathStack/LoggerService/LoggerService.h> +#include <LoggerService/LoggerService.h> using namespace miosix; using namespace DeathStackBoard; @@ -31,7 +31,7 @@ int main() UNUSED(logger); - while(1) + while (1) { TRACE("I'm alive bitches!\n"); } diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/generated/lh_circles_data.h b/src/tests/test-pinhandler.cpp similarity index 61% rename from src/boards/DeathStack/ADA/DeploymentUtils/generated/lh_circles_data.h rename to src/tests/test-pinhandler.cpp index 14fd5e2ac807cf17c118b35624479f137b6a80f4..271e37281ec0266399e351899a67ccb683e916f3 100644 --- a/src/boards/DeathStack/ADA/DeploymentUtils/generated/lh_circles_data.h +++ b/src/tests/test-pinhandler.cpp @@ -1,34 +1,46 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta - * +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -/* - ****************************************************************************** - * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * - ****************************************************************************** - */ +#include "Common.h" +#include "PinHandler/PinHandler.h" + +using namespace DeathStackBoard; + +int main() +{ + TimestampTimer::enableTimestampTimer(); + + PinHandler pin_handler; + + if (!pin_handler.start()) + { + printf("Error starting pin handler"); + return -1; + } -// Generated from: https://git.skywarder.eu/r2a-mini/elevation-map -// Autogen date: 2019-11-02 19:13:26.801716 + while (true) + { + Thread::sleep(1000); + } -static constexpr int NUM_CIRCLES = 0; -static const LHCircle circles[NUM_CIRCLES] = {}; \ No newline at end of file + return 0; +} diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp index bc6a709bfe53cf4e5d0c68ad8a71f00c9481b124..e2e90c5f647d6cdfe836f2495010d25b290c1c54 100644 --- a/src/tests/test-sensormanager.cpp +++ b/src/tests/test-sensormanager.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2018 Skyward Experimental Rocketry - * Authors: Luca Erbetta + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,60 +13,44 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ +#include "ADA/ADAController.h" #include "Common.h" -#include "DeathStack/ADA/ADAController.h" -#include "DeathStack/events/Events.h" -#include "DeathStack/LoggerService/LoggerService.h" -#include "DeathStack/SensorManager/SensorManager.h" +#include "LoggerService/LoggerService.h" +#include "Radio/TMTCManager.h" +#include "SensorManager/SensorManager.h" #include "diagnostic/CpuMeter.h" #include "events/EventBroker.h" +#include "events/Events.h" using namespace miosix; using namespace DeathStackBoard; int main() { - Stats s; - ADAController ada; - SensorManager mgr{&ada}; - // ada.start(); - try - { - LoggerService::getInstance()->start(); - } - catch (const std::exception& e) - { - printf("SDCARD MISSING\n"); - for (;;) - { - led1::high(); - Thread::sleep(200); - led1::low(); - Thread::sleep(200); - } - } - led1::high(); + TimestampTimer::enableTimestampTimer(); sEventBroker->start(); - mgr.start(); - sEventBroker->post({EV_TC_START_SENSOR_LOGGING}, TOPIC_TC); + Stats s; + SensorManager mgr; + Bus bus; + Sensors sensors(*bus.spi1, new TaskScheduler()); + Radio radio(*bus.spi2); - // printf("Wait for calibration to complete.\n"); + sensors.start(); + radio.start(); Thread::sleep(500); - // sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); - - for (int i = 0; i < 60 * 3 * 10; i++) + for (int i = 0; i < 1 * 3 * 10; i++) { s.add(averageCpuUtilization()); Thread::sleep(100); @@ -79,9 +63,9 @@ int main() for (;;) { - led1::high(); + ledOn(); Thread::sleep(1000); - led1::low(); + ledOff(); Thread::sleep(1000); } } diff --git a/src/tests/test-sm+tmtc.cpp b/src/tests/test-sm+tmtc.cpp index 4a73cf98df3b27b027c56a1632cbe0ab72e95810..9024a013b8dc8d5d61d310eb8eaf1b8b37897a9a 100644 --- a/src/tests/test-sm+tmtc.cpp +++ b/src/tests/test-sm+tmtc.cpp @@ -1,5 +1,6 @@ + /* Copyright (c) 2018-2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron + * Author: Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,24 +14,22 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISIN\G - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS - * IN THE SOFTWARE. + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. */ -#include <boards/DeathStack/TMTCManager/TMTCManager.h> - -#include <boards/DeathStack/events/Events.h> -#include <boards/DeathStack/events/Topics.h> - +#include <TMTCManager/TMTCManager.h> +#include <events/Events.h> +#include <events/Topics.h> #include <interfaces-impl/hwmapping.h> +#include "ADA/ADAController.h" #include "Common.h" -#include "DeathStack/ADA/ADAController.h" -#include "DeathStack/LoggerService/LoggerService.h" -#include "DeathStack/SensorManager/SensorManager.h" +#include "LoggerService/LoggerService.h" +#include "SensorManager/SensorManager.h" #include "diagnostic/CpuMeter.h" #include "events/EventBroker.h" @@ -85,7 +84,7 @@ int main() Thread::sleep(1000); - //printf("\nOk, press open to post liftoff...\n"); + // printf("\nOk, press open to post liftoff...\n"); // while(inputs::btn_open::value()) // { // Thread::sleep(100); diff --git a/src/tests/test-tmtc.cpp b/src/tests/test-tmtc.cpp index 878b490848c4eb67f52095611e4a9cc1051b882b..b7eedc5556c1980a955a29326337a215fb88d895 100644 --- a/src/tests/test-tmtc.cpp +++ b/src/tests/test-tmtc.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2018-2019 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron + * Author: Alvise de'Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,20 +13,18 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISIN\G - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS - * IN THE SOFTWARE. + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. */ -#include <boards/DeathStack/TMTCManager/TMTCManager.h> -#include <skyward-boardcore/src/shared/utils/EventSniffer.h> - -#include <boards/DeathStack/events/Events.h> -#include <boards/DeathStack/events/Topics.h> - +#include <TMTCManager/TMTCManager.h> +#include <events/Events.h> +#include <events/Topics.h> #include <interfaces-impl/hwmapping.h> +#include <skyward-boardcore/src/shared/utils/EventSniffer.h> using namespace miosix; using namespace DeathStackBoard; @@ -41,7 +39,7 @@ int main() { busSPI2::init(); - TMTCManager* tmtc = new TMTCManager(); + TMTCController* tmtc = new TMTCController(); tmtc->start(); sEventBroker->start();